From cbaf62a36685b35110afc46097f9d5bba59f27fa Mon Sep 17 00:00:00 2001
From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com>
Date: Mon, 27 May 2024 15:39:05 +0900
Subject: [PATCH] chore(tensorrt_yolox): add processing time and cyclic time
 debug topic (#7126)

fix: add processing time topic

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../tensorrt_yolox/tensorrt_yolox_node.hpp    |  4 +++
 .../src/tensorrt_yolox_node.cpp               | 25 +++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp
index 9b440f1dbdfec..fc396ae4b3473 100644
--- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp
+++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox_node.hpp
@@ -19,6 +19,8 @@
 #include <opencv2/opencv.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tensorrt_yolox/tensorrt_yolox.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <sensor_msgs/msg/image.hpp>
 #include <std_msgs/msg/header.hpp>
@@ -61,6 +63,8 @@ class TrtYoloXNode : public rclcpp::Node
 
   LabelMap label_map_;
   std::unique_ptr<tensorrt_yolox::TrtYoloX> trt_yolox_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
 };
 
 }  // namespace tensorrt_yolox
diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp
index af435ce6efe83..93380b95680c2 100644
--- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp
+++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp
@@ -29,6 +29,14 @@ namespace tensorrt_yolox
 TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options)
 : Node("tensorrt_yolox", node_options)
 {
+  {
+    stop_watch_ptr_ =
+      std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ =
+      std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "tensorrt_yolox");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
   using std::placeholders::_1;
   using std::chrono_literals::operator""ms;
 
@@ -132,6 +140,7 @@ void TrtYoloXNode::onConnect()
 
 void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects;
 
   cv_bridge::CvImagePtr in_image_ptr;
@@ -173,6 +182,22 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg)
 
   out_objects.header = msg->header;
   objects_pub_->publish(out_objects);
+
+  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() - out_objects.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);
+  }
 }
 
 bool TrtYoloXNode::readLabelFile(const std::string & label_path)