change to timer callback to check the error

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

Author: a-maumau

perception/autoware_lidar_centerpoint/config/centerpoint_diagnostics.param.yaml

@@ -3,6 +3,8 @@
     # 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
+    # If the warning is triggered and if a timestamp of last normal processing is more than
+    # max_acceptable_consecutive_delay_ms, an error diagnostic message will be sent.
+    max_acceptable_consecutive_delay_ms: 1000.0
+    # interval of diagnostic callback in milliseconds
+    validation_callback_interval_ms: 100 # in milliseconds, in integer

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

@@ -30,6 +30,7 @@
 #include <autoware_perception_msgs/msg/object_classification.hpp>
 #include <autoware_perception_msgs/msg/shape.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/header.hpp>
 
 #include <memory>
 #include <string>
@@ -45,6 +46,7 @@ class LidarCenterPointNode : public rclcpp::Node
 
 private:
   void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_pointcloud_msg);
+  void diagnosticsTimerCallback();
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
@@ -55,15 +57,20 @@ 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;
+
+  bool is_processing_delayed_{false};
+  double max_allowed_processing_time_;
+  double max_acceptable_consecutive_delay_ms_;
+  // set optional to avoid sending the error diagnostics before node start processing
+  std::optional<rclcpp::Time> last_normal_processing_timestamp_;
 
   NonMaximumSuppression iou_bev_nms_;
   DetectionClassRemapper detection_class_remapper_;
 
   std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};
   std::unique_ptr<autoware_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
+  std::unique_ptr<autoware_utils::DiagnosticsInterface> diagnostics_processing_delay_;
+  rclcpp::TimerBase::SharedPtr diagnostics_processing_time_timer_;
 
   // debugger
   std::unique_ptr<autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{nullptr};

perception/autoware_lidar_centerpoint/src/node.cpp

@@ -109,10 +109,15 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     std::make_unique<CenterPointTRT>(encoder_param, head_param, densification_param, config);
   diagnostics_interface_ptr_ =
     std::make_unique<autoware_utils::DiagnosticsInterface>(this, "centerpoint_trt");
+  diagnostics_processing_delay_ =
+    std::make_unique<autoware_utils::DiagnosticsInterface>(this, "node_processing_time_status");
 
   // diagnostics parameters
-  max_allowed_processing_time_ = declare_parameter<float>("max_allowed_processing_time");
-  max_consecutive_warn_count_ = declare_parameter<int>("max_consecutive_warn_count");
+  max_allowed_processing_time_ = declare_parameter<double>("max_allowed_processing_time");
+  max_acceptable_consecutive_delay_ms_ =
+    declare_parameter<double>("max_acceptable_consecutive_delay_ms");
+  const long validation_callback_interval_ms =
+    declare_parameter<long>("validation_callback_interval_ms");
 
   pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1),
@@ -130,6 +135,12 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     stop_watch_ptr_->tic("processing_time");
   }
 
+  if (stop_watch_ptr_) {
+    diagnostics_processing_time_timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(validation_callback_interval_ms),
+      std::bind(&LidarCenterPointNode::diagnosticsTimerCallback, this));
+  }
+
   if (this->declare_parameter("build_only", false)) {
     RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node.");
     rclcpp::shutdown();
@@ -194,24 +205,21 @@ void LidarCenterPointNode::pointCloudCallback(
     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_++;
+      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.";
 
-      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());
 
-        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.");
+      // just in case the processing starts with a delayed inference
+      if (!last_normal_processing_timestamp_) {
+        last_normal_processing_timestamp_ = this->get_clock()->now();
       }
     } else {
       diagnostics_interface_ptr_->add_key_value("is_processing_time_ms_in_expected_range", true);
-      consecutive_delay_count_ = 0;
+      last_normal_processing_timestamp_ = this->get_clock()->now();
     }
 
     // add processing time for debug
@@ -234,6 +242,33 @@ void LidarCenterPointNode::pointCloudCallback(
   diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp);
 }
 
+// Check the timestamp of consecutively delayed processing
+// If the node is consistently delayed, publish an error diagnostic message
+void LidarCenterPointNode::diagnosticsTimerCallback() {
+  // skip if the node has not performed inference yet
+  if (last_normal_processing_timestamp_) {
+    diagnostics_processing_delay_->clear();
+
+    const rclcpp::Time timestamp_now = this->get_clock()->now();
+    const double delayed_state_duration =
+          std::chrono::duration<double, std::milli>(
+            std::chrono::nanoseconds(
+              (timestamp_now - last_normal_processing_timestamp_.value()).nanoseconds()))
+            .count();
+
+    if (delayed_state_duration > max_acceptable_consecutive_delay_ms_) {
+      diagnostics_processing_delay_->add_key_value("is_processing_time_ms_in_expected_range", false);
+      diagnostics_processing_delay_->update_level_and_message(
+        diagnostic_msgs::msg::DiagnosticStatus::ERROR,
+        "CenterPoint processing delay has exceeded the acceptable limit continuously.");
+    } else {
+      diagnostics_processing_delay_->add_key_value("is_processing_time_ms_in_expected_range", true);
+    }
+
+    diagnostics_processing_delay_->publish(timestamp_now);
+  }
+}
+
 }  // namespace autoware::lidar_centerpoint
 
 #include <rclcpp_components/register_node_macro.hpp>