Initial PlayerClock integration - functionality unchanged

Create new PlayerClock class with `now()`, `sleep_until()`, and rate handling, with tests.
Removes time handling from `Player` in favor of using this new class.

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

rosbag2_cpp/CMakeLists.txt

@@ -53,6 +53,7 @@ add_library(${PROJECT_NAME} SHARED
   src/rosbag2_cpp/cache/message_cache.cpp
   src/rosbag2_cpp/converter.cpp
   src/rosbag2_cpp/info.cpp
+  src/rosbag2_cpp/player_clock.cpp
   src/rosbag2_cpp/reader.cpp
   src/rosbag2_cpp/readers/sequential_reader.cpp
   src/rosbag2_cpp/rmw_implemented_serialization_format_converter.cpp
@@ -217,6 +218,13 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_multifile_reader rosbag2_storage)
     target_link_libraries(test_multifile_reader ${PROJECT_NAME})
   endif()
+
+  ament_add_gmock(test_player_clock
+    test/rosbag2_cpp/test_player_clock.cpp)
+  if(TARGET test_player_clock)
+    # ament_target_dependencies(test_player_clock rosbag2_cpp)
+    target_link_libraries(test_player_clock ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package()

rosbag2_cpp/include/rosbag2_cpp/player_clock.hpp

@@ -0,0 +1,84 @@
+// Copyright 2021 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ROSBAG2_CPP__PLAYER_CLOCK_HPP_
+#define ROSBAG2_CPP__PLAYER_CLOCK_HPP_
+
+#include <chrono>
+#include <functional>
+#include <memory>
+
+#include "rcutils/time.h"
+
+namespace rosbag2_cpp
+{
+class PlayerClockImpl;
+
+/**
+ * Used to control the timing of bag playback.
+ * This clock should be used to query times and sleep between message playing,
+ * so that the complexity involved around time control and time sources
+ * is encapsulated in this one place.
+ */
+class PlayerClock
+{
+public:
+  typedef rcutils_time_point_value_t PlayerTimePoint;
+  typedef std::chrono::time_point<std::chrono::steady_clock> RealTimePoint;
+  typedef std::function<RealTimePoint()> NowFunction;
+
+  /**
+   * Constructor.
+   *
+   * \param starting_time: provides an initial offset for managing time
+   *    This will likely be the timestamp of the first message in the bag
+   * \param rate: Rate of playback, a unit-free ratio. 1.0 is real-time
+   * \param now_fn: Function used to get the current steady time
+   *   defaults to std::chrono::steady_clock::now
+   *   Used to control for unit testing, or for specialized needs
+   * \throws std::runtime_error if rate is <= 0
+   */
+  PlayerClock(
+    PlayerTimePoint starting_time,
+    double rate = 1.0,
+    NowFunction now_fn = std::chrono::steady_clock::now);
+
+  virtual ~PlayerClock();
+
+  /**
+   * Calculate and return current PlayerTimePoint based on starting time, playback rate, pause state.
+   */
+  PlayerTimePoint now() const;
+
+  /**
+   * Try to sleep (non-busy) the current thread until the provided time is reached - according to this Clock
+   *
+   * Return true if the time has been reached, false if it was not successfully reached after sleeping
+   * for the appropriate duration.
+   * The user should not take action based on this sleep until it returns true.
+   */
+  bool sleep_until(PlayerTimePoint until);
+
+  /**
+   * Return the current playback rate.
+   */
+  float get_rate() const;
+
+private:
+  std::unique_ptr<PlayerClockImpl> impl_;
+};
+
+}  // namespace rosbag2_cpp
+
+#endif  // ROSBAG2_CPP__PLAYER_CLOCK_HPP_

rosbag2_cpp/src/rosbag2_cpp/player_clock.cpp

@@ -0,0 +1,99 @@
+// Copyright 2021 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#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::RealTimePoint real_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
+{
+public:
+  PlayerClockImpl() = default;
+
+  TimeSync reference;
+  double rate = 1.0;
+  PlayerClock::NowFunction now_fn;
+  std::mutex mutex;
+};
+
+PlayerClock::PlayerClock(PlayerTimePoint 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.real_time = impl_->now_fn();
+  impl_->rate = rate;
+}
+
+PlayerClock::~PlayerClock()
+{}
+
+PlayerClock::PlayerTimePoint PlayerClock::now() const
+{
+  std::lock_guard<std::mutex> lock(impl_->mutex);
+  const auto real_diff = impl_->now_fn() - impl_->reference.real_time;
+  const int64_t player_diff = duration_nanos(real_diff) * impl_->rate;
+  return impl_->reference.player_time + player_diff;
+}
+
+bool PlayerClock::sleep_until(PlayerTimePoint until)
+{
+  RealTimePoint real_until;
+  {
+    std::lock_guard<std::mutex> lock(impl_->mutex);
+    const rcutils_duration_value_t diff_nanos =
+      (until - impl_->reference.player_time) / impl_->rate;
+    real_until = impl_->reference.real_time + std::chrono::nanoseconds(diff_nanos);
+  }
+  // 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
+  std::this_thread::sleep_until(real_until);
+  return now() >= until;
+}
+
+float PlayerClock::get_rate() const
+{
+  return impl_->rate;
+}
+
+}  // namespace rosbag2_cpp

