feat(multi_object_tracker): add diagnostics warning when extrapolation time exceeds limit with latency guarantee enabled

Author: lei.gu

perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp

@@ -189,8 +189,12 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   }
 
   // Debugger
-  debugger_ = std::make_unique<TrackerDebugger>(*this, world_frame_id_, input_channels_config_);
+  debugger_ = std::make_unique<TrackerDebugger>(*this, world_frame_id_,input_channels_config_);
+
   published_time_publisher_ = std::make_unique<autoware_utils::PublishedTimePublisher>(this);
+  // Diagnostics
+  diagnostics_interface_ptr_ =
+  std::make_unique<autoware_utils::DiagnosticsInterface>(this, "multi_object_tracker");
 }
 
 void MultiObjectTracker::onTrigger()
@@ -238,6 +242,17 @@ void MultiObjectTracker::onTimer()
   const double maximum_publish_interval = publisher_period_ * maximum_publish_interval_ratio;
   should_publish = should_publish || elapsed_time > maximum_publish_interval;
 
+  diagnostics_interface_ptr_->clear();
+  diagnostics_interface_ptr_->add_key_value("elapsed_time", elapsed_time);
+  if (elapsed_time > 500e-3) {
+    std::stringstream message;
+    message << "MultiObjectTracker::onTimer: elapsed_time exceeds its maximum value, "
+            << "which may limit the predition performance.";
+    diagnostics_interface_ptr_->update_level_and_message(
+      diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str());
+  }
+  diagnostics_interface_ptr_->publish(current_time);
+
   // Publish with delay compensation to the current time
   if (should_publish) checkAndPublish(current_time);
 }

perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp

@@ -22,6 +22,7 @@
 #include "autoware/multi_object_tracker/object_model/types.hpp"
 #include "autoware/multi_object_tracker/odometry.hpp"
 #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp"
+
 #include "debugger/debugger.hpp"
 #include "processor/input_manager.hpp"
 #include "processor/processor.hpp"
@@ -30,6 +31,7 @@
 
 #include "autoware_perception_msgs/msg/detected_objects.hpp"
 #include "autoware_perception_msgs/msg/tracked_objects.hpp"
+#include <autoware_utils/ros/diagnostics_interface.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
 #include <tf2/LinearMath/Transform.h>
@@ -68,6 +70,8 @@ class MultiObjectTracker : public rclcpp::Node
   // debugger
   std::unique_ptr<TrackerDebugger> debugger_;
   std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_;
+  // diagnostics
+  std::unique_ptr<autoware_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
 
   // publish timer
   rclcpp::TimerBase::SharedPtr publish_timer_;