From 6a4d8c10dc8e0fe76b17f192e2eba86d18e340ae Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Thu, 8 Aug 2024 09:43:45 +0900
Subject: [PATCH] fix: add delay time for reschedule gpu task

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../config/yolox_s_plus_opt.param.yaml                      | 1 +
 .../autoware_tensorrt_yolox/config/yolox_tiny.param.yaml    | 1 +
 .../include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp | 1 +
 .../autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp     | 6 +++++-
 4 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/perception/autoware_tensorrt_yolox/config/yolox_s_plus_opt.param.yaml b/perception/autoware_tensorrt_yolox/config/yolox_s_plus_opt.param.yaml
index d963074e51840..a1623fa4e0fa1 100644
--- a/perception/autoware_tensorrt_yolox/config/yolox_s_plus_opt.param.yaml
+++ b/perception/autoware_tensorrt_yolox/config/yolox_s_plus_opt.param.yaml
@@ -27,6 +27,7 @@
     color_map_path: "$(var data_path)/tensorrt_yolox/semseg_color_map.csv"
     score_threshold: 0.35
     nms_threshold: 0.7
+    inference_delay_ms: 0 # Duration time for yolox inference delay in milliseconds.
 
     precision: "int8" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8].
     calibration_algorithm: "Entropy" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax].
diff --git a/perception/autoware_tensorrt_yolox/config/yolox_tiny.param.yaml b/perception/autoware_tensorrt_yolox/config/yolox_tiny.param.yaml
index e1ece63a4940f..686be1aed6308 100644
--- a/perception/autoware_tensorrt_yolox/config/yolox_tiny.param.yaml
+++ b/perception/autoware_tensorrt_yolox/config/yolox_tiny.param.yaml
@@ -27,6 +27,7 @@
     color_map_path: "$(var data_path)/tensorrt_yolox/semseg_color_map.csv"
     score_threshold: 0.35 # Objects with a score lower than this value will be ignored. This threshold will be ignored if specified model contains EfficientNMS_TRT module in it.
     nms_threshold: 0.7 # Detection results will be ignored if IoU over this value. This threshold will be ignored if specified model contains EfficientNMS_TRT module in it.
+    inference_delay_ms: 0 # Duration time for yolox inference delay in milliseconds.
 
     precision: "fp16" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8].
     calibration_algorithm: "MinMax" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax].
diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp
index 2e317139f27c0..e105ad57698ff 100644
--- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp
+++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox_node.hpp
@@ -110,6 +110,7 @@ class TrtYoloXNode : public rclcpp::Node
   RoiOverlaySemsegLabel roi_overlay_segment_labels_;
   std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
   std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;
+  int inference_delay_ms_ = 0;
 };
 
 }  // namespace autoware::tensorrt_yolox
diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp
index 7f398ca005d67..851079f5971cc 100644
--- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp
+++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp
@@ -20,8 +20,10 @@
 #include <autoware_perception_msgs/msg/object_classification.hpp>
 
 #include <algorithm>
+#include <chrono>
 #include <memory>
 #include <string>
+#include <thread>
 #include <utility>
 #include <vector>
 
@@ -80,6 +82,8 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options)
   roi_overlay_segment_labels_.PEDESTRIAN =
     declare_parameter<bool>("roi_overlay_segment_label.PEDESTRIAN");
   roi_overlay_segment_labels_.ANIMAL = declare_parameter<bool>("roi_overlay_segment_label.ANIMAL");
+  inference_delay_ms_ = declare_parameter("inference_delay_ms");
+
   replaceLabelMap();
 
   tensorrt_common::BuildConfig build_config(
@@ -146,7 +150,7 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg)
   std::vector<cv::Mat> masks = {cv::Mat(cv::Size(height, width), CV_8UC1, cv::Scalar(0))};
   std::vector<cv::Mat> color_masks = {
     cv::Mat(cv::Size(height, width), CV_8UC3, cv::Scalar(0, 0, 0))};
-
+  std::this_thread::sleep_for(std::chrono::milliseconds(inference_delay_ms_));
   if (!trt_yolox_->doInference({in_image_ptr->image}, objects, masks, color_masks)) {
     RCLCPP_WARN(this->get_logger(), "Fail to inference");
     return;