rosbag2_cpp/test/rosbag2_cpp/test_player_clock.cpp

@@ -0,0 +1,110 @@
+// Copyright 2021 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gmock/gmock.h>
+
+#include "rosbag2_cpp/player_clock.hpp"
+
+using namespace testing;  // NOLINT
+using RealTimePoint = rosbag2_cpp::PlayerClock::RealTimePoint;
+using PlayerTimePoint = rosbag2_cpp::PlayerClock::PlayerTimePoint;
+using NowFunction = rosbag2_cpp::PlayerClock::NowFunction;
+
+PlayerTimePoint as_nanos(const RealTimePoint & time)
+{
+  return std::chrono::duration_cast<std::chrono::nanoseconds>(time.time_since_epoch()).count();
+}
+
+TEST(PlayerClock, realtime_precision)
+{
+  RealTimePoint return_time;
+  NowFunction now_fn = [&return_time]() {
+      return return_time;
+    };
+  rosbag2_cpp::PlayerClock pclock(0, 1.0, now_fn);
+
+  const RealTimePoint begin_time(std::chrono::seconds(0));
+  return_time = begin_time;
+  EXPECT_EQ(pclock.now(), as_nanos(begin_time));
+
+  const RealTimePoint ten_seconds(std::chrono::seconds(10));
+  return_time = ten_seconds;
+  EXPECT_EQ(pclock.now(), as_nanos(ten_seconds));
+
+  // NOTE: this would have already lost precision at 100 seconds if we were multiplying by float
+  const RealTimePoint hundred_seconds(std::chrono::seconds(100));
+  return_time = hundred_seconds;
+  EXPECT_EQ(pclock.now(), as_nanos(hundred_seconds));
+
+  const int64_t near_limit_nanos = 1LL << 61;
+  const auto near_limit_time = RealTimePoint(std::chrono::nanoseconds(near_limit_nanos));
+  return_time = near_limit_time;
+  EXPECT_EQ(pclock.now(), near_limit_nanos);
+
+  // Expect to lose exact equality at this point due to precision limit of double*int mult
+  const int64_t over_limit_nanos = (1LL << 61) + 1LL;
+  const auto over_limit_time = RealTimePoint(std::chrono::nanoseconds(over_limit_nanos));
+  return_time = over_limit_time;
+  EXPECT_NE(pclock.now(), over_limit_nanos);
+  EXPECT_LT(std::abs(pclock.now() - over_limit_nanos), 10LL);
+}
+
+TEST(PlayerClock, nonzero_start_time)
+{
+  RealTimePoint return_time;
+  NowFunction now_fn = [&return_time]() {
+      return return_time;
+    };
+  const PlayerTimePoint start_time = 1234567890LL;
+  rosbag2_cpp::PlayerClock pclock(start_time, 1.0, now_fn);
+
+  EXPECT_EQ(pclock.now(), start_time);
+
+  return_time = RealTimePoint(std::chrono::seconds(1));
+  EXPECT_EQ(pclock.now(), start_time + as_nanos(return_time));
+}
+
+TEST(PlayerClock, fast_rate)
+{
+  RealTimePoint return_time;
+  NowFunction now_fn = [&return_time]() {
+      return return_time;
+    };
+  rosbag2_cpp::PlayerClock pclock(0, 2.5, now_fn);
+
+  const RealTimePoint begin_time(std::chrono::seconds(0));
+  return_time = begin_time;
+  EXPECT_EQ(pclock.now(), as_nanos(begin_time));
+
+  const RealTimePoint some_time(std::chrono::seconds(3));
+  return_time = some_time;
+  EXPECT_EQ(pclock.now(), as_nanos(some_time) * 2.5);
+}
+
+TEST(PlayerClock, slow_rate)
+{
+  RealTimePoint return_time;
+  NowFunction now_fn = [&return_time]() {
+      return return_time;
+    };
+  rosbag2_cpp::PlayerClock pclock(0, 0.4, now_fn);
+
+  const RealTimePoint begin_time(std::chrono::seconds(0));
+  return_time = begin_time;
+  EXPECT_EQ(pclock.now(), as_nanos(begin_time));
+
+  const RealTimePoint some_time(std::chrono::seconds(12));
+  return_time = some_time;
+  EXPECT_EQ(pclock.now(), as_nanos(some_time) * 0.4);
+}

rosbag2_storage/include/rosbag2_storage/serialized_bag_message.hpp

@@ -31,6 +31,8 @@ struct SerializedBagMessage
   std::string topic_name;
 };
 
+typedef std::shared_ptr<SerializedBagMessage> SerializedBagMessagePtr;
+
 }  // namespace rosbag2_storage
 
 #endif  // ROSBAG2_STORAGE__SERIALIZED_BAG_MESSAGE_HPP_