Adapted to two features input topics

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)

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.
      */

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>