Commit f5f3d667 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Adapted to two features input topics

parent a3492b18
......@@ -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)
......
......@@ -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.
*/
......
......@@ -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>
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment