Merge pull request #8 from jkk-research/cluster-outline

Cluster outlining for all working nodes

Author: horverno

CMakeLists.txt

@@ -61,7 +61,7 @@ set(OUTLINE_SOURCES
   include/cluster_outline.hpp
 )
 
-add_executable(dbscan_spatial src/dbscan_spatial.cpp ${BENCHMARK_SOURCES})
+add_executable(dbscan_spatial src/dbscan_spatial.cpp ${BENCHMARK_SOURCES} ${OUTLINE_SOURCES})
 target_link_libraries(dbscan_spatial ${PCL_LIBRARIES})
 ament_target_dependencies(dbscan_spatial  ${ament_dependencies} )
 target_compile_features(dbscan_spatial PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
@@ -76,7 +76,7 @@ target_link_libraries(dblane_spatial ${PCL_LIBRARIES})
 ament_target_dependencies(dblane_spatial  ${ament_dependencies} )
 target_compile_features(dblane_spatial PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 
-add_executable(euclidean_spatial src/euclidean_spatial.cpp)
+add_executable(euclidean_spatial src/euclidean_spatial.cpp ${OUTLINE_SOURCES})
 target_link_libraries(euclidean_spatial ${PCL_LIBRARIES})
 ament_target_dependencies(euclidean_spatial  ${ament_dependencies} )
 target_compile_features(euclidean_spatial PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17

src/dbscan_spatial.cpp

@@ -24,6 +24,8 @@
 // TBB
 #include <tbb/tbb.h>
 
+#include "cluster_outline.hpp"
+
 // Point class, which stores the x and y coordinates of a point, the number of neighbors, whether it is a core point, and the cluster ID
 class Point {
 public:
@@ -429,6 +431,18 @@ class DbscanSpatial : public rclcpp::Node
       }
     }
 
+    // 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);
+    }
+
+    cluster_outline.computeOutline(cloud_filtered, mark_array, 20, max_clust_reached, input_msg->header.frame_id);
+
     // Convert to ROS data type
     sensor_msgs::msg::PointCloud2 output_msg;
     pcl::toROSMsg(*cloud_filtered, output_msg);
@@ -438,6 +452,7 @@ class DbscanSpatial : public rclcpp::Node
     pub_lidar_->publish(output_msg);
     pub_marker_->publish(mark_array);
 
+
     bench4.finish();
 
     fullbenchmark.finish();
@@ -448,12 +463,14 @@ class DbscanSpatial : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_marker_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_lidar_;
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+  outline::ClusterOutline cluster_outline;
   float minX = -80.0, minY = -25.0, minZ = -2.0;
   float maxX = +80.0, maxY = +25.0, maxZ = -0.15;
   double eps = 3.5;
   bool verbose1 = false, verbose2 = false, pub_undecided = false;
   std::string points_in_topic, points_out_topic, marker_out_topic;
   size_t count_;
+  int max_clust_reached = 0;
 };
 
 int main(int argc, char *argv[])

src/euclidean_spatial.cpp

@@ -30,6 +30,8 @@
 // ROS package
 #include "lidar_cluster/marker.hpp"
 
+#include "cluster_outline.hpp"
+
 using namespace std::chrono_literals;
 using std::placeholders::_1;
 
@@ -260,6 +262,20 @@ class EuclideanSpatial : public rclcpp::Node
       mark_array.markers.push_back(center_marker);     
     }
 
+    int num_of_clusters = clusters.size();
+
+    // 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);
+    }
+
+    cluster_outline.computeOutline(cloud_cluster, mark_array, 20, max_clust_reached, input_msg->header.frame_id);
+
     // Convert to ROS data type
     sensor_msgs::msg::PointCloud2 output_msg;
     pcl::toROSMsg(*cloud_cluster, output_msg);
@@ -274,10 +290,12 @@ class EuclideanSpatial : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_marker_;
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_lidar_;
   OnSetParametersCallbackHandle::SharedPtr callback_handle_;
+  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_ = 0.02;
-  int min_cluster_size_ = 10;
+  int min_cluster_size_ = 10, max_clust_reached = 0;
   int max_cluster_size_ = 500;
   bool use_height_ = false;
   bool verbose1 = false, verbose2 = false;