Added marker visualization publication

cfg/AdcTagLocalizationFilter.cfg

@@ -43,6 +43,7 @@ time_persistence_filter = gen.add_group("Detection time persistence filter param
 
 #       Name     Type       Reconf.level Description             Default Min   Max
 gen.add("rate",  double_t,  0,           "Main loop rate (Hz)",  10.0,   0.1, 1000.0)
+gen.add("publish_visualization",  bool_t,  0,           "Boolean to publish or not visualization markers",  True)
 
 range_filter.add("range_filter_en",               bool_t,       0,           "Boolean to filter detections by range",       False)
 range_filter.add("detection_max_range",           double_t,     0,           "Max range to take into account a detection",  3.0, 1.0, 12.0)

config/params.yaml

-rate: 10
+rate: 100
+publish_visualization: True
 
 range_filter_en: True
-detection_max_range: 3.0
+detection_max_range: 4.0
 
 confidence_filter_en: False
 detection_min_confidence: 126

include/adc_tag_localization_filter_alg_node.h

@@ -71,7 +71,7 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
 
     // [publisher attributes]
     ros::Publisher tag_features_publisher_;
-    visualization_msgs::MarkerArray tag_features_MarkerArray_msg_;
+    visualization_msgs::MarkerArray visualization_msg_;
 
     ros::Publisher landmarks_publisher_;
     iri_adc_msgs::landmark_array landmarks_msg_;
@@ -122,6 +122,8 @@ class AdcTagLocalizationFilterAlgNode : public algorithm_base::IriBaseAlgorithm<
     bool update_feature_candidates(double r, double theta);
 
     void update_landmarks_msg();
+
+    void publish_visualization_msg();
   public:
    /**
     * \brief Constructor

src/adc_tag_localization_filter_alg_node.cpp

@@ -38,6 +38,10 @@ AdcTagLocalizationFilterAlgNode::AdcTagLocalizationFilterAlgNode(void) :
       this->ids_enabled.push_back(id);
     }
   }
+  std::cout << "Ids enabled: ";
+  for (unsigned int i = 0; i < this->ids_enabled.size(); i++)
+    std::cout << this->ids_enabled[i] << ", ";
+  std::cout << std::endl;
   this->new_data = false;
 
   // [init services]
@@ -70,6 +74,8 @@ void AdcTagLocalizationFilterAlgNode::mainNodeThread(void)
   {
     update_landmarks_msg();
     this->landmarks_publisher_.publish(this->landmarks_msg_);
+    if (this->config_.publish_visualization)
+      publish_visualization_msg();
     this->new_data = false;
   }
   // [fill msg structures]
@@ -169,6 +175,43 @@ void AdcTagLocalizationFilterAlgNode::update_landmarks_msg()
   }
 }
 
+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[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].id = i;
+    this->visualization_msg_.markers[i].ns = this->landmarks_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.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].scale.x = 0.05;
+    this->visualization_msg_.markers[i].scale.y = 0.05;
+    this->visualization_msg_.markers[i].scale.z = 0.02;
+
+    this->visualization_msg_.markers[i].color.a = 1.0;
+    this->visualization_msg_.markers[i].color.r = 0.0;
+    this->visualization_msg_.markers[i].color.g = 1.0;
+    this->visualization_msg_.markers[i].color.b = 0.0;
+  }
+  if (this->visualization_msg_.markers.size() > 0)
+    this->tag_features_publisher_.publish(this->visualization_msg_);
+}
+
 /*  [subscriber callbacks] */
 void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)
 {
@@ -197,7 +240,6 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
     dist = position.norm();
     if (is_a_tag(dist, msg->markers[i].confidence, msg->markers[i].id))
     {
-      ROS_INFO("is a tag");
       double theta = std::atan2(msg->markers[i].pose.pose.position.x, msg->markers[i].pose.pose.position.z);
       while (theta >= M_PI)
         theta -= 2*M_PI;
@@ -217,10 +259,6 @@ void AdcTagLocalizationFilterAlgNode::ar_tag_detections_callback(const ar_track_
         this->candidates.push_back(cand);
       }
     }
-    else
-    {
-      ROS_INFO("not a tag");
-    }
   }
   this->new_data = true;