From 6f0b47bfa5b22c2a04481700b1fe82fbcda10f12 Mon Sep 17 00:00:00 2001
From: kminoda <koji.minoda@tier4.jp>
Date: Fri, 27 Dec 2024 11:45:32 +0900
Subject: [PATCH] update

Signed-off-by: kminoda <koji.minoda@tier4.jp>
---
 .../pointpainting_fusion/node.hpp                |  2 +-
 .../src/pointpainting_fusion/node.cpp            | 16 ++++++++++++++--
 .../include/autoware/lidar_centerpoint/node.hpp  |  2 +-
 .../autoware_lidar_centerpoint/src/node.cpp      |  2 +-
 4 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp
index 4dd7f858c452e..5bc428a8e7ab8 100644
--- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp
+++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp
@@ -59,7 +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::DiagnosticInterface> diagnostics_interface_ptr_;
+  std::unique_ptr<autoware::universe_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
 
   int omp_num_threads_{1};
   float score_threshold_{0.0};
diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
index 55c3012f75203..396fd388c4c18 100644
--- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
+++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp
@@ -187,7 +187,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
   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::DiagnosticInterface>(this, "pointpainting_trt");
+    std::make_unique<autoware::universe_utils::DiagnosticsInterface>(this, "pointpainting_trt");
 
   obj_pub_ptr_ = this->create_publisher<DetectedObjects>("~/output/objects", rclcpp::QoS{1});
 
@@ -385,6 +385,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();
@@ -399,10 +400,20 @@ 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 << "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());
@@ -421,6 +432,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)
diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp
index 6a55e18fbb2fb..1748db0e48392 100644
--- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp
+++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp
@@ -60,7 +60,7 @@ class LidarCenterPointNode : public rclcpp::Node
   DetectionClassRemapper detection_class_remapper_;
 
   std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};
-  std::unique_ptr<autoware::universe_utils::DiagnosticInterface> diagnostics_interface_ptr_;
+  std::unique_ptr<autoware::universe_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
 
   // debugger
   std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{
diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp
index 6137fa418fd3f..825fc40234120 100644
--- a/perception/autoware_lidar_centerpoint/src/node.cpp
+++ b/perception/autoware_lidar_centerpoint/src/node.cpp
@@ -108,7 +108,7 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
   detector_ptr_ =
     std::make_unique<CenterPointTRT>(encoder_param, head_param, densification_param, config);
   diagnostics_interface_ptr_ =
-    std::make_unique<autoware::universe_utils::DiagnosticInterface>(this, "centerpoint_trt");
+    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),