diff --git a/rosbag2_cpp/include/rosbag2_cpp/player_clock.hpp b/rosbag2_cpp/include/rosbag2_cpp/player_clock.hpp index 81a0c1343e..fcee674356 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/player_clock.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/player_clock.hpp @@ -20,6 +20,7 @@ #include #include "rcutils/time.h" +#include "rosbag2_cpp/visibility_control.hpp" namespace rosbag2_cpp { @@ -35,7 +36,7 @@ class PlayerClock { public: // Type representing the current time as according to the playback - typedef rcutils_time_point_value_t PlayerTimePoint; + typedef rcutils_time_point_value_t ROSTimePoint; /** * Type representing an arbitrary steady time, used to measure real-time durations * This type is never exposed by the PlayerClock - it is only used as input for internal @@ -55,17 +56,20 @@ class PlayerClock * Used to control for unit testing, or for specialized needs * \throws std::runtime_error if rate is <= 0 */ + ROSBAG2_CPP_PUBLIC PlayerClock( - PlayerTimePoint starting_time, + ROSTimePoint starting_time, double rate = 1.0, NowFunction now_fn = std::chrono::steady_clock::now); + ROSBAG2_CPP_PUBLIC virtual ~PlayerClock(); /** - * Calculate and return current PlayerTimePoint based on starting time, playback rate, pause state. + * Calculate and return current ROSTimePoint based on starting time, playback rate, pause state. */ - PlayerTimePoint now() const; + ROSBAG2_CPP_PUBLIC + ROSTimePoint now() const; /** * Try to sleep (non-busy) the current thread until the provided time is reached - according to this Clock @@ -74,11 +78,13 @@ class PlayerClock * for the appropriate duration. * The user should not take action based on this sleep until it returns true. */ - bool sleep_until(PlayerTimePoint until); + ROSBAG2_CPP_PUBLIC + bool sleep_until(ROSTimePoint until); /** * Return the current playback rate. */ + ROSBAG2_CPP_PUBLIC double get_rate() const; private: diff --git a/rosbag2_cpp/src/rosbag2_cpp/player_clock.cpp b/rosbag2_cpp/src/rosbag2_cpp/player_clock.cpp index cb13e451e0..b45a93bae7 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/player_clock.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/player_clock.cpp @@ -16,74 +16,77 @@ #include #include +#include "rcpputils/thread_safety_annotations.hpp" #include "rosbag2_cpp/player_clock.hpp" #include "rosbag2_cpp/types.hpp" -namespace -{ - -/** - * Stores an exact time match between a system steady clock and the semantic play clock. - * This is created whenever a factor changes such that a new base reference is needed - * such as pause, resume, rate change, or jump - */ -struct TimeSync -{ - rosbag2_cpp::PlayerClock::PlayerTimePoint player_time; - rosbag2_cpp::PlayerClock::SteadyTimePoint steady_time; -}; - -template -rcutils_duration_value_t duration_nanos(const T & duration) -{ - return std::chrono::duration_cast(duration).count(); -} - -} // namespace - namespace rosbag2_cpp { class PlayerClockImpl { + /** + * Stores an exact time match between a system steady clock and the playback ROS clock. + * This is created whenever a factor changes such that a new base reference is needed + * such as pause, resume, rate change, or jump + */ + struct TimeReference + { + PlayerClock::ROSTimePoint ros; + PlayerClock::SteadyTimePoint steady; + }; + public: PlayerClockImpl() = default; - TimeSync reference; + template + rcutils_duration_value_t duration_nanos(const T & duration) + { + return std::chrono::duration_cast(duration).count(); + } + + PlayerClock::ROSTimePoint steady_to_ros(PlayerClock::SteadyTimePoint steady_time) + { + return reference.ros + (rate * duration_nanos(steady_time - reference.steady)); + } + + PlayerClock::SteadyTimePoint ros_to_steady(PlayerClock::ROSTimePoint ros_time) + { + const rcutils_duration_value_t diff_nanos = (ros_time - reference.ros) / rate; + return reference.steady + std::chrono::nanoseconds(diff_nanos); + } + double rate = 1.0; PlayerClock::NowFunction now_fn; std::mutex mutex; + TimeReference reference RCPPUTILS_TSA_GUARDED_BY(mutex); }; -PlayerClock::PlayerClock(PlayerTimePoint starting_time, double rate, NowFunction now_fn) +PlayerClock::PlayerClock(ROSTimePoint starting_time, double rate, NowFunction now_fn) : impl_(std::make_unique()) { std::lock_guard lock(impl_->mutex); impl_->now_fn = now_fn; - impl_->reference.player_time = starting_time; - impl_->reference.steady_time = impl_->now_fn(); + impl_->reference.ros = starting_time; + impl_->reference.steady = impl_->now_fn(); impl_->rate = rate; } PlayerClock::~PlayerClock() {} -PlayerClock::PlayerTimePoint PlayerClock::now() const +PlayerClock::ROSTimePoint PlayerClock::now() const { std::lock_guard lock(impl_->mutex); - const auto steady_diff = impl_->now_fn() - impl_->reference.steady_time; - const int64_t player_diff = duration_nanos(steady_diff) * impl_->rate; - return impl_->reference.player_time + player_diff; + return impl_->steady_to_ros(impl_->now_fn()); } -bool PlayerClock::sleep_until(PlayerTimePoint until) +bool PlayerClock::sleep_until(ROSTimePoint until) { SteadyTimePoint steady_until; { std::lock_guard lock(impl_->mutex); - const rcutils_duration_value_t diff_nanos = - (until - impl_->reference.player_time) / impl_->rate; - steady_until = impl_->reference.steady_time + std::chrono::nanoseconds(diff_nanos); + steady_until = impl_->ros_to_steady(until); } // TODO(emersonknapp) - when we have methods that can change timeflow during a sleep, // it will probably be better to use a condition_variable::wait_until @@ -93,6 +96,7 @@ bool PlayerClock::sleep_until(PlayerTimePoint until) double PlayerClock::get_rate() const { + std::lock_guard lock(impl_->mutex); return impl_->rate; } diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_player_clock.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_player_clock.cpp index abe59458cb..80fef1b09c 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_player_clock.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_player_clock.cpp @@ -18,10 +18,10 @@ using namespace testing; // NOLINT using SteadyTimePoint = rosbag2_cpp::PlayerClock::SteadyTimePoint; -using PlayerTimePoint = rosbag2_cpp::PlayerClock::PlayerTimePoint; +using ROSTimePoint = rosbag2_cpp::PlayerClock::ROSTimePoint; using NowFunction = rosbag2_cpp::PlayerClock::NowFunction; -PlayerTimePoint as_nanos(const SteadyTimePoint & time) +ROSTimePoint as_nanos(const SteadyTimePoint & time) { return std::chrono::duration_cast(time.time_since_epoch()).count(); } @@ -66,7 +66,7 @@ TEST(PlayerClock, nonzero_start_time) NowFunction now_fn = [&return_time]() { return return_time; }; - const PlayerTimePoint start_time = 1234567890LL; + const ROSTimePoint start_time = 1234567890LL; rosbag2_cpp::PlayerClock pclock(start_time, 1.0, now_fn); EXPECT_EQ(pclock.now(), start_time); diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index a70215ad6f..9c275cbc80 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -95,7 +95,7 @@ void Player::play(const PlayOptions & options) { // Initialize Clock { - rosbag2_cpp::PlayerClock::PlayerTimePoint time_first_message; + rosbag2_cpp::PlayerClock::ROSTimePoint time_first_message; float rate = 1.0; if (options.rate > 0.0) {