Adapted to name change from landmarks to features. Updated documentation.

include/adc_tag_localization_filter_alg_node.h

@@ -33,8 +33,8 @@
 
 // [publisher subscriber headers]
 #include <visualization_msgs/MarkerArray.h>
-#include <iri_adc_msgs/landmark_array.h>
-#include <iri_adc_msgs/landmark.h>
+#include <iri_adc_msgs/feature_array.h>
+#include <iri_adc_msgs/feature.h>
 #include <ar_track_alvar_msgs/AlvarMarkers.h>
 
 // [service client headers]
@@ -51,10 +51,10 @@ typedef struct {
   geometry_msgs::Pose pose; ///< Ar tag pose.
   double r; ///< Range at detection.
   double th; ///< Angle at detection.
-  bool detected;
-  int count;
-  int id;
-  int confidence;
+  bool detected; ///< If it has been detected.
+  int count; ///< Number of time detected in a row
+  int id; ///< Tag id
+  int confidence; ///< Tag confidence
 }Feature_candidate;
 
 /**
@@ -73,8 +73,8 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
     ros::Publisher tag_features_publisher_;
     visualization_msgs::MarkerArray visualization_msg_;
 
-    ros::Publisher landmarks_publisher_;
-    iri_adc_msgs::landmark_array landmarks_msg_;
+    ros::Publisher features_publisher_;
+    iri_adc_msgs::feature_array features_msg_;
 
 
     // [subscriber attributes]
@@ -102,27 +102,49 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
     Config config_;
 
     /**
-    * \brief config variable
+    * \brief Function to apply range, confidence and id filters.
     *
-    * This variable has all the driver parameters defined in the cfg config file.
-    * Is updated everytime function config_update() is called.
+    * \param range The detection distance.
+    *
+    * \param confidence The detection confidence.
+    *
+    * \param id The Tag id.
+    *
+    * \return If is a Tag.
     */
     bool is_a_tag(double range, int confidence, int id);
 
     /**
-    * \brief config variable
+    * \brief Function to filter tag by ids if enabled
     *
-    * This variable has all the driver parameters defined in the cfg config file.
-    * Is updated everytime function config_update() is called.
+    * \param id The Tag id.
+    *
+    * \return If is id enabled.
     */
     bool check_tag_id(int id);
 
+    /**
+     * \brief Function to remove feature candidates not detected and reinit its variables.
+     */
     void prepare_feature_candidates();
 
+    /**
+     * \brief Function to update features candidates applying time persistance filter if enabled.
+     *
+     * \param r The detection distance.
+     *
+     * \param theta The detection theta.
+     */
     bool update_feature_candidates(double r, double theta);
 
-    void update_landmarks_msg();
+    /**
+     * \brief Function to update features msg.
+     */
+    void update_features_msg();
 
