feat: add processing time publisher for tracker merger

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp

@@ -20,6 +20,8 @@
 #include "tracking_object_merger/utils/utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
 #include <std_msgs/msg/header.hpp>
@@ -87,6 +89,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
     debug_object_pub_;
   bool publish_interpolated_sub_objects_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
 
   /* handle objects */
   std::unordered_map<MEASUREMENT_STATE, std::function<void(TrackedObject &, const TrackedObject &)>>

perception/tracking_object_merger/src/decorative_tracker_merger.cpp

@@ -151,6 +151,13 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio
   set3dDataAssociation("lidar-radar", data_association_map_);
   // radar-radar association matrix
   set3dDataAssociation("radar-radar", data_association_map_);
+
+  // debug publisher
+  processing_time_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "decorative_object_merger_node");
+  stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+  stop_watch_ptr_->tic("cyclic_time");
+  stop_watch_ptr_->tic("processing_time");
 }
 
 void DecorativeTrackerMergerNode::set3dDataAssociation(
@@ -182,6 +189,7 @@ void DecorativeTrackerMergerNode::set3dDataAssociation(
 void DecorativeTrackerMergerNode::mainObjectsCallback(
   const TrackedObjects::ConstSharedPtr & main_objects)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   // try to merge sub object
   if (!sub_objects_buffer_.empty()) {
     // get interpolated sub objects
@@ -214,6 +222,10 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
   this->decorativeMerger(main_sensor_type_, main_objects);
 
   merged_object_pub_->publish(getTrackedObjects(main_objects->header));
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
 }
 
 /**