Add a marker topic

This feature add a new topic on which the circles detected by the node
are returned as a spherical marker.
This facilitates the debugging process on rviz2.

Signed-off-by: Thomas Bonnefille <[email protected]>

CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(cdf_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -25,7 +26,7 @@ install(TARGETS
         cluster_detector
         DESTINATION lib/${PROJECT_NAME})
 
-ament_target_dependencies(cluster_detector rclcpp sensor_msgs cdf_msgs)
+ament_target_dependencies(cluster_detector rclcpp sensor_msgs cdf_msgs visualization_msgs)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

include/cluster_detector/cluster_detector.hpp

@@ -11,12 +11,15 @@
 #include "cdf_msgs/msg/obstacles.hpp"
 #include "cdf_msgs/msg/circle_obstacle.hpp"
 
+#include "visualization_msgs/msg/marker.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
+
 #include <cmath>
 #include <vector>
 
 #define NB_POINT_SCAN 1600
 #define NB_MEDIAN 2
-#define DIST_CLUSTER 0.2
+#define DIST_CLUSTER 0.1
 #define PI 3.141592653589793
 
 struct PolarPoint {
@@ -34,14 +37,17 @@ class ClusterDetector : public rclcpp::Node{
     void clusterize();
     void create_message();
     void publish_obstacle();
+    void publish_marker();
 
     int nb_cluster;
+    int nb_marker = 0;
 
     PolarPoint points[NB_POINT_SCAN];
     cdf_msgs::msg::Obstacles buffer_message;
 
     rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub;
     rclcpp::Publisher<cdf_msgs::msg::Obstacles>::SharedPtr obstacle_pub;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub;
 };
 
 #endif

package.xml

@@ -12,6 +12,7 @@
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>cdf_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/cluster_detector.cpp

@@ -5,6 +5,7 @@ using std::placeholders::_1;
 ClusterDetector::ClusterDetector() : Node("cluster_detector"){
     this->scan_sub = this->create_subscription<sensor_msgs::msg::LaserScan>("scan",1000,std::bind(&ClusterDetector::scan_callback, this, _1));
     this->obstacle_pub = this->create_publisher<cdf_msgs::msg::Obstacles>("raw_obstacles",1000);
+    this->marker_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("markers",1000);
 }
 
 ClusterDetector::~ClusterDetector(){
@@ -53,6 +54,7 @@ void ClusterDetector::scan_callback(const sensor_msgs::msg::LaserScan msg){
 
     create_message();
 
+    publish_marker();
     publish_obstacle();
 }
 
@@ -158,6 +160,42 @@ void ClusterDetector::publish_obstacle(){
     buffer_message.circles.clear();
 }
 
+void ClusterDetector::publish_marker(){
+    visualization_msgs::msg::MarkerArray markers;
+    visualization_msgs::msg::MarkerArray flush;
+    visualization_msgs::msg::Marker tmp;
+    int i, prev_size;
+    prev_size = this->nb_marker;
+
+    this->nb_marker = this->buffer_message.circles.size();
+
+    for (i = 0; i<this->nb_marker; i++){
+        tmp.header.frame_id = "laser";
+        tmp.action = visualization_msgs::msg::Marker::ADD;
+        tmp.type = visualization_msgs::msg::Marker::SPHERE;
+        tmp.id = i;
+        tmp.scale.x = 0.1;
+        tmp.scale.y = 0.1;
+        tmp.scale.z = 0.1;
+        tmp.color.a = 1;
+        tmp.color.r = 0;
+        tmp.color.g = 1;
+        tmp.color.b = 0;
+        tmp.pose.position.x = this->buffer_message.circles.at(i).center.x;
+        tmp.pose.position.y = this->buffer_message.circles.at(i).center.y;
+        tmp.pose.position.z = this->buffer_message.circles.at(i).center.z;
+        markers.markers.push_back(tmp);
+    }
+    for (i = this->nb_marker; i<prev_size; i++){
+        tmp.id = i;
+        tmp.action = visualization_msgs::msg::Marker::DELETE;
+        markers.markers.push_back(tmp);
+    }
+
+    this->marker_pub->publish(markers);
+
+}
+
 int main(int argc, char * argv[]){
     rclcpp::init(argc, argv);
     rclcpp::spin(std::make_shared<ClusterDetector>());