+    /**
+     * \brief Function to publish visualization markers.
+     */
     void publish_visualization_msg();
   public:
    /**

src/adc_tag_localization_filter_alg_node.cpp

@@ -13,7 +13,7 @@ AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode(void) :
 
   // [init publishers]
   this->tag_features_publisher_ = this->private_node_handle_.advertise<visualization_msgs::MarkerArray>("tag_features", 1);
-  this->landmarks_publisher_ = this->private_node_handle_.advertise<iri_adc_msgs::landmark_array>("landmarks", 1);
+  this->features_publisher_ = this->private_node_handle_.advertise<iri_adc_msgs::feature_array>("features", 1);
   
   // [init subscribers]
   this->ar_tag_detections_subscriber_ = this->private_node_handle_.subscribe("ar_tag_detections", 1, &AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback, this);
@@ -72,8 +72,8 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
 
   if (this->new_data)
   {
-    update_landmarks_msg();
-    this->landmarks_publisher_.publish(this->landmarks_msg_);
+    update_features_msg();
+    this->features_publisher_.publish(this->features_msg_);
     if (this->config_.publish_visualization)
       publish_visualization_msg();
     this->new_data = false;
@@ -83,7 +83,7 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
   //this->tag_features_MarkerArray_msg_.data = my_var;
 
   // Initialize the topic message structure
-  //this->landmarks_landmark_array_msg_.data = my_var;
+  //this->features_feature_array_msg_.data = my_var;
 
   
   // [fill srv structure and make request to the server]
@@ -95,7 +95,7 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
   //this->tag_features_publisher_.publish(this->tag_features_MarkerArray_msg_);
 
   // Uncomment the following line to publish the topic message
-  //this->landmarks_publisher_.publish(this->landmarks_landmark_array_msg_);
+  //this->features_publisher_.publish(this->features_feature_array_msg_);
 
   this->ar_tag_detections_mutex_exit();
   // this->alg_.unlock();
@@ -159,45 +159,45 @@ bool AdcTagLocalizationFilterAlgNode::update_feature_candidates(double r, double
   return true;
 }
 
-void AdcTagLocalizationFilterAlgNode::update_landmarks_msg()
+void AdcTagLocalizationFilterAlgNode::update_features_msg()
 {
-  std::vector<iri_adc_msgs::landmark>().swap(this->landmarks_msg_.landmarks);
+  std::vector<iri_adc_msgs::feature>().swap(this->features_msg_.features);
   for (auto it = this->candidates.begin(); it != this->candidates.end(); ++it)
   {
     if (!this->config_.time_persistance_filter_en || (it->count >= this->config_.time_persistance_min_detections))
     {
-      iri_adc_msgs::landmark landmark;
-      landmark.pose = it->pose;
-      landmark.probability = it->confidence;
-      landmark.type = it->id;
-      this->landmarks_msg_.landmarks.push_back(landmark);
+      iri_adc_msgs::feature feature;
+      feature.pose = it->pose;
+      feature.probability = it->confidence;
+      feature.type = it->id;
+      this->features_msg_.features.push_back(feature);
     }
   }
 }
 
 void AdcTagLocalizationFilterAlgNode::publish_visualization_msg()
 {
-  this->visualization_msg_.markers.resize(this->landmarks_msg_.landmarks.size());
-  for (unsigned int i = 0; i < this->landmarks_msg_.landmarks.size(); i++)
+  this->visualization_msg_.markers.resize(this->features_msg_.features.size());
+  for (unsigned int i = 0; i < this->features_msg_.features.size(); i++)
   {
-    this->visualization_msg_.markers[i].header.frame_id = this->landmarks_msg_.header.frame_id;
-    this->visualization_msg_.markers[i].header.stamp = this->landmarks_msg_.header.stamp;
+    this->visualization_msg_.markers[i].header.frame_id = this->features_msg_.header.frame_id;
+    this->visualization_msg_.markers[i].header.stamp = this->features_msg_.header.stamp;
     this->visualization_msg_.markers[i].id = i;
-    this->visualization_msg_.markers[i].ns = this->landmarks_msg_.header.frame_id;
+    this->visualization_msg_.markers[i].ns = this->features_msg_.header.frame_id;
     this->visualization_msg_.markers[i].ns.append("_features");
     // this->visualization_msg_.markers[i].type = visualization_msgs::Marker::TEXT_VIEW_FACING;
     this->visualization_msg_.markers[i].type = visualization_msgs::Marker::CYLINDER;
     this->visualization_msg_.markers[i].action = visualization_msgs::Marker::ADD;
     this->visualization_msg_.markers[i].lifetime = ros::Duration(0.1);
 
-    this->visualization_msg_.markers[i].pose.position.x = this->landmarks_msg_.landmarks[i].pose.position.x;
-    this->visualization_msg_.markers[i].pose.position.y = this->landmarks_msg_.landmarks[i].pose.position.y;
-    this->visualization_msg_.markers[i].pose.position.z = this->landmarks_msg_.landmarks[i].pose.position.z;
+    this->visualization_msg_.markers[i].pose.position.x = this->features_msg_.features[i].pose.position.x;
+    this->visualization_msg_.markers[i].pose.position.y = this->features_msg_.features[i].pose.position.y;
+    this->visualization_msg_.markers[i].pose.position.z = this->features_msg_.features[i].pose.position.z;
 
-    this->visualization_msg_.markers[i].pose.orientation.x = this->landmarks_msg_.landmarks[i].pose.orientation.x;
-    this->visualization_msg_.markers[i].pose.orientation.y = this->landmarks_msg_.landmarks[i].pose.orientation.y;
-    this->visualization_msg_.markers[i].pose.orientation.z = this->landmarks_msg_.landmarks[i].pose.orientation.z;
-    this->visualization_msg_.markers[i].pose.orientation.w = this->landmarks_msg_.landmarks[i].pose.orientation.w;
+    this->visualization_msg_.markers[i].pose.orientation.x = this->features_msg_.features[i].pose.orientation.x;
+    this->visualization_msg_.markers[i].pose.orientation.y = this->features_msg_.features[i].pose.orientation.y;
+    this->visualization_msg_.markers[i].pose.orientation.z = this->features_msg_.features[i].pose.orientation.z;
+    this->visualization_msg_.markers[i].pose.orientation.w = this->features_msg_.features[i].pose.orientation.w;
 
     this->visualization_msg_.markers[i].scale.x = 0.05;
     this->visualization_msg_.markers[i].scale.y = 0.05;
@@ -222,13 +222,13 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
   this->ar_tag_detections_mutex_enter();
   if (msg->markers.size() == 0)
   {
-    this->landmarks_msg_.header.frame_id = msg->header.frame_id;
-    this->landmarks_msg_.header.stamp = msg->header.stamp;
+    this->features_msg_.header.frame_id = msg->header.frame_id;
+    this->features_msg_.header.stamp = msg->header.stamp;
   }
   else
   {
-    this->landmarks_msg_.header.frame_id = msg->markers[0].header.frame_id;
-    this->landmarks_msg_.header.stamp = msg->markers[0].header.stamp;
+    this->features_msg_.header.frame_id = msg->markers[0].header.frame_id;
+    this->features_msg_.header.stamp = msg->markers[0].header.stamp;
   }
   
   prepare_feature_candidates();