diff --git a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp index a0c971fa8e..692972b93a 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp @@ -136,6 +136,16 @@ class TimeControllerClock : public PlayerClock rclcpp::JumpHandler::post_callback_t post_callback, const rcl_jump_threshold_t & threshold) override; + /** + * @brief Convert an arbitrary ROSTime to a SteadyTime, based on the current reference snapshot. + * @note Backported as non-virtual to Humble to maintain ABI + * @param ros_time - time point in ROSTime + * @return time point in steady clock i.e. std::chrono::steady_clock + */ + ROSBAG2_CPP_PUBLIC + std::chrono::steady_clock::time_point + ros_to_steady(rcutils_time_point_value_t ros_time) const; + private: std::unique_ptr impl_; }; diff --git a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp index d951569751..692d9ae890 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp @@ -259,4 +259,11 @@ rclcpp::JumpHandler::SharedPtr TimeControllerClock::create_jump_callback( return nullptr; } +std::chrono::steady_clock::time_point +TimeControllerClock::ros_to_steady(rcutils_time_point_value_t ros_time) const +{ + std::lock_guard lock(impl_->state_mutex); + return impl_->ros_to_steady(ros_time); +} + } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp index f9fa002a0b..66ae791cd5 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp @@ -145,13 +145,35 @@ TEST_F(TimeControllerClockTest, is_paused) TEST_F(TimeControllerClockTest, unpaused_sleep_returns_true) { + const float error_ratio = 1.2f; + const std::chrono::nanoseconds test_timeout{RCUTILS_S_TO_NS(2)}; + const std::chrono::nanoseconds sleep_duration{RCUTILS_S_TO_NS(1)}; rosbag2_cpp::TimeControllerClock clock(ros_start_time); - clock.resume(); - EXPECT_TRUE(clock.sleep_until(clock.now() + 100)); - clock.pause(); - clock.resume(); - EXPECT_TRUE(clock.sleep_until(clock.now() + RCUTILS_S_TO_NS(1))); + // Run the whole thing twice to make sure everything also works properly after pause and resume + for (int i = 0; i < 2; i++) { + clock.resume(); + auto ros_start = clock.now(); + auto sleep_until_timestamp = ros_start + sleep_duration.count(); + auto steady_start = clock.ros_to_steady(ros_start); + bool sleep_result = false; + while (!sleep_result && (std::chrono::steady_clock::now() - steady_start) < test_timeout) { + sleep_result = clock.sleep_until(sleep_until_timestamp); + } + const auto ros_end = clock.now(); + const auto steady_end = clock.ros_to_steady(ros_end); + EXPECT_TRUE(sleep_result); + EXPECT_GE(steady_end - steady_start, sleep_duration); + EXPECT_LT(steady_end - steady_start, sleep_duration * error_ratio); + EXPECT_GE(ros_end - ros_start, sleep_duration.count()); + EXPECT_LT(ros_end - ros_start, sleep_duration.count() * error_ratio); + + clock.pause(); + // Sleep long enough between runs that ros time and steady time are diverged by an error bound + if (i == 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } + } } TEST_F(TimeControllerClockTest, paused_sleep_returns_false_quickly)