feat(map_based_prediction): add diagnostic handler to warn the processing time excess

perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml

@@ -55,3 +55,4 @@
     publish_processing_time: false
     publish_processing_time_detail: false
     publish_debug_markers: false
+    processing_time_tolerance_ms: 500.0 #[ms]

perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -21,6 +21,7 @@
 
 #include <autoware_utils/geometry/geometry.hpp>
 #include <autoware_utils/ros/debug_publisher.hpp>
+#include <autoware_utils/ros/diagnostics_interface.hpp>
 #include <autoware_utils/ros/polling_subscriber.hpp>
 #include <autoware_utils/ros/published_time_publisher.hpp>
 #include <autoware_utils/ros/transform_listener.hpp>
@@ -115,6 +116,10 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Predictor
   std::shared_ptr<PredictorVru> predictor_vru_;
 
+  // Diagnostics
+  std::unique_ptr<autoware_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
+  double processing_time_tolerance_ms_;
+
   ////// Parameters
 
   //// Vehicle Parameters

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

@@ -30,6 +30,7 @@
 #include <autoware_utils/ros/uuid_helper.hpp>
 
 #include <autoware_perception_msgs/msg/detected_objects.hpp>
+#include <diagnostic_msgs/msg/diagnostic_status.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <boost/geometry.hpp>
@@ -50,6 +51,7 @@
 #include <functional>
 #include <limits>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <utility>
 #include <vector>
@@ -479,26 +481,35 @@
   // publishers
   pub_objects_ = this->create_publisher<PredictedObjects>("~/output/objects", rclcpp::QoS{1});
 
+  // stopwatch
+  stop_watch_ptr_ = std::make_unique<autoware_utils::StopWatch<std::chrono::milliseconds>>();
+  stop_watch_ptr_->tic("cyclic_time");
+  stop_watch_ptr_->tic("processing_time");
+
+  // diagnostics
+  diagnostics_interface_ptr_ =
+    std::make_unique<autoware_utils::DiagnosticsInterface>(this, "map_based_prediction");
+  processing_time_tolerance_ms_ = declare_parameter<double>("processing_time_tolerance_ms");
+
   // debug publishers
   if (use_time_publisher) {
     processing_time_publisher_ =
       std::make_unique<autoware_utils::DebugPublisher>(this, "map_based_prediction");
     published_time_publisher_ = std::make_unique<autoware_utils::PublishedTimePublisher>(this);
-    stop_watch_ptr_ = std::make_unique<autoware_utils::StopWatch<std::chrono::milliseconds>>();
-    stop_watch_ptr_->tic("cyclic_time");
-    stop_watch_ptr_->tic("processing_time");
   }
 
+  // debug time keeper
   if (use_time_keeper) {
     detailed_processing_time_publisher_ =
       this->create_publisher<autoware_utils::ProcessingTimeDetail>(
         "~/debug/processing_time_detail_ms", 1);
     auto time_keeper = autoware_utils::TimeKeeper(detailed_processing_time_publisher_);
     time_keeper_ = std::make_shared<autoware_utils::TimeKeeper>(time_keeper);
     path_generator_->setTimeKeeper(time_keeper_);
     predictor_vru_->setTimeKeeper(time_keeper_);
   }
 
