Refactor to say ROSTime instead of PlayerTime, add time conversion he…

…lper functions to match design

Signed-off-by: Emerson Knapp <[email protected]>

rosbag2_cpp/include/rosbag2_cpp/player_clock.hpp

@@ -20,6 +20,7 @@
 #include <memory>
 
 #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:

rosbag2_cpp/src/rosbag2_cpp/player_clock.cpp

@@ -16,74 +16,77 @@
 #include <mutex>
 #include <thread>
 
+#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<typename T>
-rcutils_duration_value_t duration_nanos(const T & duration)
-{
-  return std::chrono::duration_cast<std::chrono::nanoseconds>(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<typename T>
+  rcutils_duration_value_t duration_nanos(const T & duration)
+  {
+    return std::chrono::duration_cast<std::chrono::nanoseconds>(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<PlayerClockImpl>())
 {
   std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(impl_->mutex);
   return impl_->rate;
 }
 

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<std::chrono::nanoseconds>(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);

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) {