feat(euclidean_cluster): add diagnostics warning when cluster skipped due to excessive points from large objects

Author: ogrex

perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp

@@ -13,9 +13,10 @@
 // limitations under the License.
 
 #pragma once
-
 #include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp"
 #include "autoware/euclidean_cluster/utils.hpp"
+#include <autoware_utils/ros/diagnostics_interface.hpp>
+#include <rclcpp/node.hpp>
 
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/point_types.h>
@@ -44,12 +45,20 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
   {
     min_points_number_per_voxel_ = min_points_number_per_voxel;
   }
-
+  void setDiagnosticsInterface(autoware_utils::DiagnosticsInterface* diag_ptr)
+  {
+    diagnostics_interface_ptr_ = diag_ptr;
+  }
 private:
   pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
   float tolerance_;
   float voxel_leaf_size_;
   int min_points_number_per_voxel_;
+
+  void publishDiagnosticsSummary(
+    const std::vector<std::string> & warnings,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg);
+    autoware_utils::DiagnosticsInterface* diagnostics_interface_ptr_{nullptr};
 };
 
 }  // namespace autoware::euclidean_cluster

perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"
+#include <rclcpp/node.hpp>
 
 #include <pcl/kdtree/kdtree.h>
 #include <pcl/segmentation/extract_clusters.h>
@@ -41,6 +42,35 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster(
   min_points_number_per_voxel_(min_points_number_per_voxel)
 {
 }
+
+// After processing all clusters, publish a summary of diagnostics.
+void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
+  const std::vector<std::string> & warnings,
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg)
+{
+  if (!diagnostics_interface_ptr_) {
+    return;
+  }
+  diagnostics_interface_ptr_->clear();
+
+  std::stringstream summary;
+  if (!warnings.empty()) {
+    summary << warnings.size() << " clusters skipped because cluster size exceeds the maximum allowed "
+                    << max_cluster_size_  << " \n";
+    for (const auto & warn : warnings) {
+      summary << " - " << warn << "\n";
+    }
+    diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", false);
+  } else {
+    diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", true);
+  }
+  diagnostics_interface_ptr_->update_level_and_message(
+    warnings.empty() ? diagnostic_msgs::msg::DiagnosticStatus::OK : diagnostic_msgs::msg::DiagnosticStatus::WARN,
+    summary.str());
+  diagnostics_interface_ptr_->publish(pointcloud_msg->header.stamp);
+}
+
+
 // TODO(badai-nguyen): remove this function when field copying also implemented for
 // euclidean_cluster.cpp
 bool VoxelGridBasedEuclideanCluster::cluster(
@@ -136,12 +166,25 @@ bool VoxelGridBasedEuclideanCluster::cluster(
 
   // build output and check cluster size
   {
+    // At the start, create a container to collect warnings.
+    std::vector<std::string> warning_messages;  
     for (size_t i = 0; i < temporary_clusters.size(); ++i) {
       auto & i_cluster_data_size = clusters_data_size.at(i);
-      if (!(min_cluster_size_ <= static_cast<int>(i_cluster_data_size / point_step) &&
-            static_cast<int>(i_cluster_data_size / point_step) <= max_cluster_size_)) {
+      int cluster_size = static_cast<int>(i_cluster_data_size / point_step);
+      if (cluster_size < min_cluster_size_) {
+        // Cluster size is below the minimum threshold; skip without messaging.
+        continue;
+      }
+      if (cluster_size > max_cluster_size_ /10) {
+        // Cluster size exceeds the maximum threshold; log a warning.
+        warning_messages.push_back(" Cluster " + std::to_string(i) + " (" + std::to_string(cluster_size) + ").");
         continue;
       }
+
+      // if (!(min_cluster_size_ <= static_cast<int>(i_cluster_data_size / point_step) &&
+      //       static_cast<int>(i_cluster_data_size / point_step) <= max_cluster_size_)) { 
+      //   continue;
+      // }
       const auto & cluster = temporary_clusters.at(i);
       tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
       feature_object.feature.cluster = cluster;
@@ -164,6 +207,9 @@ bool VoxelGridBasedEuclideanCluster::cluster(
       objects.feature_objects.push_back(feature_object);
     }
     objects.header = pointcloud_msg->header;
+
+    publishDiagnosticsSummary(warning_messages,  pointcloud_msg);
+
   }
 
   return true;

perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp

@@ -31,10 +31,14 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode(
   const float tolerance = this->declare_parameter("tolerance", 1.0);
   const float voxel_leaf_size = this->declare_parameter("voxel_leaf_size", 0.5);
   const int min_points_number_per_voxel = this->declare_parameter("min_points_number_per_voxel", 3);
+
   cluster_ = std::make_shared<VoxelGridBasedEuclideanCluster>(
     use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size,
     min_points_number_per_voxel);
-
+  // Pass the diagnostics interface pointer from the node to the cluster
+  diagnostics_interface_ptr_ = std::make_unique<autoware_utils::DiagnosticsInterface>(this, "euclidean_cluster");
+  cluster_->setDiagnosticsInterface(diagnostics_interface_ptr_.get());
+  
   using std::placeholders::_1;
   pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "input", rclcpp::SensorDataQoS().keep_last(1),
@@ -48,6 +52,8 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode(
     std::make_unique<autoware_utils::DebugPublisher>(this, "voxel_grid_based_euclidean_cluster");
   stop_watch_ptr_->tic("cyclic_time");
   stop_watch_ptr_->tic("processing_time");
+  
+
 }
 
 void VoxelGridBasedEuclideanClusterNode::onPointCloud(

perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp

@@ -18,7 +18,7 @@
 
 #include <autoware_utils/ros/debug_publisher.hpp>
 #include <autoware_utils/system/stop_watch.hpp>
-#include <rclcpp/rclcpp.hpp>
+#include <autoware_utils/ros/diagnostics_interface.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -43,6 +43,9 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node
   std::shared_ptr<VoxelGridBasedEuclideanCluster> cluster_;
   std::unique_ptr<autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
   std::unique_ptr<autoware_utils::DebugPublisher> debug_publisher_;
+
+  std::unique_ptr<autoware_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
+
 };
 
 }  // namespace autoware::euclidean_cluster