diff --git a/cfg/AdcLandmarksSlamSolver.cfg b/cfg/AdcLandmarksSlamSolver.cfg
index e5ed1827febea5a1eb51199efb090dbe613a9c92..20b60450736048b5411c7c7cbcdb47ff57d97546 100755
--- a/cfg/AdcLandmarksSlamSolver.cfg
+++ b/cfg/AdcLandmarksSlamSolver.cfg
@@ -48,6 +48,7 @@ gen.add("global_frame", str_t, 0, "Global
gen.add("odom_frame", str_t, 0, "Odom frame", "odom")
gen.add("base_link_frame", str_t, 0, "Robot base_link frame", "base_link")
gen.add("tf_timeout", double_t, 0, "Timeout to find a transform", 0.2, 0.1, 2.0)
+gen.add("old_feature_timeout", double_t, 0, "Timeout to set a features source as old", 0.5, 0.1, 2.0)
gen.add("amcl_pose_estimated_sigma", double_t, 0, "AMCL pose sigma when using AMCL localization", 1, 0.1, 10)
gen.add("publish_pose_rate", double_t, 0, "Rate to publish the robot pose", 1.0, 0.00001, 10)
gen.add("publish_pose_distance", double_t, 0, "Distance from last robot state to publish the robot pose", 1.0, 0.1, 100000)
diff --git a/include/adc_landmarks_slam_solver_alg_node.h b/include/adc_landmarks_slam_solver_alg_node.h
index 61c7bf43b30d39fe41a8acf95d3ac76c542fedf9..b724d8fb92499b7700ccd1b8405e9ea60375ee0b 100644
--- a/include/adc_landmarks_slam_solver_alg_node.h
+++ b/include/adc_landmarks_slam_solver_alg_node.h
@@ -73,6 +73,7 @@ typedef struct {
double th; ///< Angle at detection.
double landmark_key; ///< The landmark id detected. -1 if no landmark detected.
int type; ///< Feature type.
+ std::string frame; ///< features origin frame.
}FeatureInfo;
/**
@@ -89,9 +90,18 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
double covariance_m_[3 * 3]; ///< Last robot pose covariance.
bool new_features_data_; ///< Bool to know that new features has been received.
- std::string features_frame_; ///< Features origin frame.
- ros::Time features_stamp_; ///< Time stamp from the features.
- std::vector<FeatureInfo> features_; ///< Feature data vector.
+ ros::Time features_stamp_; ///< Time stamp from the last features.
+ std::vector<FeatureInfo> features_; ///< Both features data vector.
+
+ bool new_front_features_data_; ///< Bool to know that new front_features has been received.
+ std::string front_features_frame_; ///< front_features origin frame.
+ ros::Time front_features_stamp_; ///< Time stamp from the front_features.
+ std::vector<FeatureInfo> front_features_; ///< Feature data vector.
+
+ bool new_rear_features_data_; ///< Bool to know that new rear_features has been received.
+ std::string rear_features_frame_; ///< rear_features origin frame.
+ ros::Time rear_features_stamp_; ///< Time stamp from the rear_features.
+ std::vector<FeatureInfo> rear_features_; ///< Feature data vector.
std::list<LandmarkCandidate> landmarks_candidates_; ///< Landmark candidates.
@@ -111,7 +121,7 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
Eigen::Vector3d estimated_sigma_; ///< Estimated main sigmas.
bool reset_estimated_pose_; ///< Boolena to set new_estimated_pose to false.
- bool last_frame_min_detections_; ///< Flag to know that last frame was because a lot of features seen.
+ bool last_frame_min_detections_; ///< Flag to know that last frame was because a lot of front_features seen.
// [publisher attributes]
ros::Publisher frame_data_publisher_;
@@ -144,11 +154,17 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
tf2_ros::TransformListener tf2_listener;
- ros::Subscriber features_subscriber_;
- void features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg);
- pthread_mutex_t features_mutex_;
- void features_mutex_enter(void);
- void features_mutex_exit(void);
+ ros::Subscriber front_features_subscriber_;
+ void front_features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg);
+ pthread_mutex_t front_features_mutex_;
+ void front_features_mutex_enter(void);
+ void front_features_mutex_exit(void);
+
+ ros::Subscriber rear_features_subscriber_;
+ void rear_features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg);
+ pthread_mutex_t rear_features_mutex_;
+ void rear_features_mutex_enter(void);
+ void rear_features_mutex_exit(void);
// [service attributes]
@@ -168,8 +184,10 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
/**
* \brief Function to remove landmark candidates not detected and reinit its variables.
+ *
+ * \param _front Flag to select front or rear features.
*/
- void prepare_landmark_candidates(void);
+ void prepare_landmark_candidates(bool _front);
/**
* \brief Function to analyze the feature information.
@@ -178,10 +196,12 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
* If enabled, it searchs for new landmarks.
*
* \param _update_landmarks Flag to know if it must search for new landmarks.
+ *
+ * \param _front Flag to select front or rear features.
*
* \return True on success.
*/
- bool check_landmarks(bool _update_landmarks);
+ bool check_landmarks(bool _update_landmarks, bool _front);
/**
* \brief Function to check if the feature is an existing landmark candidate and to update landmark candidates info.
@@ -191,10 +211,12 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
* If the feature isn't an existing landmark candidate, a new landmark candidate is added with its information.
*
* \param _feature The feature to check.
+ *
+ * \param _front Flag to select front or rear features.
*
* \return If is a landmark candidate have been seen enough times (is a landmark).
*/
- bool is_a_new_landmark(FeatureInfo &_feature);
+ bool is_a_new_landmark(FeatureInfo &_feature, bool _front);
/**
* \brief Function to update the frame data with the detected landmarks' information.
@@ -202,10 +224,12 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
* It transform The sensor information to base_link frame.
*
* \param _frame_data The frame data.
+ *
+ * \param _front Flag to select front or rear features.
*
* \return If succeded.
*/
- bool update_landmarks_frame_data(FrameData& _frame_data);
+ bool update_landmarks_frame_data(FrameData& _frame_data, bool _front);
/**
* \brief Function to update tf from map to odom at a requested time stamp.
@@ -238,6 +262,13 @@ class AdcLandmarksSlamSolverAlgNode : public algorithm_base::IriBaseAlgorithm<Ad
*/
bool get_tf_map_odom(ros::Time _t, Eigen::Vector3d& _tf_map_odom);
+ /**
+ * \brief Function to merge features received.
+ *
+ * It only add features when they are not too old.
+ */
+ void merge_features();
+
/**
* \brief Function to publish landmarks markers.
*/
diff --git a/launch/node.launch b/launch/node.launch
index b333afe78b6d379e0f3d84519de3a4c8a8cb8ef0..336600572dd4854dba192f093306d402cd96fd4f 100644
--- a/launch/node.launch
+++ b/launch/node.launch
@@ -14,7 +14,8 @@
<!-- <arg name="topic_name" default="new_topic_name"/> -->
<arg name="estimated_pose_topic_name" default="~/estimated_pose"/>
<arg name="initialpose_topic_name" default="~/initialpose"/>
- <arg name="features_topic_name" default="~/features"/>
+ <arg name="front_features_topic_name" default="~/front_features"/>
+ <arg name="rear_features_topic_name" default="~/rear_features"/>
<node name="$(arg node_name)"
@@ -30,7 +31,8 @@
<!--<remap from="~/topic" to="$(arg topic_name)"/>-->
<remap from="~/estimated_pose" to="$(arg estimated_pose_topic_name)"/>
<remap from="~/initialpose" to="$(arg initialpose_topic_name)"/>
- <remap from="~/features" to="$(arg features_topic_name)"/>
+ <remap from="~/front_features" to="$(arg front_features_topic_name)"/>
+ <remap from="~/rear_features" to="$(arg rear_features_topic_name)"/>
</node>
</launch>
diff --git a/src/adc_landmarks_slam_solver_alg_node.cpp b/src/adc_landmarks_slam_solver_alg_node.cpp
index f7da6129cb18ad43d779027e5e8d6ffb19b2f47f..473b0030ee79bcb5dd2494670f356022b1e3a3c0 100644
--- a/src/adc_landmarks_slam_solver_alg_node.cpp
+++ b/src/adc_landmarks_slam_solver_alg_node.cpp
@@ -129,11 +129,16 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->estimated_pose_subscriber_ = this->private_node_handle_.subscribe("estimated_pose", 1, &AdcLandmarksSlamSolverAlgNode::estimated_pose_callback, this);
pthread_mutex_init(&this->estimated_pose_mutex_,NULL);
- this->features_subscriber_ = this->private_node_handle_.subscribe("features", 1, &AdcLandmarksSlamSolverAlgNode::features_callback, this);
- pthread_mutex_init(&this->features_mutex_,NULL);
+ this->front_features_subscriber_ = this->private_node_handle_.subscribe("front_features", 1, &AdcLandmarksSlamSolverAlgNode::front_features_callback, this);
+ pthread_mutex_init(&this->front_features_mutex_,NULL);
+
+ this->rear_features_subscriber_ = this->private_node_handle_.subscribe("rear_features", 1, &AdcLandmarksSlamSolverAlgNode::rear_features_callback, this);
+ pthread_mutex_init(&this->rear_features_mutex_,NULL);
//Init class atributes
this->tf_odom_base_ << 0.0, 0.0, 0.0;
+ this->new_front_features_data_ = false;
+ this->new_rear_features_data_ = false;
this->new_features_data_ = false;
this->new_estimated_pose_ = false;
this->reset_estimated_pose_ = false;
@@ -147,6 +152,8 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->publish_pose_ = false;
this->new_estimated_pose_t_ = ros::Time::now();
+ this->front_features_stamp_ = ros::Time::now();
+ this->rear_features_stamp_ = ros::Time::now();
this->features_stamp_ = ros::Time::now();
this->tf_map_odom_ << 0.0, 0.0, 0.0;
@@ -166,6 +173,8 @@ AdcLandmarksSlamSolverAlgNode::AdcLandmarksSlamSolverAlgNode(void) :
this->covariance_m_[7] = 0.0;
this->covariance_m_[8] = 0.0;
+ std::vector<FeatureInfo>().swap(this->front_features_);
+ std::vector<FeatureInfo>().swap(this->rear_features_);
std::vector<FeatureInfo>().swap(this->features_);
std::list<LandmarkCandidate>().swap(this->landmarks_candidates_);
@@ -183,7 +192,10 @@ AdcLandmarksSlamSolverAlgNode::~AdcLandmarksSlamSolverAlgNode(void)
// [free dynamic memory]
pthread_mutex_destroy(&this->initialpose_mutex_);
pthread_mutex_destroy(&this->estimated_pose_mutex_);
- pthread_mutex_destroy(&this->features_mutex_);
+ pthread_mutex_destroy(&this->front_features_mutex_);
+ pthread_mutex_destroy(&this->rear_features_mutex_);
+ std::vector<FeatureInfo>().swap(this->front_features_);
+ std::vector<FeatureInfo>().swap(this->rear_features_);
std::vector<FeatureInfo>().swap(this->features_);
std::list<LandmarkCandidate>().swap(this->landmarks_candidates_);
}
@@ -192,7 +204,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
{
//lock access to algorithm if necessary
// this->alg_.lock();
- this->features_mutex_enter();
+ this->front_features_mutex_enter();
this->estimated_pose_mutex_enter();
// ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::mainNodeThread");
@@ -258,7 +270,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
}
// this->new_estimated_pose_ = false;
this->estimated_pose_mutex_exit();
- this->features_mutex_exit();
+ this->front_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_);
@@ -297,8 +309,15 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
if (this->new_features_data_)
{
+ merge_features();
// Check landmarks
- bool land_success = check_landmarks(this->config_.search_for_new_landmarks);
+ bool front_land_success = false;
+ if (this->new_front_features_data_)
+ front_land_success = check_landmarks(this->config_.search_for_new_landmarks, true);
+ bool rear_land_success = false;
+ if (this->new_rear_features_data_)
+ rear_land_success = check_landmarks(this->config_.search_for_new_landmarks, false);
+
if (this->config_.publish_visualization)
publish_landmarks_markers();
@@ -308,8 +327,11 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
dist(1) = this->tf_odom_base_(1) - this->last_update_odom_base_(1);
inc_ang = std::fabs(this->tf_odom_base_(2) - this->last_update_odom_base_(2));
- if (((this->features_stamp_ - this->last_update_t_) > ros::Duration(1/this->config_.update_problem_rate)) || (dist.norm() > this->config_.update_problem_distance) ||
- (inc_ang > this->config_.update_problem_angle) || this->new_estimated_pose_ || (!this->last_frame_min_detections_ && (this->features_.size() >= this->config_.update_problem_features_detected)))
+ if (((this->features_stamp_ - this->last_update_t_) > ros::Duration(1/this->config_.update_problem_rate))
+ || (dist.norm() > this->config_.update_problem_distance)
+ || (inc_ang > this->config_.update_problem_angle)
+ || this->new_estimated_pose_
+ || (!this->last_frame_min_detections_ && (this->features_.size() >= this->config_.update_problem_features_detected)))
{
this->last_frame_min_detections_ = !this->last_frame_min_detections_;
if (!this->waiting_for_feature_)
@@ -323,9 +345,9 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
frame_data.current_odom = this->tf_odom_base_;
if (!get_tf_map_odom(this->features_stamp_, frame_data.tf_map_odom))
{
- // this->new_features_data_ = false;
+ // this->new_front_features_data_ = false;
this->estimated_pose_mutex_exit();
- this->features_mutex_exit();
+ this->front_features_mutex_exit();
return;
}
//Get global robot_state
@@ -335,21 +357,28 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
{
if (!calculate_robot_state(frame_data.ts, frame_data.robot_state))
{
- // this->new_features_data_ = false;
+ // this->new_front_features_data_ = false;
this->estimated_pose_mutex_exit();
- this->features_mutex_exit();
+ this->front_features_mutex_exit();
return;
}
}
// Update frame information and Ceres problem
- if (this->alg_.get_frames_data_size() != 0 && land_success)
+ if (this->alg_.get_frames_data_size() != 0 && (front_land_success || rear_land_success))
{
- if (!update_landmarks_frame_data(frame_data))
+ front_land_success = true;
+ rear_land_success = true;
+ if (this->new_front_features_data_)
+ front_land_success = update_landmarks_frame_data(frame_data, true);
+ if (this->new_rear_features_data_)
+ rear_land_success = update_landmarks_frame_data(frame_data, false);
+
+ if (!front_land_success || !rear_land_success)
{
- // this->new_features_data_ = false;
+ // this->new_front_features_data_ = false;
this->estimated_pose_mutex_exit();
- this->features_mutex_exit();
+ this->front_features_mutex_exit();
return;
}
}
@@ -424,7 +453,7 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
this->reset_estimated_pose_ = false;
}
this->estimated_pose_mutex_exit();
- this->features_mutex_exit();
+ this->front_features_mutex_exit();
return;
}
tf_robot = this->tf2_buffer.lookupTransform(this->config_.global_frame, this->config_.base_link_frame, t);
@@ -506,32 +535,32 @@ void AdcLandmarksSlamSolverAlgNode::mainNodeThread(void)
// this->alg_.unlock();
this->estimated_pose_mutex_exit();
- this->features_mutex_exit();
+ this->front_features_mutex_exit();
}
-bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks)
+bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks, bool _front)
{
- ///Transform local features to global
+ ///Transform local front_features to global
//get transform from fixed_frame to sensor frame
geometry_msgs::TransformStamped tf_map_sensor_msg;
geometry_msgs::PoseStamped local_feature, global_feature;
- if (this->features_frame_ == "")
+ if ((_front && this->front_features_frame_ == "") || (!_front && this->rear_features_frame_ == ""))
{
- ROS_WARN("AdcLandmarksSlamSolverAlgNode::check_landmarks -> 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;
}
//Prepare landmarks status
if (_update_landmarks)
- prepare_landmark_candidates();
- this->alg_.reset_landmarks_detected(this->features_frame_);
+ prepare_landmark_candidates(_front);
+ this->alg_.reset_landmarks_detected((_front? this->front_features_frame_: this->rear_features_frame_));
+ tf_map_sensor_msg.header.frame_id = this->config_.global_frame;
+ tf_map_sensor_msg.header.stamp = this->features_stamp_;//front/rear
+ tf_map_sensor_msg.child_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
if(!this->config_.amcl_localization)
{
- tf_map_sensor_msg.header.frame_id = this->config_.global_frame;
- tf_map_sensor_msg.header.stamp = this->features_stamp_;
- tf_map_sensor_msg.child_frame_id = this->features_frame_;
tf2::Transform tf_map_odom;
tf2::Quaternion q;
@@ -540,12 +569,12 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks)
tf_map_odom.setRotation(q);
geometry_msgs::TransformStamped tf_odom_sensor_msg;
- if (!this->tf2_buffer.canTransform(this->config_.odom_frame, this->features_frame_, this->features_stamp_, ros::Duration(this->config_.tf_timeout)))
+ if (!this->tf2_buffer.canTransform(this->config_.odom_frame, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp, ros::Duration(this->config_.tf_timeout)))
{
- ROS_WARN_STREAM("LandmarksTrackerAlgNode:check_landmarks -> Can't transform from " << this->config_.odom_frame << " to " << this->features_frame_ << " at " << this->features_stamp_);
+ ROS_WARN_STREAM("LandmarksTrackerAlgNode:check_landmarks -> Can't transform from " << this->config_.odom_frame << " to " << tf_map_sensor_msg.child_frame_id << " at " << tf_map_sensor_msg.header.stamp);
return false;
}
- tf_odom_sensor_msg = this->tf2_buffer.lookupTransform(this->config_.odom_frame, this->features_frame_, this->features_stamp_);
+ tf_odom_sensor_msg = this->tf2_buffer.lookupTransform(this->config_.odom_frame, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp);
tf2::Transform tf_odom_sensor;
// tf2::transformMsgToTF2(tf_odom_sensor_msg.transform, tf_odom_sensor);
tf_odom_sensor = tf2::Transform(tf2::Quaternion(tf_odom_sensor_msg.transform.rotation.x, tf_odom_sensor_msg.transform.rotation.y, tf_odom_sensor_msg.transform.rotation.z, tf_odom_sensor_msg.transform.rotation.w), tf2::Vector3(tf_odom_sensor_msg.transform.translation.x, tf_odom_sensor_msg.transform.translation.y, tf_odom_sensor_msg.transform.translation.z));
@@ -563,20 +592,20 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks)
}
else
{
- if (!this->tf2_buffer.canTransform(this->config_.global_frame, this->features_frame_, this->features_stamp_, ros::Duration(this->config_.tf_timeout)))
+ if (!this->tf2_buffer.canTransform(this->config_.global_frame, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp, ros::Duration(this->config_.tf_timeout)))
{
- ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:check_landmarks -> Can't transform from " << this->config_.global_frame << " to " << this->features_frame_ << " at " << this->features_stamp_);
+ ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode:check_landmarks -> Can't transform from " << this->config_.global_frame << " to " << tf_map_sensor_msg.child_frame_id << " at " << tf_map_sensor_msg.header.stamp);
return false;
}
- tf_map_sensor_msg = this->tf2_buffer.lookupTransform(this->config_.global_frame, this->features_frame_, this->features_stamp_);
+ tf_map_sensor_msg = this->tf2_buffer.lookupTransform(this->config_.global_frame, tf_map_sensor_msg.child_frame_id, tf_map_sensor_msg.header.stamp);
}
- local_feature.header.stamp = this->features_stamp_;
- local_feature.header.frame_id = this->features_frame_;
- for (unsigned int i = 0; i < this->features_.size(); i++)
+ local_feature.header.stamp = this->features_stamp_;//front/rear
+ local_feature.header.frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
+ for (unsigned int i = 0; i < (_front? this->front_features_.size(): this->rear_features_.size()); i++)
{
//Get global feature
- local_feature.pose = this->features_[i].msg_pose;
+ local_feature.pose = (_front? this->front_features_: this->rear_features_)[i].msg_pose;
tf2::doTransform(local_feature, global_feature, tf_map_sensor_msg);
//Check if is a mapped landmark
@@ -585,39 +614,39 @@ bool AdcLandmarksSlamSolverAlgNode::check_landmarks(bool _update_landmarks)
tf2::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
Eigen::Vector3d feature_pose;
feature_pose << global_feature.pose.position.x, global_feature.pose.position.y, yaw;
- this->features_[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pose, this->features_[i].r, this->features_[i].th, this->features_[i].type, this->features_frame_);
+ (_front? this->front_features_: this->rear_features_)[i].landmark_key = this->alg_.is_a_mapped_landmark(feature_pose, (_front? this->front_features_: this->rear_features_)[i].r, (_front? this->front_features_: this->rear_features_)[i].th, (_front? this->front_features_: this->rear_features_)[i].type, (_front? this->front_features_frame_: this->rear_features_frame_));
//if it isn't a mapped landmark, update them if enabled
- if (_update_landmarks && (this->features_[i].landmark_key < 0.0))
+ if (_update_landmarks && ((_front? this->front_features_: this->rear_features_)[i].landmark_key < 0.0))
{
- this->features_[i].pose(0) = global_feature.pose.position.x;
- this->features_[i].pose(1) = global_feature.pose.position.y;
- this->features_[i].pose(2) = yaw;
- if (is_a_new_landmark(this->features_[i]))
+ (_front? this->front_features_: this->rear_features_)[i].pose(0) = global_feature.pose.position.x;
+ (_front? this->front_features_: this->rear_features_)[i].pose(1) = global_feature.pose.position.y;
+ (_front? this->front_features_: this->rear_features_)[i].pose(2) = yaw;
+ if (is_a_new_landmark((_front? this->front_features_: this->rear_features_)[i], _front))
{
LandmarkInfo landmark;
- landmark.pose = this->features_[i].pose;
+ landmark.pose = (_front? this->front_features_: this->rear_features_)[i].pose;
landmark.detected = true;
- landmark.source_frame_id = this->features_frame_;
- landmark.type = this->features_[i].type;
- double landmark_key = std::fmod(this->features_stamp_.toSec(), L_KEY_DIV)*100+i;
- this->features_[i].landmark_key = landmark_key;
+ landmark.source_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
+ landmark.type = (_front? this->front_features_: this->rear_features_)[i].type;
+ double landmark_key = std::fmod(this->front_features_stamp_.toSec(), L_KEY_DIV)*100+i;
+ (_front? this->front_features_: this->rear_features_)[i].landmark_key = landmark_key;
this->alg_.add_landmark(landmark_key, landmark);
- ROS_DEBUG_STREAM("New landmark " << landmark_key << " at " << this->features_[i].pose(0) << ", " << this->features_[i].pose(1) << "; yaw " << this->features_[i].pose(2));
+ ROS_DEBUG_STREAM("New landmark " << landmark_key << " at " << (_front? this->front_features_: this->rear_features_)[i].pose(0) << ", " << (_front? this->front_features_: this->rear_features_)[i].pose(1) << "; yaw " << (_front? this->front_features_: this->rear_features_)[i].pose(2));
}
}
}
return true;
}
-void AdcLandmarksSlamSolverAlgNode::prepare_landmark_candidates(void)
+void AdcLandmarksSlamSolverAlgNode::prepare_landmark_candidates(bool _front)
{
//Remove not detected candidates
if (!this->config_.landmarks_candidates_filter_en)
return;
for (auto it = this->landmarks_candidates_.begin(); it != this->landmarks_candidates_.end(); ++it)
{
- if (it->source_frame_id.compare(this->features_frame_) == 0)//equal
+ if (it->source_frame_id == (_front ? this->front_features_frame_: this->rear_features_frame_))
{
if (it->detected)
it->detected = false;
@@ -631,7 +660,7 @@ void AdcLandmarksSlamSolverAlgNode::prepare_landmark_candidates(void)
}
}
-bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature)
+bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature, bool _front)
{
//Check if a landmark candidate has been seen enough times
if (!this->config_.landmarks_candidates_filter_en)
@@ -660,7 +689,7 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature)
if (min_dist_land != this->landmarks_candidates_.end())
{
min_dist_land->detected = true;
- min_dist_land->source_frame_id = this->features_frame_;
+ min_dist_land->source_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
min_dist_land->count++;
if (min_dist_land->count >= this->config_.landmarks_min_detections)
{
@@ -673,32 +702,32 @@ bool AdcLandmarksSlamSolverAlgNode::is_a_new_landmark(FeatureInfo &_feature)
cand.pose = _feature.pose;
cand.count = 1;
cand.detected = true;
- cand.source_frame_id = this->features_frame_;
+ cand.source_frame_id = (_front? this->front_features_frame_: this->rear_features_frame_);
cand.type = _feature.type;
this->landmarks_candidates_.push_back(cand);
return false;
}
-bool AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data(FrameData& _frame_data)
+bool AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data(FrameData& _frame_data, bool _front)
{
//calculate tf from base_link to sensor
geometry_msgs::TransformStamped tf;
geometry_msgs::PoseStamped from_sensor, from_base_link;
- if (!this->tf2_buffer.canTransform(this->config_.base_link_frame, this->features_frame_, this->features_stamp_, ros::Duration(this->config_.tf_timeout)))
+ 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 " << this->features_frame_ << " at " << this->features_stamp_);
+ 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_);
return false;
}
- tf = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, this->features_frame_, this->features_stamp_);
- from_sensor.header.stamp = this->features_stamp_;
- from_sensor.header.frame_id = this->features_frame_;
- for (unsigned int i = 0; i < this->features_.size(); i++)
+ tf = this->tf2_buffer.lookupTransform(this->config_.base_link_frame, (_front? this->front_features_frame_: this->rear_features_frame_), this->features_stamp_);
+ from_sensor.header.stamp = this->features_stamp_;//front/rear
+ 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->features_[i].landmark_key > 0.0)
+ if(this->front_features_[i].landmark_key > 0.0)
{
LandmarkResidualData land_res_data;
// Transform sensor info to base_link frame
- from_sensor.pose = this->features_[i].msg_pose;
+ from_sensor.pose = (_front? this->front_features_: this->rear_features_)[i].msg_pose;
tf2::doTransform(from_sensor, from_base_link, tf);
Eigen::Vector2d feature;
feature << from_base_link.pose.position.x, from_base_link.pose.position.y;
@@ -709,7 +738,7 @@ bool AdcLandmarksSlamSolverAlgNode::update_landmarks_frame_data(FrameData& _fram
while (th < -M_PI)
th += 2*M_PI;
land_res_data.theta = th;
- land_res_data.landmark_key = this->features_[i].landmark_key;
+ land_res_data.landmark_key = (_front? this->front_features_: this->rear_features_)[i].landmark_key;
_frame_data.landmark_residuals_data.push_back(land_res_data);
}
}
@@ -796,6 +825,36 @@ void AdcLandmarksSlamSolverAlgNode::load_landmarks(void)
ROS_ERROR_STREAM("AdcLandmarksSlamSolverAlgNode::load_landmarks -> File" << this->config_.landmarks_map_file << " can't be opened");
}
+void AdcLandmarksSlamSolverAlgNode::merge_features()
+{
+ std::vector<FeatureInfo>().swap(this->features_);
+ 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());
+ for (unsigned int i=0; i<this->rear_features_.size(); i++)
+ this->features_[i] = this->rear_features_[i];
+ }
+ else
+ {
+ if (this->new_rear_features_data_)
+ 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();
+ this->features_.resize(this->features_.size() + this->front_features_.size());
+ for (unsigned int i=0; i<this->front_features_.size(); i++)
+ this->features_[old_size + i] = this->front_features_[i];
+ }
+ else
+ {
+ if (this->new_front_features_data_)
+ ROS_WARN_STREAM("AdcLandmarksSlamSolverAlgNode::merge_features -> front features too old: last stamp " << this->front_features_stamp_);
+ this->new_front_features_data_ = false;
+ }
+}
+
void AdcLandmarksSlamSolverAlgNode::publish_landmarks_markers()
{
this->landmarks_MarkerArray_msg_.markers.resize(this->alg_.get_landmarks_size());
@@ -803,7 +862,7 @@ void AdcLandmarksSlamSolverAlgNode::publish_landmarks_markers()
for (unsigned int i = 0; i < this->landmarks_MarkerArray_msg_.markers.size() && it != this->alg_.get_landmarks_end(); i++, ++it)
{
this->landmarks_MarkerArray_msg_.markers[i].header.frame_id = this->config_.global_frame;
- this->landmarks_MarkerArray_msg_.markers[i].header.stamp = this->features_stamp_;
+ this->landmarks_MarkerArray_msg_.markers[i].header.stamp = this->front_features_stamp_;
this->landmarks_MarkerArray_msg_.markers[i].id = i;
this->landmarks_MarkerArray_msg_.markers[i].ns = "landmarks";
// this->landmarks_MarkerArray_msg_.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
@@ -1028,17 +1087,19 @@ void AdcLandmarksSlamSolverAlgNode::estimated_pose_mutex_exit(void)
pthread_mutex_unlock(&this->estimated_pose_mutex_);
}
-void AdcLandmarksSlamSolverAlgNode::features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg)
+void AdcLandmarksSlamSolverAlgNode::front_features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg)
{
ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::landmarks_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
- this->features_mutex_enter();
- this->features_frame_ = msg->header.frame_id;
+ this->front_features_mutex_enter();
+ this->front_features_frame_ = msg->header.frame_id;
+ this->front_features_stamp_ = msg->header.stamp;
this->features_stamp_ = msg->header.stamp;
+ this->new_front_features_data_ = true;
this->new_features_data_ = true;
- this->features_.resize(msg->features.size());
+ this->front_features_.resize(msg->features.size());
for (unsigned int i = 0; i < msg->features.size(); i++)
{
FeatureInfo feature;
@@ -1049,25 +1110,69 @@ void AdcLandmarksSlamSolverAlgNode::features_callback(const iri_adc_msgs::featur
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);
- this->features_[i] = feature;
+ feature.frame = msg->header.frame_id;
+ this->front_features_[i] = feature;
}
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();
- this->features_mutex_exit();
+ this->front_features_mutex_exit();
}
-void AdcLandmarksSlamSolverAlgNode::features_mutex_enter(void)
+void AdcLandmarksSlamSolverAlgNode::front_features_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->front_features_mutex_);
+}
+
+void AdcLandmarksSlamSolverAlgNode::front_features_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->front_features_mutex_);
+}
+
+
+void AdcLandmarksSlamSolverAlgNode::rear_features_callback(const iri_adc_msgs::feature_array::ConstPtr& msg)
{
- pthread_mutex_lock(&this->features_mutex_);
+ ROS_DEBUG("AdcLandmarksSlamSolverAlgNode::landmarks_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->front_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++)
+ {
+ 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;
+ }
+
+ //std::cout << msg->data << std::endl;
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->front_features_mutex_exit();
}
-void AdcLandmarksSlamSolverAlgNode::features_mutex_exit(void)
+void AdcLandmarksSlamSolverAlgNode::rear_features_mutex_enter(void)
{
- pthread_mutex_unlock(&this->features_mutex_);
+ pthread_mutex_lock(&this->rear_features_mutex_);
}
+void AdcLandmarksSlamSolverAlgNode::rear_features_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->rear_features_mutex_);
+}
/* [service callbacks] */
@@ -1078,7 +1183,7 @@ void AdcLandmarksSlamSolverAlgNode::features_mutex_exit(void)
void AdcLandmarksSlamSolverAlgNode::node_config_update(Config &config, uint32_t level)
{
this->alg_.lock();
- this->features_mutex_enter();
+ this->front_features_mutex_enter();
if (config.load_landmarks && !this->config_.load_landmarks)
load_landmarks();
if (!config.load_landmarks && this->config_.load_landmarks)
@@ -1098,7 +1203,7 @@ void AdcLandmarksSlamSolverAlgNode::node_config_update(Config &config, uint32_t
}
}
this->config_=config;
- this->features_mutex_exit();
+ this->front_features_mutex_exit();
this->alg_.unlock();
}