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

Author: kminoda

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

@@ -23,6 +23,7 @@
 #include <autoware/image_projection_based_fusion/utils/utils.hpp>
 #include <autoware/lidar_centerpoint/centerpoint_trt.hpp>
 #include <autoware/lidar_centerpoint/detection_class_remapper.hpp>
+#include <autoware/universe_utils/ros/diagnostics_interface.hpp>
 
 #include <map>
 #include <memory>
@@ -58,6 +59,7 @@ class PointPaintingFusionNode
   void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
 
   rclcpp::Publisher<DetectedObjects>::SharedPtr obj_pub_ptr_;
+  std::unique_ptr<autoware::universe_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
 
   int omp_num_threads_{1};
   float score_threshold_{0.0};

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -186,6 +186,8 @@ 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_interface_ptr_ =
+    std::make_unique<autoware::universe_utils::DiagnosticsInterface>(this, "pointpainting_trt");
 
   obj_pub_ptr_ = this->create_publisher<DetectedObjects>("~/output/objects", rclcpp::QoS{1});
 
@@ -389,6 +391,7 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte
 {
   std::unique_ptr<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+  diagnostics_interface_ptr_->clear();
 
   const auto objects_sub_count =
     obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count();
@@ -403,10 +406,21 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte
   }
 
   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_interface_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_interface_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());
@@ -425,6 +439,7 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte
   if (objects_sub_count > 0) {
     obj_pub_ptr_->publish(output_msg);
   }
+  diagnostics_interface_ptr_->publish(painted_pointcloud_msg.header.stamp);
 }
 
 bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj)

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

@@ -62,7 +62,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

@@ -20,6 +20,7 @@
 #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp"
 
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
+#include <autoware/universe_utils/ros/diagnostics_interface.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -59,6 +60,7 @@ class LidarCenterPointNode : public rclcpp::Node
   DetectionClassRemapper detection_class_remapper_;
 
   std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};
+  std::unique_ptr<autoware::universe_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
 
   // debugger
   std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{

perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp

@@ -19,6 +19,7 @@
 #include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp"
 
 #include <autoware/universe_utils/math/constants.hpp>
+#include <autoware/universe_utils/ros/diagnostics_interface.hpp>
 
 #include <algorithm>
 #include <cstdlib>
@@ -127,8 +128,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 +158,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,8 @@ 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_interface_ptr_ =
+    std::make_unique<autoware::universe_utils::DiagnosticsInterface>(this, "centerpoint_trt");
 
   pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1),
@@ -144,12 +146,24 @@ void LidarCenterPointNode::pointCloudCallback(
   if (stop_watch_ptr_) {
     stop_watch_ptr_->toc("processing_time", true);
   }
+  diagnostics_interface_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_interface_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_interface_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());
@@ -169,6 +183,7 @@ void LidarCenterPointNode::pointCloudCallback(
     objects_pub_->publish(output_msg);
     published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp);
   }
+  diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp);
 
   // add processing time for debug
   if (debug_publisher_ptr_ && stop_watch_ptr_) {