wip

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

Author: a-maumau

perception/autoware_lidar_centerpoint/config/centerpoint_diagnostics.param.yaml

@@ -5,4 +5,4 @@
     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
+    max_acceptable_consecutive_delay_ms: 1000.0

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

@@ -24,6 +24,7 @@
 #include <autoware_utils/ros/published_time_publisher.hpp>
 #include <autoware_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/header.hpp>
 
 #include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
@@ -55,15 +56,18 @@ class LidarCenterPointNode : public rclcpp::Node
   std::vector<std::string> class_names_;
   bool has_variance_{false};
   bool has_twist_{false};
+
+  bool is_processing_delayed_{false};
   float max_allowed_processing_time_;
-  int max_consecutive_warn_count_;
-  int consecutive_delay_count_ = 0;
+  double max_acceptable_consecutive_delay_ms_;
+  builtin_interfaces::msg::Time started_delayed_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_;
+  rclcpp::TimerBase::SharedPtr diagnostics_timer_;
 
   // debugger
   std::unique_ptr<autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{nullptr};

perception/autoware_lidar_centerpoint/src/node.cpp

@@ -112,7 +112,11 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
 
   // 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_acceptable_consecutive_delay_ms_ = declare_parameter<int>("max_acceptable_consecutive_delay_ms");
+
+  diagnostics_timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(100),
+      std::bind(&LidarCenterPointNode::diagnosticTimerCallback, this));
 
   pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
     "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1),
@@ -194,24 +198,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_++;
+      if (!is_processing_delayed_) {
+        started_delayed_timestamp_ = this->get_clock()->now();
+        is_processing_delayed_ = true;
+      }
 
-      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.";
+      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.");
-      }
+      diagnostics_interface_ptr_->update_level_and_message(
+        diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
     } else {
       diagnostics_interface_ptr_->add_key_value("is_processing_time_ms_in_expected_range", true);
-      consecutive_delay_count_ = 0;
+      is_processing_delayed_ = false;
     }
 
     // add processing time for debug
@@ -234,6 +235,25 @@ void LidarCenterPointNode::pointCloudCallback(
   diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp);
 }
 
+// Check the timestamp of the processing that has started consecutively delaying,
+// and if the node is delayed constantly, publish the error diagnostic message
+void LidarCenterPointNode::diagnosticTimerCallback() {
+  const double delayed_state_duration =
+        std::chrono::duration<double, std::milli>(
+          std::chrono::nanoseconds(
+            (this->get_clock()->now() - started_delayed_timestamp_).nanoseconds()))
+          .count();
+
+  if (delayed_state_duration > max_acceptable_consecutive_delay_ms_) {
+    diagnostics_interface_ptr_->add_key_value("is_processing_time_ms_in_expected_range", false);
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::ERROR,
+      "CenterPoint processing delay has exceeded the acceptable limit continuously.");
+
+    diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp);
+  }
+}
+
 }  // namespace autoware::lidar_centerpoint
 
 #include <rclcpp_components/register_node_macro.hpp>