Main -> cluster-outline merge

README.md

@@ -3,6 +3,9 @@ LIDAR pointcloud clustering in `ROS 2` `Humble`
 
 [![Static Badge](https://img.shields.io/badge/ROS_2-Humble-34aec5)](https://docs.ros.org/en/humble/)
 
+<center>
+<img src="img/lidar_cluster01.png" width="80%"> 
+</center>
 
 ## Build this `ROS 2` package
 
@@ -32,6 +35,9 @@ or with optimized build:
 MAKEFLAGS="-j4" colcon build --packages-select lidar_cluster --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 
+> [!TIP]
+> There is a detailded decription how to make this package work with ground segmentation: [jkk-research.github.io/workshops/clustering_a](https://jkk-research.github.io/workshops/clustering_a/)
+
 ## ROS 2 graph
 
 

include/cluster_outline.hpp

@@ -26,7 +26,7 @@ void addPointsIfNecessary(pcl::PointCloud<pcl::PointXYZ>::Ptr& hull,
 // The input pointcloud has to be PointXYZI where Intensity is the cluster ID.
 // max_added_points is the maximum number of points that can be added to the hull
 void computeOutline(pcl::PointCloud<pcl::PointXYZI>::Ptr& pointcloud, 
-                        visualization_msgs::msg::MarkerArray& hull_markers, int max_added_points);
+                        visualization_msgs::msg::MarkerArray& hull_markers, int max_added_points, int max_clust_reached, std::string frame_id);
 
 
 }; // class ClusterOutline

launch/euclidean_grid.launch.py

@@ -19,7 +19,7 @@ def generate_launch_description():
                 {'points_in_topic': LaunchConfiguration("topic")},
                 {'points_out_topic': 'clustered_points'},
                 {'marker_out_topic': 'clustered_marker'},
-                {'verbose1': True},
+                {'verbose1': False},
                 {'verbose2': False},
             ]
         )

src/cluster_outline.cpp

@@ -59,10 +59,8 @@ void ClusterOutline::addPointsIfNecessary(pcl::PointCloud<pcl::PointXYZ>::Ptr& h
 
 
 void ClusterOutline::computeOutline(pcl::PointCloud<pcl::PointXYZI>::Ptr& pointcloud, 
-                    visualization_msgs::msg::MarkerArray& hull_markers, int max_added_points) {
+                    visualization_msgs::msg::MarkerArray& hull_markers, int max_added_points, int max_clust_reached, std::string frame_id) {
 
-    // Clear the previous markers
-    hull_markers.markers.clear();
 
     // Map to store clusters, with the intensity as the key and the points as the value
     std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;
@@ -93,23 +91,23 @@ void ClusterOutline::computeOutline(pcl::PointCloud<pcl::PointXYZI>::Ptr& pointc
         chull.reconstruct(*cloud_hull);
 
         // Add intermediate points if necessary
-        double max_distance = 2.0;  // maximum allowed distance between consecutive points 
+        double max_distance = 4.0;  // maximum allowed distance between consecutive points 
         int counter = 0;
         addPointsIfNecessary(cloud_hull, cluster.second, max_distance, counter, max_added_points);
 
         // Create a marker for this cluster
         visualization_msgs::msg::Marker hull_marker;
-        hull_marker.header.frame_id = "lexus3/os_center_a_laser_data_frame"; // Replace with your frame
+        hull_marker.header.frame_id = frame_id;
         hull_marker.header.stamp = rclcpp::Clock().now();
-        hull_marker.ns = "hull" + std::to_string(cluster_id);
+        hull_marker.ns = "hull";
         hull_marker.id = cluster_id++;
         hull_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
         hull_marker.action = visualization_msgs::msg::Marker::ADD;
-        hull_marker.scale.x = 0.1;
+        hull_marker.scale.x = 0.2;
         hull_marker.color.a = 1.0;
-        hull_marker.color.r = 1.0; // make it white
-        hull_marker.color.g = 1.0;
-        hull_marker.color.b = 1.0;
+        hull_marker.color.r = 0.30; // 0.30 0.69 0.31 md_green_500 https://github.com/jkk-research/colors
+        hull_marker.color.g = 0.69;
+        hull_marker.color.b = 0.31;
 
         // Add the points of the Convex Hull to the marker
         for (const auto& point : cloud_hull->points) {
@@ -130,6 +128,19 @@ void ClusterOutline::computeOutline(pcl::PointCloud<pcl::PointXYZI>::Ptr& pointc
         // Add the marker to the array
         hull_markers.markers.push_back(hull_marker);
     }
+    // Add markers for clusters that are not present in the current frame to avoid ghost markers   
+    visualization_msgs::msg::Marker hull_marker;
+    hull_marker.header.stamp = rclcpp::Clock().now();
+    hull_marker.header.frame_id = frame_id;
+    hull_marker.ns = "hull";
+    hull_marker.color.a = 0.0;    // alpha = 0.0 makes the marker invisible
+    hull_marker.scale.x = 0.2;
+    hull_marker.scale.y = 0.2;
+    hull_marker.scale.z = 0.2;
+    for(int i = clusters.size() + 1 ; i < max_clust_reached; i++) {
+        hull_marker.id = cluster_id++;
+        hull_markers.markers.push_back(hull_marker);
+    }
 
 }
 

src/euclidean_grid.cpp

@@ -240,7 +240,6 @@ class EuclideanGrid : public rclcpp::Node
     sub_lidar_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(points_in_topic, 10, std::bind(&EuclideanGrid::lidar_callback, this, std::placeholders::_1));
     callback_handle_ = this->add_on_set_parameters_callback(std::bind(&EuclideanGrid::parametersCallback, this, std::placeholders::_1));
 
-    marker_pub = this->create_publisher<visualization_msgs::msg::MarkerArray>("hull_markers_topic", 10);
 
     RCLCPP_INFO(this->get_logger(), "EuclideanGrid node has been started.");
     RCLCPP_INFO(this->get_logger(), "Subscribing to: '%s'", points_in_topic.c_str());
@@ -312,7 +311,8 @@ class EuclideanGrid : public rclcpp::Node
         cloud_filtered->points.push_back(point_i);
       }
     }
-
+    max_clust_reached = std::max(prev_cluster_size, num_of_clusters);
+
     std::vector<double> center_x(num_of_clusters + 1), center_y(num_of_clusters + 1);
     std::vector<int> count(num_of_clusters + 1);
     for (int i = 0; i <= num_of_clusters; i++) {
@@ -339,12 +339,21 @@ class EuclideanGrid : public rclcpp::Node
         mark_array.markers.push_back(center_marker);
       }
     }
