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,5 @@
     publish_processing_time: false
     publish_processing_time_detail: false
     publish_debug_markers: false
+    processing_time_tolerance: 0.5 #[s]
+    processing_time_consecutive_excess_tolerance: 1.0 #[s]

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,12 @@ 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_;
+  double processing_time_consecutive_excess_tolerance_ms_;
+  std::optional<rclcpp::Time> last_in_time_processing_timestamp_;
+
   ////// Parameters
 
   //// Vehicle Parameters
@@ -163,6 +170,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   void trafficSignalsCallback(const TrafficLightGroupArray::ConstSharedPtr msg);
   void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects);
 
+  // Diagnostics proccess
+  void updateDiagnostics(const rclcpp::Time & timestamp, double processing_time_ms);
+
   // Map process
   bool doesPathCrossFence(
     const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line);

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -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,8 @@
 #include <functional>
 #include <limits>
 #include <memory>
+#include <ratio>
+#include <sstream>
 #include <string>
 #include <utility>
 #include <vector>
@@ -479,16 +482,29 @@
   // 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");
+
+    // [s] -> [ms]
+    processing_time_tolerance_ms_ = declare_parameter<double>("processing_time_tolerance") * 1e3;
+    processing_time_consecutive_excess_tolerance_ms_ =
+      declare_parameter<double>("processing_time_consecutive_excess_tolerance") * 1e3;
+  }
+
   // 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>(
@@ -499,6 +515,7 @@
     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});
@@ -531,6 +548,54 @@
   return result;
 }
 
+void MapBasedPredictionNode::updateDiagnostics(
+  const rclcpp::Time & timestamp, double processing_time_ms)
+{
+  diagnostics_interface_ptr_->clear();
+  diagnostics_interface_ptr_->add_key_value("timestamp", timestamp.seconds());
+  diagnostics_interface_ptr_->add_key_value("processing_time_ms", processing_time_ms);
+  // check processing time is in time
+  bool is_processing_in_time = processing_time_ms <= processing_time_tolerance_ms_;
+  diagnostics_interface_ptr_->add_key_value("is_processing_in_time", is_processing_in_time);
+  if (!is_processing_in_time) {
+    // publish warning if the current processing time exceeded
+    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());
+  }
+
+  if (is_processing_in_time || !last_in_time_processing_timestamp_) {
+    last_in_time_processing_timestamp_ = timestamp;
+  }
+
+  // calculate consecutive excess duration
+  const double consecutive_excess_duration_ms =
+    std::chrono::duration<double, std::milli>(
+      std::chrono::nanoseconds(
+        (timestamp - last_in_time_processing_timestamp_.value()).nanoseconds()))
+      .count();
+
+  bool is_consecutive_excess_duration_ok =
+    consecutive_excess_duration_ms < processing_time_consecutive_excess_tolerance_ms_;
+  diagnostics_interface_ptr_->add_key_value(
+    "consecutive_excess_duration_ms", consecutive_excess_duration_ms);
+  diagnostics_interface_ptr_->add_key_value(
+    "is_consecutive_excess_duration_ok", is_consecutive_excess_duration_ok);
+  if (!is_consecutive_excess_duration_ok) {
+    // publish error if the processing time exceeded in a long term
+    std::ostringstream oss;
+    oss << "Processing time exceeded consecutively in a long term: "
+        << processing_time_consecutive_excess_tolerance_ms_ << "[ms] < "
+        << consecutive_excess_duration_ms << "[ms]";
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::ERROR, oss.str());
+  }
+
+  diagnostics_interface_ptr_->publish(timestamp);
+}
+
 void MapBasedPredictionNode::mapCallback(const LaneletMapBin::ConstSharedPtr msg)
 {
   RCLCPP_DEBUG(get_logger(), "[Map Based Prediction]: Start loading lanelet");
@@ -555,118 +620,123 @@
   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
+  updateDiagnostics(output.header.stamp, processing_time_ms);
+
   // 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>(