From 594774fc7484e97687e8546951c20b33ab68bba7 Mon Sep 17 00:00:00 2001
From: kminoda <44218668+kminoda@users.noreply.github.com>
Date: Mon, 24 Jun 2024 22:39:19 +0900
Subject: [PATCH] feat(auoware_test_utils): add jump_clock interface (#7638)

* feat(auoware_test_utils): add jump_clock interface

Signed-off-by: kminoda <koji.minoda@tier4.jp>

* add comment

Signed-off-by: kminoda <koji.minoda@tier4.jp>

---------

Signed-off-by: kminoda <koji.minoda@tier4.jp>
---
 .../autoware_test_utils.hpp                   | 20 +++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp
index d7de1397bfaac..254a2d03f71d1 100644
--- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp
+++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp
@@ -31,6 +31,7 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <rosgraph_msgs/msg/clock.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
@@ -527,6 +528,7 @@ class AutowareTestManager
   AutowareTestManager()
   {
     test_node_ = std::make_shared<rclcpp::Node>("autoware_test_manager_node");
+    pub_clock_ = test_node_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
   }
 
   template <typename MessageType>
@@ -560,10 +562,28 @@ class AutowareTestManager
     }
   }
 
+  /**
+   * @brief Publishes a ROS Clock message with the specified time.
+   *
+   * This function publishes a ROS Clock message with the specified time.
+   * Be careful when using this function, as it can affect the behavior of
+   * the system under test. Consider using ament_add_ros_isolated_gtest to
+   * isolate the system under test from the ROS clock.
+   *
+   * @param time The time to publish.
+   */
+  void jump_clock(const rclcpp::Time & time)
+  {
+    rosgraph_msgs::msg::Clock clock;
+    clock.clock = time;
+    pub_clock_->publish(clock);
+  }
+
 protected:
   // Publisher
   std::unordered_map<std::string, std::shared_ptr<rclcpp::PublisherBase>> publishers_;
   std::unordered_map<std::string, std::shared_ptr<rclcpp::SubscriptionBase>> subscribers_;
+  rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr pub_clock_;
 
   // Node
   rclcpp::Node::SharedPtr test_node_;