PlayerClock initial implementation - Player functionally unchanged

rosbag2_cpp/CMakeLists.txt

@@ -53,6 +53,7 @@ add_library(${PROJECT_NAME} SHARED
   src/rosbag2_cpp/cache/cache_consumer.cpp
   src/rosbag2_cpp/cache/message_cache_buffer.cpp
   src/rosbag2_cpp/cache/message_cache.cpp
+  src/rosbag2_cpp/clocks/time_controller_clock.cpp
   src/rosbag2_cpp/converter.cpp
   src/rosbag2_cpp/info.cpp
   src/rosbag2_cpp/reader.cpp
@@ -222,6 +223,12 @@ if(BUILD_TESTING)
     ament_target_dependencies(test_multifile_reader rosbag2_storage)
     target_link_libraries(test_multifile_reader ${PROJECT_NAME})
   endif()
+
+  ament_add_gmock(test_time_controller_clock
+    test/rosbag2_cpp/test_time_controller_clock.cpp)
+  if(TARGET test_time_controller_clock)
+    target_link_libraries(test_time_controller_clock ${PROJECT_NAME})
+  endif()
 endif()
 
 ament_package()

rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp

@@ -0,0 +1,71 @@
+// 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__CLOCKS__PLAYER_CLOCK_HPP_
+#define ROSBAG2_CPP__CLOCKS__PLAYER_CLOCK_HPP_
+
+#include <chrono>
+#include <functional>
+#include <memory>
+
+#include "rcutils/time.h"
+#include "rosbag2_cpp/visibility_control.hpp"
+
+namespace rosbag2_cpp
+{
+
+/**
+ * Virtual interface used to drive 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:
+  /**
+   * 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 to the PlayerClock.
+   */
+  typedef std::function<std::chrono::steady_clock::time_point()> NowFunction;
+
+  ROSBAG2_CPP_PUBLIC
+  virtual ~PlayerClock() = default;
+
+  /**
+   * Calculate and return current rcutils_time_point_value_t based on starting time, playback rate, pause state.
+   */
+  ROSBAG2_CPP_PUBLIC
+  virtual rcutils_time_point_value_t now() const = 0;
+
+  /**
+   * 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.
+   */
+  ROSBAG2_CPP_PUBLIC
+  virtual bool sleep_until(rcutils_time_point_value_t until) = 0;
+
+  /**
+   * Return the current playback rate.
+   */
+  ROSBAG2_CPP_PUBLIC
+  virtual double get_rate() const = 0;
+};
+
+}  // namespace rosbag2_cpp
+
+#endif  // ROSBAG2_CPP__CLOCKS__PLAYER_CLOCK_HPP_

rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp

@@ -0,0 +1,83 @@
+// 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__CLOCKS__TIME_CONTROLLER_CLOCK_HPP_
+#define ROSBAG2_CPP__CLOCKS__TIME_CONTROLLER_CLOCK_HPP_
+
+#include <chrono>
+#include <memory>
+
+#include "rosbag2_cpp/clocks/player_clock.hpp"
+
+namespace rosbag2_cpp
+{
+
+/**
+ * Version of the PlayerClock interface that has control over time.
+ * It does not listen to any external ROS Time Source and can optionally publish /clock
+ */
+class TimeControllerClockImpl;
+class TimeControllerClock : public PlayerClock
+{
+public:
+  /**
+   * 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
+   */
+  ROSBAG2_CPP_PUBLIC
+  TimeControllerClock(
+    rcutils_time_point_value_t starting_time,
+    double rate = 1.0,
+    NowFunction now_fn = std::chrono::steady_clock::now);
+
+  ROSBAG2_CPP_PUBLIC
+  virtual ~TimeControllerClock();
+
+  /**
+   * Calculate and return current rcutils_time_point_value_t based on starting time, playback rate, pause state.
+   */
+  ROSBAG2_CPP_PUBLIC
+  rcutils_time_point_value_t now() const override;
+
+  /**
+   * 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.
+   */
+  ROSBAG2_CPP_PUBLIC
+  bool sleep_until(rcutils_time_point_value_t until) override;
+
+  /**
+   * Return the current playback rate.
+   */
+  ROSBAG2_CPP_PUBLIC
+  double get_rate() const override;
+
+private:
+  std::unique_ptr<TimeControllerClockImpl> impl_;
+};
+
+
+}  // namespace rosbag2_cpp
+
+#endif  // ROSBAG2_CPP__CLOCKS__TIME_CONTROLLER_CLOCK_HPP_

rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp

@@ -0,0 +1,108 @@
+// 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 <condition_variable>
+#include <memory>
+#include <mutex>
+#include <thread>
+
+#include "rcpputils/thread_safety_annotations.hpp"
+#include "rosbag2_cpp/clocks/time_controller_clock.hpp"
+#include "rosbag2_cpp/types.hpp"
+
+namespace rosbag2_cpp
+{
+
+class TimeControllerClockImpl
+{
+public:
+  /**
+   * Stores an exact time match between a system steady clock and the playback ROS clock.
+   * This snapshot is taken whenever a factor changes such that a new reference is needed,
+   * such as pause, resume, rate change, or jump
+   */
+  struct TimeReference
+  {
+    rcutils_time_point_value_t ros;
+    std::chrono::steady_clock::time_point steady;
+  };
+
+  TimeControllerClockImpl() = default;
+  virtual ~TimeControllerClockImpl() = default;
+
+  template<typename T>
+  rcutils_duration_value_t duration_nanos(const T & duration)
+  {
+    return std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count();
+  }
+
+  rcutils_time_point_value_t steady_to_ros(std::chrono::steady_clock::time_point steady_time)
+  {
+    return reference.ros + static_cast<rcutils_duration_value_t>(
+      rate * duration_nanos(steady_time - reference.steady));
+  }
+
+  std::chrono::steady_clock::time_point ros_to_steady(rcutils_time_point_value_t ros_time)
+  {
+    const auto diff_nanos = static_cast<rcutils_duration_value_t>(
+      (ros_time - reference.ros) / rate);
+    return reference.steady + std::chrono::nanoseconds(diff_nanos);
+  }
+
+  std::mutex mutex;
+  std::condition_variable cv;
+  PlayerClock::NowFunction now_fn RCPPUTILS_TSA_GUARDED_BY(mutex);
+  double rate RCPPUTILS_TSA_GUARDED_BY(mutex) = 1.0;
+  TimeReference reference RCPPUTILS_TSA_GUARDED_BY(mutex);
+};
+
+TimeControllerClock::TimeControllerClock(
+  rcutils_time_point_value_t starting_time,
+  double rate,
+  NowFunction now_fn)
+: impl_(std::make_unique<TimeControllerClockImpl>())
+{
+  std::lock_guard<std::mutex> lock(impl_->mutex);
+  impl_->now_fn = now_fn;
+  impl_->reference.ros = starting_time;
+  impl_->reference.steady = impl_->now_fn();
+  impl_->rate = rate;
+}
+
+TimeControllerClock::~TimeControllerClock()
+{}
+
+rcutils_time_point_value_t TimeControllerClock::now() const
+{
+  std::lock_guard<std::mutex> lock(impl_->mutex);
+  return impl_->steady_to_ros(impl_->now_fn());
+}
+
+bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until)
+{
+  {
+    std::unique_lock<std::mutex> lock(impl_->mutex);
+    const auto steady_until = impl_->ros_to_steady(until);
+    impl_->cv.wait_until(lock, steady_until);
+  }
+  return now() >= until;
+}
+
+double TimeControllerClock::get_rate() const
+{
+  std::lock_guard<std::mutex> lock(impl_->mutex);
+  return impl_->rate;
+}
+
+}  // namespace rosbag2_cpp