Commit 834b5e61 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Merge branch 'development' into 'master'

Adapted to two features input topics

See merge request !1
parents a3492b18 f5f3d667
......@@ -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