From df782270bb844a00095e88eb40a21acebefeb897 Mon Sep 17 00:00:00 2001 From: Emerson Knapp Date: Tue, 6 Apr 2021 16:12:30 -0700 Subject: [PATCH] Add pause/resume to PlayerClock Expose it on Player class as well Signed-off-by: Emerson Knapp --- .../rosbag2_cpp/clocks/player_clock.hpp | 20 ++++++ .../clocks/time_controller_clock.hpp | 23 ++++++- .../clocks/time_controller_clock.cpp | 63 +++++++++++++++++-- 3 files changed, 99 insertions(+), 7 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp b/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp index 78f25f95c3..cb442063b0 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp @@ -64,6 +64,26 @@ class PlayerClock */ ROSBAG2_CPP_PUBLIC virtual double get_rate() const = 0; + + /** + * Stop the flow of time of the clock. + * If this changes the pause state, this will wake any waiting `sleep_until` + */ + ROSBAG2_CPP_PUBLIC + virtual void pause() = 0; + + /** + * Start the flow of time of the clock + * If this changes the pause state, this will wake any waiting `sleep_until` + */ + ROSBAG2_CPP_PUBLIC + virtual void resume() = 0; + + /** + * Return whether the clock is currently paused. + */ + ROSBAG2_CPP_PUBLIC + virtual bool is_paused() const = 0; }; } // namespace rosbag2_cpp 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 980e43545f..7d2595e645 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp @@ -46,7 +46,8 @@ class TimeControllerClock : public PlayerClock TimeControllerClock( rcutils_time_point_value_t starting_time, double rate = 1.0, - NowFunction now_fn = std::chrono::steady_clock::now); + NowFunction now_fn = std::chrono::steady_clock::now, + std::chrono::milliseconds sleep_time_while_paused = std::chrono::milliseconds{100}); ROSBAG2_CPP_PUBLIC virtual ~TimeControllerClock(); @@ -73,6 +74,26 @@ class TimeControllerClock : public PlayerClock ROSBAG2_CPP_PUBLIC double get_rate() const override; + /** + * Stop the flow of time of the clock. + * If this changes the pause state, this will wake any waiting `sleep_until` + */ + ROSBAG2_CPP_PUBLIC + void pause() override; + + /** + * Start the flow of time of the clock + * If this changes the pause state, this will wake any waiting `sleep_until` + */ + ROSBAG2_CPP_PUBLIC + void resume() override; + + /** + * Return whether the clock is currently paused. + */ + ROSBAG2_CPP_PUBLIC + bool is_paused() const override; + 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 a51eb95938..0fb3098791 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp @@ -55,8 +55,10 @@ class TimeControllerClockImpl std::chrono::steady_clock::time_point steady; }; - explicit TimeControllerClockImpl(PlayerClock::NowFunction now_fn) - : now_fn(now_fn) + explicit TimeControllerClockImpl( + PlayerClock::NowFunction now_fn, std::chrono::milliseconds sleep_time_while_paused) + : now_fn(now_fn), + sleep_time_while_paused(sleep_time_while_paused) {} virtual ~TimeControllerClockImpl() = default; @@ -81,19 +83,29 @@ class TimeControllerClockImpl return reference.steady + std::chrono::nanoseconds(diff_nanos); } + void snapshot(rcutils_time_point_value_t ros_time) + RCPPUTILS_TSA_REQUIRES(state_mutex) + { + reference.ros = ros_time; + reference.steady = now_fn(); + } + const PlayerClock::NowFunction now_fn; + const std::chrono::milliseconds sleep_time_while_paused; std::mutex state_mutex; 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; TimeReference reference RCPPUTILS_TSA_GUARDED_BY(state_mutex); }; TimeControllerClock::TimeControllerClock( rcutils_time_point_value_t starting_time, double rate, - NowFunction now_fn) -: impl_(std::make_unique(now_fn)) + NowFunction now_fn, + std::chrono::milliseconds sleep_time_while_paused) +: impl_(std::make_unique(now_fn, sleep_time_while_paused)) { std::lock_guard lock(impl_->state_mutex); impl_->reference.ros = starting_time; @@ -114,8 +126,12 @@ bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until) { { TSAUniqueLock lock(impl_->state_mutex); - const auto steady_until = impl_->ros_to_steady(until); - impl_->cv.wait_until(lock, steady_until); + if (impl_->paused) { + impl_->cv.wait_for(lock, impl_->sleep_time_while_paused); + } else { + const auto steady_until = impl_->ros_to_steady(until); + impl_->cv.wait_until(lock, steady_until); + } } return now() >= until; } @@ -126,4 +142,39 @@ double TimeControllerClock::get_rate() const return impl_->rate; } +void TimeControllerClock::pause() +{ + { + std::lock_guard lock(impl_->state_mutex); + if (impl_->paused) { + return; + } + // Note: needs to not be paused when taking snapshot, otherwise it will use last ros ref + impl_->snapshot(now()); + impl_->paused = true; + } + impl_->cv.notify_all(); +} + +void TimeControllerClock::resume() +{ + { + std::lock_guard lock(impl_->state_mutex); + if (!impl_->paused) { + return; + } + // Note: needs to not be paused when taking snapshot, otherwise it will use last ros ref + impl_->paused = false; + impl_->snapshot(now()); + } + impl_->cv.notify_all(); +} + +bool TimeControllerClock::is_paused() const +{ + std::lock_guard lock(impl_->state_mutex); + return impl_->paused; +} + + } // namespace rosbag2_cpp