From 7055439d35bd670c51afb81daae6fea827d498f9 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Wed, 22 Jan 2025 21:45:06 +0900
Subject: [PATCH] feat: add debug topic

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../src/traffic_light_selector_node.cpp       | 24 +++++++++++++++++++
 .../src/traffic_light_selector_node.hpp       |  7 ++++++
 2 files changed, 31 insertions(+)

diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp
index 08675a7f84031..74ddbbc64a190 100644
--- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp
+++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.cpp
@@ -34,6 +34,15 @@ TrafficLightSelectorNode::TrafficLightSelectorNode(const rclcpp::NodeOptions & n
   expected_rois_sub_(this, "input/expect_rois", rclcpp::QoS{1}.get_rmw_qos_profile()),
   sync_(SyncPolicy(10), detected_rois_sub_, rough_rois_sub_, expected_rois_sub_)
 {
+  {
+    stop_watch_ptr_ =
+      std::make_unique<autoware::universe_utils::StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ =
+      std::make_unique<autoware::universe_utils::DebugPublisher>(this, this->get_name());
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+
   max_iou_threshold_ = declare_parameter<double>("max_iou_threshold");
   using std::placeholders::_1;
   using std::placeholders::_2;
@@ -65,6 +74,7 @@ void TrafficLightSelectorNode::objectsCallback(
   const TrafficLightRoiArray::ConstSharedPtr & rough_rois_msg,
   const TrafficLightRoiArray::ConstSharedPtr & expected_rois_msg)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   if (!camera_info_subscribed_) {
     return;
   }
@@ -149,6 +159,20 @@ void TrafficLightSelectorNode::objectsCallback(
     }
   }
   pub_traffic_light_rois_->publish(output);
+  if (debug_publisher_) {
+    const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+    const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+    const double pipeline_latency_ms =
+      std::chrono::duration<double, std::milli>(
+        std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds()))
+        .count();
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/cyclic_time_ms", cyclic_time_ms);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+    debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/pipeline_latency_ms", pipeline_latency_ms);
+  }
   return;
 }
 }  // namespace autoware::traffic_light
diff --git a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp
index 4aa645c5f2d9a..9dab987b7523d 100644
--- a/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp
+++ b/perception/autoware_traffic_light_selector/src/traffic_light_selector_node.hpp
@@ -17,7 +17,9 @@
 
 #include "utils.hpp"
 
+#include <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/ros/transform_listener.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <sensor_msgs/msg/camera_info.hpp>
@@ -32,6 +34,8 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <memory>
+
 namespace autoware::traffic_light
 {
 using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
@@ -72,6 +76,9 @@ class TrafficLightSelectorNode : public rclcpp::Node
   uint32_t image_width_{1280};
   uint32_t image_height_{960};
   double max_iou_threshold_{0.0};
+
+  std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;
 };
 
 }  // namespace autoware::traffic_light