add diagnostics for processing time

Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>

Author: a-maumau

perception/autoware_lidar_centerpoint/config/centerpoint_diagnostics.param.yaml

@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    # When the processing time exceeds the max_allowed_processing_time,
+    # a warning will be triggered.
+    max_allowed_processing_time: 200.0 # in milliseconds
+    # If the warning is triggered more than max_consecutive_warn_count times in a row,
+    # an error diagnostic message will be sent.
+    max_consecutive_warn_count: 10

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

@@ -55,6 +55,9 @@ class LidarCenterPointNode : public rclcpp::Node
   std::vector<std::string> class_names_;
   bool has_variance_{false};
   bool has_twist_{false};
+  float max_allowed_processing_time_;
+  int max_consecutive_warn_count_;
+  int consecutive_delay_count_=0;
 
   NonMaximumSuppression iou_bev_nms_;
   DetectionClassRemapper detection_class_remapper_;

perception/autoware_lidar_centerpoint/launch/lidar_centerpoint.launch.xml

@@ -8,6 +8,7 @@
   <arg name="model_param_path" default="$(find-pkg-share autoware_lidar_centerpoint)/config/$(var model_name).param.yaml"/>
   <arg name="ml_package_param_path" default="$(var model_path)/$(var model_name)_ml_package.param.yaml"/>
   <arg name="class_remapper_param_path" default="$(var model_path)/detection_class_remapper.param.yaml"/>
+  <arg name="diagnostics_param_path" default="$(find-pkg-share autoware_lidar_centerpoint)/config/centerpoint_diagnostics.param.yaml"/>
   <arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>
 
   <arg name="use_pointcloud_container" default="false" description="use pointcloud_container"/>
@@ -21,6 +22,7 @@
         <param from="$(var model_param_path)" allow_substs="true"/>
         <param from="$(var ml_package_param_path)" allow_substs="true"/>
         <param from="$(var class_remapper_param_path)"/>
+        <param from="$(var diagnostics_param_path)"/>
 
         <!-- This parameter shall NOT be included in param file. See also: https://github.com/autowarefoundation/autoware.universe/pull/6167 -->
         <param name="build_only" value="$(var build_only)"/>
@@ -34,6 +36,7 @@
       <param from="$(var model_param_path)" allow_substs="true"/>
       <param from="$(var ml_package_param_path)" allow_substs="true"/>
       <param from="$(var class_remapper_param_path)"/>
+      <param from="$(var diagnostics_param_path)"/>
 
       <!-- This parameter shall NOT be included in param file. See also: https://github.com/autowarefoundation/autoware.universe/pull/6167 -->
       <param name="build_only" value="$(var build_only)"/>

perception/autoware_lidar_centerpoint/src/node.cpp

@@ -110,6 +110,10 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
   diagnostics_interface_ptr_ =
     std::make_unique<autoware_utils::DiagnosticsInterface>(this, "centerpoint_trt");
 
+  // diagnostics parameters
+  max_allowed_processing_time_ = declare_parameter<float>("max_allowed_processing_time");
+  max_consecutive_warn_count_ = declare_parameter<int>("max_consecutive_warn_count");
+
   pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1),
     std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1));
@@ -182,24 +186,52 @@ 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_) {
-    const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+  if (stop_watch_ptr_) {
     const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
-    const double pipeline_latency_ms =
-      std::chrono::duration<double, std::milli>(
-        std::chrono::nanoseconds(
-          (this->get_clock()->now() - output_msg.header.stamp).nanoseconds()))
-        .count();
-    debug_publisher_ptr_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
-      "debug/cyclic_time_ms", cyclic_time_ms);
-    debug_publisher_ptr_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
-      "debug/processing_time_ms", processing_time_ms);
-    debug_publisher_ptr_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
-      "debug/pipeline_latency_ms", pipeline_latency_ms);
+
+    // check processing time is acceptable
+    if (processing_time_ms > max_allowed_processing_time_) {
+      diagnostics_interface_ptr_->add_key_value("is_processing_time_ms_in_expected_range", false);
+
+      consecutive_delay_count_++;
+
+      if(consecutive_delay_count_ <= max_consecutive_warn_count_){
+        std::stringstream message;
+        message << "CenterPoint processing time exceeds the acceptable limit of "
+          << max_allowed_processing_time_ << " ms by "
+          << (processing_time_ms - max_allowed_processing_time_) << " ms.";
+
+        diagnostics_interface_ptr_->update_level_and_message(
+          diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+      } else {
+        diagnostics_interface_ptr_->update_level_and_message(
+          diagnostic_msgs::msg::DiagnosticStatus::ERROR,
+          "CenterPoint processing delay has exceeded the acceptable limit continuously.");
+      }
+    } else {
+      diagnostics_interface_ptr_->add_key_value("is_processing_time_ms_in_expected_range", true);
+      consecutive_delay_count_ = 0;
+    }
+
+    // add processing time for debug
+    if (debug_publisher_ptr_) {
+      const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+      const double pipeline_latency_ms =
+        std::chrono::duration<double, std::milli>(
+          std::chrono::nanoseconds(
+            (this->get_clock()->now() - output_msg.header.stamp).nanoseconds()))
+          .count();
+      debug_publisher_ptr_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
+        "debug/cyclic_time_ms", cyclic_time_ms);
+      debug_publisher_ptr_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
+        "debug/processing_time_ms", processing_time_ms);
+      debug_publisher_ptr_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
+        "debug/pipeline_latency_ms", pipeline_latency_ms);
+    }
   }
+
+  diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp);
 }
 
 }  // namespace autoware::lidar_centerpoint