diff --git a/src/adc_landmarks_slam_solver_alg.cpp b/src/adc_landmarks_slam_solver_alg.cpp
index a695b5b68d2bcecbe623224b841b01c3a1b78027..7d8067ec726d3b2aeb889ed09da76645127ac951 100644
--- a/src/adc_landmarks_slam_solver_alg.cpp
+++ b/src/adc_landmarks_slam_solver_alg.cpp
@@ -168,7 +168,7 @@ double AdcLandmarksSlamSolverAlgorithm::is_a_mapped_landmark(Eigen::Vector3d _fe
geometry_msgs::PoseStamped local_land, global_land;
global_land.header.stamp = _tf_sensor_map.header.stamp;
- global_land.header.frame_id = this->config_.global_frame;
+ global_land.header.frame_id = _tf_sensor_map.child_frame_id;//global_frame
global_land.pose.position.z = 0.0;
global_land.pose.position.x = it->second.pose(0);
global_land.pose.position.y = it->second.pose(1);
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index dd44955064e49c454add4f675f2cf699c54f86c8..a5fd3b37394c080c706e528131cc921fdb5e4159 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -204,6 +204,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
{
//lock access to algorithm if necessary
// this->alg_.lock();
+ this->rear_features_mutex_enter();
this->front_features_mutex_enter();
this->estimated_pose_mutex_enter();
// ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::mainNodeThread");
@@ -274,6 +275,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->new_estimated_pose_ = false;
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
+ this->rear_features_mutex_exit();
return;
}
tf_odom_base_msg = this->tf2_buffer.lookupTransform(this->config_.odom_frame, this->config_.base_link_frame, this->new_estimated_pose_t_);
@@ -351,6 +353,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->new_front_features_data_ = false;
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
+ this->rear_features_mutex_exit();
return;
}
//Get global robot_state
@@ -363,6 +366,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->new_front_features_data_ = false;
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
+ this->rear_features_mutex_exit();
return;
}
}
@@ -380,8 +384,10 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
if (!front_land_success || !rear_land_success)
{
// this->new_front_features_data_ = false;
+ this->new_features_data_ = false;
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
+ this->rear_features_mutex_exit();
return;
}
}
@@ -457,6 +463,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
}
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
+ this->rear_features_mutex_exit();
return;
}
tf_robot = this->tf2_buffer.lookupTransform(this->config_.global_frame, this->config_.base_link_frame, t);
@@ -539,6 +546,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->alg_.unlock();
this->estimated_pose_mutex_exit();
this->front_features_mutex_exit();
+ this->rear_features_mutex_exit();
}
bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool _front)
@@ -550,7 +558,7 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool
if ((_front && this->front_features_frame_ == "") || (!_front && this->rear_features_frame_ == ""))
{
- ROS_DEBUG_STREAM("AdcLandmarksSlamSolverAlgNode::check_landmarks -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
+ ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::check_landmarks -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
return false;
}
@@ -705,7 +713,7 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
geometry_msgs::PoseStamped local_land, global_land;
global_land.header.stamp = _tf_sensor_map.header.stamp;
- global_land.header.frame_id = this->config_.global_frame;
+ global_land.header.frame_id = _tf_sensor_map.child_frame_id;//global
global_land.pose.position.z = 0.0;
global_land.pose.position.x = it->pose(0);
global_land.pose.position.y = it->pose(1);
@@ -756,9 +764,17 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, boo
bool AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data(FrameData& _frame_data, bool _front)
{
+ if ((_front && this->front_features_frame_ == "") || (!_front && this->rear_features_frame_ == ""))
+ {
+ ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data -> " << (_front? "front ": "rear ") << "features frame id empty; AR tag filter not initialized.");
+ return false;
+ }
//calculate tf from base_link to sensor
geometry_msgs::TransformStamped tf;
geometry_msgs::PoseStamped from_sensor, from_base_link;
+ tf.header.frame_id = this->config_.base_link_frame;
+ tf.child_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
+ tf.header.stamp = this->features_stamp_;
if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, (_front? this->front_features_frame_: this->rear_features_frame_), this->features_stamp_, ros::Duration(this->config_.tf_timeout)))
{
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:update_landmarks_frame_data -> Can't transform from " << this->config_.base_link_frame << " to " << (_front? this->front_features_frame_: this->rear_features_frame_) << " at " << this->features_stamp_);
@@ -769,7 +785,7 @@ bool AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data(FrameData& _fram
from_sensor.header.frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
for (unsigned int i = 0; i < (_front? this->front_features_: this->rear_features_).size(); i++)
{
- if(this->front_features_[i].landmark_key > 0.0)
+ if((_front? this->front_features_: this->rear_features_)[i].landmark_key > 0.0)
{
LandmarkResidualData land_res_data;
// Transform sensor info to base_link frame
@@ -874,6 +890,7 @@ void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
void AdcLandmarksSlamSolverAlgNode::merge_features()
{
std::vector<FeatureInfo>().swap(this->features_);
+ this->features_.clear();
if (this->new_rear_features_data_ && (this->features_stamp_ - this->rear_features_stamp_) < ros::Duration(this->config_.old_feature_timeout))
{
this->features_.resize(this->rear_features_.size());
@@ -886,6 +903,7 @@ void AdcLandmarksSlamSolverAlgNode::merge_features()
ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::merge_features -> rear features too old: last stamp " << this->rear_features_stamp_);
this->new_rear_features_data_ = false;
}
+
if (this->new_front_features_data_ && (this->features_stamp_ - this->front_features_stamp_) < ros::Duration(this->config_.old_feature_timeout))
{
unsigned int old_size = this->features_.size();
@@ -1145,19 +1163,30 @@ void AdcLandmarksSlamSolverAlgNode::front_features_callback(const iri_adc_msgs::
this->features_stamp_ = msg->header.stamp;
this->new_front_features_data_ = true;
this->new_features_data_ = true;
- this->front_features_.resize(msg->features.size());
- for (unsigned int i = 0; i < msg->features.size(); i++)
+ if (msg->header.frame_id != "")
{
- FeatureInfo feature;
- feature.msg_pose = msg->features[i].pose;
- feature.landmark_key = -1.0;
- feature.type = msg->features[i].type;
- Eigen::Vector3d dist;
- dist << msg->features[i].pose.position.x, msg->features[i].pose.position.y, msg->features[i].pose.position.z;
- feature.r = dist.norm();
- feature.th = std::atan2(msg->features[i].pose.position.x, msg->features[i].pose.position.z);
- feature.frame = msg->header.frame_id;
- this->front_features_[i] = feature;
+ this->front_features_.resize(msg->features.size());
+ for (unsigned int i = 0; i < msg->features.size(); i++)
+ {
+ FeatureInfo feature;
+ feature.msg_pose = msg->features[i].pose;
+ feature.landmark_key = -1.0;
+ feature.type = msg->features[i].type;
+ Eigen::Vector3d dist;
+ dist << msg->features[i].pose.position.x, msg->features[i].pose.position.y, msg->features[i].pose.position.z;
+ feature.r = dist.norm();
+ feature.th = std::atan2(msg->features[i].pose.position.x, msg->features[i].pose.position.z);
+ feature.frame = msg->header.frame_id;
+ this->front_features_[i] = feature;
+ }
+ // ROS_INFO_STREAM("Front features size " << (int)this->front_features_.size());
+ }
+ else
+ {
+ this->new_front_features_data_ = false;
+ std::vector<FeatureInfo>().swap(this->front_features_);
+ this->front_features_.clear();
+ // ROS_INFO_STREAM("Front features size " << (int)this->front_features_.size());
}
//std::cout << msg->data << std::endl;
@@ -1183,31 +1212,42 @@ void AdcLandmarksSlamSolverAlgNode::rear_features_callback(const iri_adc_msgs::f
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
- this->front_features_mutex_enter();
+ this->rear_features_mutex_enter();
this->rear_features_frame_ = msg->header.frame_id;
this->rear_features_stamp_ = msg->header.stamp;
this->features_stamp_ = msg->header.stamp;
this->new_rear_features_data_ = true;
this->new_features_data_ = true;
- this->rear_features_.resize(msg->features.size());
- for (unsigned int i = 0; i < msg->features.size(); i++)
+ if (msg->header.frame_id != "")
{
- FeatureInfo feature;
- feature.msg_pose = msg->features[i].pose;
- feature.landmark_key = -1.0;
- feature.type = msg->features[i].type;
- Eigen::Vector3d dist;
- dist << msg->features[i].pose.position.x, msg->features[i].pose.position.y, msg->features[i].pose.position.z;
- feature.r = dist.norm();
- feature.th = std::atan2(msg->features[i].pose.position.x, msg->features[i].pose.position.z);
- feature.frame = msg->header.frame_id;
- this->rear_features_[i] = feature;
+ this->rear_features_.resize(msg->features.size());
+ for (unsigned int i = 0; i < msg->features.size(); i++)
+ {
+ FeatureInfo feature;
+ feature.msg_pose = msg->features[i].pose;
+ feature.landmark_key = -1.0;
+ feature.type = msg->features[i].type;
+ Eigen::Vector3d dist;
+ dist << msg->features[i].pose.position.x, msg->features[i].pose.position.y, msg->features[i].pose.position.z;
+ feature.r = dist.norm();
+ feature.th = std::atan2(msg->features[i].pose.position.x, msg->features[i].pose.position.z);
+ feature.frame = msg->header.frame_id;
+ this->rear_features_[i] = feature;
+ // ROS_INFO_STREAM("Rear features size " << (int)this->rear_features_.size());
+ }
+ }
+ else
+ {
+ this->new_rear_features_data_ = false;
+ std::vector<FeatureInfo>().swap(this->rear_features_);
+ this->rear_features_.clear();
+ // ROS_INFO_STREAM("Rear features size " << (int)this->rear_features_.size());
}
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();
- this->front_features_mutex_exit();
+ this->rear_features_mutex_exit();
}
void AdcLandmarksSlamSolverAlgNode::rear_features_mutex_enter(void)
@@ -1229,6 +1269,7 @@ void AdcLandmarksSlamSolverAlgNode::rear_features_mutex_exit(void)
void AdcLandmarksSlamSolverAlgNode::node_config_update(Config &config, uint32_t level)
{
this->alg_.lock();
+ this->rear_features_mutex_enter();
this->front_features_mutex_enter();
if (config.load_landmarks && !this->config_.load_landmarks)
load_landmarks();
@@ -1250,6 +1291,7 @@ void AdcLandmarksSlamSolverAlgNode::node_config_update(Config &config, uint32_t
}
this->config_=config;
this->front_features_mutex_exit();
+ this->rear_features_mutex_exit();
this->alg_.unlock();
}