feat(euclidean_cluster): add diagnostics warning when cluster skipped

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

@@ -13,10 +13,12 @@
 // 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 +46,21 @@ 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(
+    size_t skipped_cluster_count,
+    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

@@ -14,6 +14,8 @@
 
 #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 +43,32 @@
   min_points_number_per_voxel_(min_points_number_per_voxel)
 {
 }
+
+// After processing all clusters, publish a summary of diagnostics.
+void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
+  size_t skipped_cluster_count,
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg)
+{
+  if (!diagnostics_interface_ptr_) {
+    return;
+  }
+  diagnostics_interface_ptr_->clear();
+  std::string summary;
+  if (skipped_cluster_count > 0) {
+    summary = std::to_string(skipped_cluster_count) +
+              " clusters skipped because cluster point size exceeds the maximum allowed " +
+              std::to_string(max_cluster_size_);
+    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(
+    skipped_cluster_count > 0 ? static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::WARN)
+                              : static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::OK),
+    summary);
+  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,34 +164,43 @@
 
   // build output and check cluster size
   {
+    size_t skipped_cluster_count = 0;  // Count the skipped clusters
     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_) {
+        // Cluster size exceeds the maximum threshold; log a warning.
+        skipped_cluster_count++;
         continue;
       }
       const auto & cluster = temporary_clusters.at(i);
       tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
       feature_object.feature.cluster = cluster;
       feature_object.feature.cluster.data.resize(i_cluster_data_size);
       feature_object.feature.cluster.header = pointcloud_msg->header;
       feature_object.feature.cluster.is_bigendian = pointcloud_msg->is_bigendian;
       feature_object.feature.cluster.is_dense = pointcloud_msg->is_dense;
       feature_object.feature.cluster.point_step = point_step;
       feature_object.feature.cluster.row_step = i_cluster_data_size / pointcloud_msg->height;
       feature_object.feature.cluster.width =
         i_cluster_data_size / point_step / pointcloud_msg->height;
 
       feature_object.object.kinematics.pose_with_covariance.pose.position =
         getCentroid(feature_object.feature.cluster);
       autoware_perception_msgs::msg::ObjectClassification classification;
       classification.label = autoware_perception_msgs::msg::ObjectClassification::UNKNOWN;
       classification.probability = 1.0f;
       feature_object.object.classification.emplace_back(classification);
 
       objects.feature_objects.push_back(feature_object);
     }
     objects.header = pointcloud_msg->header;
+    // Publish the diagnostics summary.
+    publishDiagnosticsSummary(skipped_cluster_count, pointcloud_msg);
   }
 
   return true;

perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp

@@ -31,9 +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>(

perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp

@@ -17,8 +17,8 @@
 #include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"
 
 #include <autoware_utils/ros/debug_publisher.hpp>
+#include <autoware_utils/ros/diagnostics_interface.hpp>
 #include <autoware_utils/system/stop_watch.hpp>
-#include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -43,6 +43,8 @@ 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