feat(lidar_centerpoint, pointpainting): add diag publisher for max voxel size

Signed-off-by: kminoda <koji.minoda@tier4.jp>

Author: kminoda

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp

@@ -24,6 +24,8 @@
 #include <autoware/lidar_centerpoint/centerpoint_trt.hpp>
 #include <autoware/lidar_centerpoint/detection_class_remapper.hpp>
 
+#include <autoware/universe_utils/ros/diagnostics_module.hpp>
+
 #include <map>
 #include <memory>
 #include <string>
@@ -44,6 +46,8 @@ inline bool isInsideBbox(
 class PointPaintingFusionNode
 : public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>
 {
+using DiagnosticsModule = autoware::universe_utils::DiagnosticsModule;
+
 public:
   explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options);
 
@@ -75,6 +79,7 @@ class PointPaintingFusionNode
   autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_;
 
   std::unique_ptr<image_projection_based_fusion::PointPaintingTRT> detector_ptr_{nullptr};
+  std::unique_ptr<DiagnosticsModule> diagnostics_module_ptr_;
 
   bool out_of_scope(const DetectedObjects & obj) override;
 };

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -193,6 +193,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
   // create detector
   detector_ptr_ = std::make_unique<image_projection_based_fusion::PointPaintingTRT>(
     encoder_param, head_param, densification_param, config);
+  diagnostics_module_ptr_ = std::make_unique<DiagnosticsModule>(this, "pointpainting_trt");
 
   obj_pub_ptr_ = this->create_publisher<DetectedObjects>("~/output/objects", rclcpp::QoS{1});
 
@@ -401,17 +402,28 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte
     return;
   }
 
+  diagnostics_module_ptr_->clear();
+
   if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) {
     RCLCPP_WARN_STREAM_THROTTLE(
       this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!");
     return;
   }
 
   std::vector<autoware::lidar_centerpoint::Box3D> det_boxes3d;
-  bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d);
+  bool is_num_pillars_within_range = true;
+  bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range);
   if (!is_success) {
     return;
   }
+  diagnostics_module_ptr_->add_key_value("is_num_pillars_within_range", is_num_pillars_within_range);
+  if (!is_num_pillars_within_range) {
+    std::stringstream message;
+    message << "PointPaintingTRT::detect: The actual number of pillars exceeds its maximum value, "
+            << "which may limit the detection performance.";
+    diagnostics_module_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+  }
 
   std::vector<autoware_perception_msgs::msg::DetectedObject> raw_objects;
   raw_objects.reserve(det_boxes3d.size());

perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp

@@ -21,7 +21,6 @@
 #include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp"
 #include "pcl/point_cloud.h"
 #include "pcl/point_types.h"
-
 #include "sensor_msgs/msg/point_cloud2.hpp"
 
 #include <memory>
@@ -62,7 +61,7 @@ class CenterPointTRT
 
   bool detect(
     const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
-    std::vector<Box3D> & det_boxes3d);
+    std::vector<Box3D> & det_boxes3d, bool & is_num_pillars_within_range);
 
 protected:
   void initPtr();

perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp

@@ -19,6 +19,7 @@
 #include "autoware/lidar_centerpoint/detection_class_remapper.hpp"
 #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp"
 
+#include <autoware/universe_utils/ros/diagnostics_module.hpp>
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
@@ -39,6 +40,8 @@ namespace autoware::lidar_centerpoint
 
 class LidarCenterPointNode : public rclcpp::Node
 {
+using DiagnosticsModule = autoware::universe_utils::DiagnosticsModule;
+
 public:
   explicit LidarCenterPointNode(const rclcpp::NodeOptions & node_options);
 
@@ -59,6 +62,7 @@ class LidarCenterPointNode : public rclcpp::Node
   DetectionClassRemapper detection_class_remapper_;
 
   std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};
+  std::unique_ptr<DiagnosticsModule> diagnostics_module_ptr_;
 
   // debugger
   std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{

perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp

@@ -127,8 +127,10 @@ void CenterPointTRT::initPtr()
 
 bool CenterPointTRT::detect(
   const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer,
-  std::vector<Box3D> & det_boxes3d)
+  std::vector<Box3D> & det_boxes3d, bool & is_num_pillars_within_range)
 {
+  is_num_pillars_within_range = true;
+
   CHECK_CUDA_ERROR(cudaMemsetAsync(
     encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_));
   CHECK_CUDA_ERROR(
@@ -155,6 +157,7 @@ bool CenterPointTRT::detect(
       "The actual number of pillars (%u) exceeds its maximum value (%zu). "
       "Please considering increasing it since it may limit the detection performance.",
       num_pillars, config_.max_voxel_size_);
+    is_num_pillars_within_range = false;
   }
 
   return true;

perception/autoware_lidar_centerpoint/src/node.cpp

@@ -107,6 +107,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_);
   detector_ptr_ =
     std::make_unique<CenterPointTRT>(encoder_param, head_param, densification_param, config);
+  diagnostics_module_ptr_ = std::make_unique<DiagnosticsModule>(this, "centerpoint_trt");
 
   pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1),
@@ -145,11 +146,22 @@ void LidarCenterPointNode::pointCloudCallback(
     stop_watch_ptr_->toc("processing_time", true);
   }
 
+  diagnostics_module_ptr_->clear();
+
   std::vector<Box3D> det_boxes3d;
-  bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d);
+  bool is_num_pillars_within_range = true;
+  bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range);
   if (!is_success) {
     return;
   }
+  diagnostics_module_ptr_->add_key_value("is_num_pillars_within_range", is_num_pillars_within_range);
+  if (!is_num_pillars_within_range) {
+    std::stringstream message;
+    message << "CenterPointTRT::detect: The actual number of pillars exceeds its maximum value, "
+            << "which may limit the detection performance.";
+    diagnostics_module_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+  }
 
   std::vector<autoware_perception_msgs::msg::DetectedObject> raw_objects;
   raw_objects.reserve(det_boxes3d.size());
@@ -170,6 +182,8 @@ void LidarCenterPointNode::pointCloudCallback(
     published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp);
   }
 
+  diagnostics_module_ptr_->publish(input_pointcloud_msg->header.stamp);
+
   // add processing time for debug
   if (debug_publisher_ptr_ && stop_watch_ptr_) {
     const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);