+  // debug marker
   if (use_debug_marker) {
     pub_debug_markers_ =
       this->create_publisher<visualization_msgs::msg::MarkerArray>("maneuver", rclcpp::QoS{1});
@@ -555,118 +566,135 @@
   std::unique_ptr<ScopedTimeTrack> st_ptr;
   if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
 
-  if (stop_watch_ptr_) stop_watch_ptr_->toc("processing_time", true);
+  stop_watch_ptr_->toc("processing_time", true);
 
   // take traffic_signal
   {
     const auto msg = sub_traffic_signals_.take_data();
     if (msg) {
       trafficSignalsCallback(msg);
     }
   }
 
   // Guard for map pointer and frame transformation
   if (!lanelet_map_ptr_) {
     return;
   }
 
   // get world to map transform
   geometry_msgs::msg::TransformStamped::ConstSharedPtr world2map_transform;
   bool is_object_not_in_map_frame = in_objects->header.frame_id != "map";
   if (is_object_not_in_map_frame) {
     world2map_transform = transform_listener_.get_transform(
       "map",                        // target
       in_objects->header.frame_id,  // src
       in_objects->header.stamp, rclcpp::Duration::from_seconds(0.1));
     if (!world2map_transform) return;
   }
 
   // Get objects detected time
   const double objects_detected_time = rclcpp::Time(in_objects->header.stamp).seconds();
 
   // Remove old objects information in object history
   // road users
   utils::removeOldObjectsHistory(
     objects_detected_time, object_buffer_time_length_, road_users_history_);
   // crosswalk users
   predictor_vru_->removeOldKnownMatches(objects_detected_time, object_buffer_time_length_);
 
   // result output
   PredictedObjects output;
   output.header = in_objects->header;
   output.header.frame_id = "map";
 
   // result debug
   visualization_msgs::msg::MarkerArray debug_markers;
 
   // get current crosswalk users for later prediction
   predictor_vru_->loadCurrentCrosswalkUsers(*in_objects);
 
   // for each object
   for (const auto & object : in_objects->objects) {
     TrackedObject transformed_object = object;
 
     // transform object frame if it's based on map frame
     if (is_object_not_in_map_frame) {
       geometry_msgs::msg::PoseStamped pose_in_map;
       geometry_msgs::msg::PoseStamped pose_orig;
       pose_orig.pose = object.kinematics.pose_with_covariance.pose;
       tf2::doTransform(pose_orig, pose_in_map, *world2map_transform);
       transformed_object.kinematics.pose_with_covariance.pose = pose_in_map.pose;
     }
 
     // get tracking label and update it for the prediction
     const auto & label_ = transformed_object.classification.front().label;
     const auto label = utils::changeLabelForPrediction(label_, object, lanelet_map_ptr_);
 
     switch (label) {
       case ObjectClassification::PEDESTRIAN:
       case ObjectClassification::BICYCLE: {
         // Run pedestrian/bicycle prediction
         const auto predicted_vru =
           getPredictionForNonVehicleObject(output.header, transformed_object);
         output.objects.emplace_back(predicted_vru);
         break;
       }
       case ObjectClassification::CAR:
       case ObjectClassification::BUS:
       case ObjectClassification::TRAILER:
       case ObjectClassification::MOTORCYCLE:
       case ObjectClassification::TRUCK: {
         const auto predicted_object_opt = getPredictionForVehicleObject(
           output.header, transformed_object, objects_detected_time, debug_markers);
         if (predicted_object_opt) {
           output.objects.push_back(predicted_object_opt.value());
         }
         break;
       }
       default: {
         auto predicted_unknown_object = utils::convertToPredictedObject(transformed_object);
         PredictedPath predicted_path = path_generator_->generatePathForNonVehicleObject(
           transformed_object, prediction_time_horizon_.unknown);
         predicted_path.confidence = 1.0;
 
         predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path);
         output.objects.push_back(predicted_unknown_object);
         break;
       }
     }
   }
 
   // process lost crosswalk users to tackle unstable detection
   if (remember_lost_crosswalk_users_) {
     PredictedObjects retrieved_objects = predictor_vru_->retrieveUndetectedObjects();
     output.objects.insert(
       output.objects.end(), retrieved_objects.objects.begin(), retrieved_objects.objects.end());
   }
 
   // Publish Results
   publish(output, debug_markers);
 
+  // Processing time
+  const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+  const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+
+  // Diagnostics
+  diagnostics_interface_ptr_->clear();
+  diagnostics_interface_ptr_->add_key_value("processing_time_ms", processing_time_ms);
+  bool is_processing_time_exceeds_tolerance = processing_time_ms > processing_time_tolerance_ms_;
+  diagnostics_interface_ptr_->add_key_value(
+    "is_processing_time_exceeds_tolerance", is_processing_time_exceeds_tolerance);
+  if (is_processing_time_exceeds_tolerance) {
+    std::ostringstream oss;
+    oss << "Processing time exceeded " << processing_time_tolerance_ms_ << "[ms] < "
+        << processing_time_ms << "[ms]";
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, oss.str());
+  }
+  diagnostics_interface_ptr_->publish(output.header.stamp);
+
   // Publish Processing Time
-  if (stop_watch_ptr_) {
-    const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
-    const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+  if (processing_time_publisher_) {
     processing_time_publisher_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(
       "debug/cyclic_time_ms", cyclic_time_ms);
     processing_time_publisher_->publish<autoware_internal_debug_msgs::msg::Float64Stamped>(