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 #include #include +#include #include #include #include @@ -527,6 +528,7 @@ class AutowareTestManager AutowareTestManager() { test_node_ = std::make_shared("autoware_test_manager_node"); + pub_clock_ = test_node_->create_publisher("/clock", 1); } template @@ -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> publishers_; std::unordered_map> subscribers_; + rclcpp::Publisher::SharedPtr pub_clock_; // Node rclcpp::Node::SharedPtr test_node_;