diff --git a/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp b/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp index 5ee2deeb5..b7554f061 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp @@ -77,6 +77,11 @@ class PlayerClock ROSBAG2_CPP_PUBLIC virtual bool sleep_until(rclcpp::Time time) = 0; + /** + * \return whether the clock is currently sleeping. + */ + virtual bool is_sleeping() = 0; + /** * \brief Wake up the clock if it is sleeping. * \note This will wake any waiting `sleep_until`. 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 8047d5f4a..0c68b5f65 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp @@ -81,6 +81,9 @@ class TimeControllerClock : public PlayerClock ROSBAG2_CPP_PUBLIC bool sleep_until(rclcpp::Time until) override; + ROSBAG2_CPP_PUBLIC + bool is_sleeping() override; + ROSBAG2_CPP_PUBLIC void wakeup() override; 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 3913b93aa..a936acb2a 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp @@ -123,6 +123,7 @@ class TimeControllerClockImpl std::condition_variable cv RCPPUTILS_TSA_GUARDED_BY(state_mutex); double rate RCPPUTILS_TSA_GUARDED_BY(state_mutex) = 1.0; bool paused RCPPUTILS_TSA_GUARDED_BY(state_mutex) = false; + bool is_sleeping RCPPUTILS_TSA_GUARDED_BY(state_mutex) = false; TimeReference reference RCPPUTILS_TSA_GUARDED_BY(state_mutex); }; @@ -165,12 +166,16 @@ bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until) { rcpputils::unique_lock lock(impl_->state_mutex); if (impl_->paused) { + impl_->is_sleeping = true; impl_->cv.wait_for(lock, impl_->sleep_time_while_paused); + impl_->is_sleeping = false; } else { const auto steady_until = impl_->ros_to_steady(until); // wait only if necessary for performance if (steady_until > impl_->now_fn()) { + impl_->is_sleeping = true; impl_->cv.wait_until(lock, steady_until); + impl_->is_sleeping = false; } } if (impl_->paused) { @@ -187,6 +192,12 @@ bool TimeControllerClock::sleep_until(rclcpp::Time until) return sleep_until(until.nanoseconds()); } +bool TimeControllerClock::is_sleeping() +{ + std::lock_guard lock(impl_->state_mutex); + return impl_->is_sleeping; +} + void TimeControllerClock::wakeup() { impl_->cv.notify_all(); 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 1943eb0e9..34bb4aac9 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp @@ -217,6 +217,7 @@ TEST_F(TimeControllerClockTest, wakeup_sleep_returns_immediately) auto sleep_time_while_paused = std::chrono::seconds(30); rosbag2_cpp::TimeControllerClock clock(ros_start_time, now_fn, sleep_time_while_paused); clock.pause(); + EXPECT_FALSE(clock.is_sleeping()); auto steady_start = std::chrono::steady_clock::now(); std::atomic_bool thread_sleep_result{true}; auto sleep_long_thread = std::thread( @@ -224,10 +225,13 @@ TEST_F(TimeControllerClockTest, wakeup_sleep_returns_immediately) bool sleep_result = clock.sleep_until(clock.now() + RCUTILS_S_TO_NS(10)); thread_sleep_result.store(sleep_result); }); - // Wait enough time for the thread to start sleeping before we try to wake it up - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Wait for the thread to be sleeping for the purposes of this test + while (!clock.is_sleeping()) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } clock.wakeup(); sleep_long_thread.join(); + EXPECT_FALSE(clock.is_sleeping()); // Check that the clock slept less than the sleep_time_while_paused value EXPECT_LT(std::chrono::steady_clock::now() - steady_start, sleep_time_while_paused); EXPECT_FALSE(thread_sleep_result);