+    // Add markers for clusters that are not present in the current frame to avoid ghost markers
+    for(int i = num_of_clusters + 1; i <= max_clust_reached; i++) {
+      visualization_msgs::msg::Marker center_marker;
+      init_center_marker(center_marker, 0, 0, i);
+      center_marker.header.frame_id = input_msg->header.frame_id;
+      center_marker.header.stamp = this->now();
+      center_marker.color.a = 0.0;
+      mark_array.markers.push_back(center_marker);
+    }
 
     clustering.finish();
     convex_hull.start("convex_hull", verbose2);
 
     // Compute and draw an outline around the clusters
-    cluster_outline.computeOutline(cloud_filtered, hull_markers, 20);
+    cluster_outline.computeOutline(cloud_filtered, mark_array, 20, max_clust_reached, input_msg->header.frame_id);
 
     convex_hull.finish();
     fullbenchmark.finish();
@@ -355,24 +364,19 @@ class EuclideanGrid : public rclcpp::Node
     output_msg.header.frame_id = input_msg->header.frame_id;
     pub_lidar_->publish(output_msg);
     pub_marker_->publish(mark_array);
-    marker_pub->publish(hull_markers);
-
+    prev_cluster_size = num_of_clusters;
   } // EuclideanGrid::lidar_callback
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_lidar_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_marker_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_lidar_;
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;
-
-  visualization_msgs::msg::MarkerArray hull_markers;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub;
-
   outline::ClusterOutline cluster_outline;
 
   float minX = -80.0, minY = -25.0, minZ = -2.0;
   float maxX = +80.0, maxY = +25.0, maxZ = -0.15;
   float tolerance = 5;
-  int max_cluster_size = 4000;
+  int max_cluster_size = 400, max_clust_reached = 0, prev_cluster_size = 0;
   float voxel_leaf_size = 3.0;
   int min_points_number_per_voxel = 5;
   bool verbose1 = false, verbose2 = false, pub_undecided = false;