Enforce spin rate on TimeControllerClock::sleep_until

Control the rate of spinning in `while (!clock.sleep_until())` by giving a real-time sleep timeout.

This will be useful for pause/resume, to dictate the sleeping time while paused. It will also be useful for clock publishing, so that the Player thread can give control back at the right frequency to publish /clock

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

Add tests and enforce good input values

Signed-off-by: Emerson Knapp <eknapp@amazon.com>

rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp

@@ -52,9 +52,10 @@ class PlayerClock
   /**
    * 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.
+   *
+   * \param until: The ROS time to sleep until
+   * \return true if the `until` has been reached, false if timeout or awakened early.
    */
   ROSBAG2_CPP_PUBLIC
   virtual bool sleep_until(rcutils_time_point_value_t until) = 0;

rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp

@@ -36,6 +36,10 @@ class TimeControllerClock : public PlayerClock
    *
    * \param starting_time: provides an initial offset for managing time
    *    This will likely be the timestamp of the first message in the bag
+   * \param sleep_timeout: maximum amount of time to sleep even if time is not reached
+   *    This helps dictate spin rate when clock is paused, as well as allowing periodic return
+   *    of control even if messages are far apart.
+   *    Value must be greater than 0.
    * \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
@@ -45,6 +49,7 @@ class TimeControllerClock : public PlayerClock
   ROSBAG2_CPP_PUBLIC
   TimeControllerClock(
     rcutils_time_point_value_t starting_time,
+    std::chrono::nanoseconds sleep_timeout,
     double rate = 1.0,
     NowFunction now_fn = std::chrono::steady_clock::now);
 
@@ -60,9 +65,10 @@ class TimeControllerClock : public PlayerClock
   /**
    * 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.
+   *
+   * \param until: The ROS time to sleep until
+   * \return true if the `until` has been reached, false if timeout or awakened early.
    */
   ROSBAG2_CPP_PUBLIC
   bool sleep_until(rcutils_time_point_value_t until) override;

rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <algorithm>
 #include <condition_variable>
 #include <memory>
 #include <mutex>
@@ -55,8 +56,11 @@ 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::nanoseconds sleep_timeout)
+  : now_fn(now_fn),
+    sleep_timeout(sleep_timeout)
   {}
   virtual ~TimeControllerClockImpl() = default;
 
@@ -82,6 +86,7 @@ class TimeControllerClockImpl
   }
 
   const PlayerClock::NowFunction now_fn;
+  const std::chrono::nanoseconds sleep_timeout;
 
   std::mutex state_mutex;
   std::condition_variable cv RCPPUTILS_TSA_GUARDED_BY(state_mutex);
@@ -91,11 +96,15 @@ class TimeControllerClockImpl
 
 TimeControllerClock::TimeControllerClock(
   rcutils_time_point_value_t starting_time,
+  std::chrono::nanoseconds sleep_timeout,
   double rate,
   NowFunction now_fn)
-: impl_(std::make_unique<TimeControllerClockImpl>(now_fn))
+: impl_(std::make_unique<TimeControllerClockImpl>(now_fn, sleep_timeout))
 {
   std::lock_guard<std::mutex> lock(impl_->state_mutex);
+  if (sleep_timeout <= std::chrono::nanoseconds{0}) {
+    throw std::runtime_error("Sleep timeout cannot be negative or 0");
+  }
   impl_->reference.ros = starting_time;
   impl_->reference.steady = impl_->now_fn();
   impl_->rate = rate;
@@ -112,9 +121,10 @@ rcutils_time_point_value_t TimeControllerClock::now() const
 
 bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until)
 {
+  const auto until_timeout = impl_->now_fn() + impl_->sleep_timeout;
   {
     TSAUniqueLock lock(impl_->state_mutex);
-    const auto steady_until = impl_->ros_to_steady(until);
+    const auto steady_until = std::min(impl_->ros_to_steady(until), until_timeout);
     impl_->cv.wait_until(lock, steady_until);
   }
   return now() >= until;

rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp

@@ -14,6 +14,7 @@
 
 #include <gmock/gmock.h>
 
+#include "rcutils/time.h"
 #include "rosbag2_cpp/clocks/time_controller_clock.hpp"
 
 using namespace testing;  // NOLINT
@@ -30,20 +31,34 @@ class TimeControllerClockTest : public Test
       };
   }
 
+  template<typename Duration>
+  rcutils_time_point_value_t as_nanos(Duration d)
+  {
+    return std::chrono::duration_cast<std::chrono::nanoseconds>(d).count();
+  }
+
   rcutils_time_point_value_t as_nanos(const SteadyTimePoint & time)
   {
-    return std::chrono::duration_cast<std::chrono::nanoseconds>(time.time_since_epoch()).count();
+    return as_nanos(time.time_since_epoch());
+  }
+
+  template<typename Duration1, typename Duration2>
+  rcutils_duration_value_t abs_diff(Duration1 a, Duration2 b)
+  {
+    return std::abs(as_nanos(a) - as_nanos(b));
   }
 
   NowFunction now_fn;
+  double playback_rate = 1.0;
   SteadyTimePoint return_time;  // defaults to 0
+  std::chrono::nanoseconds sleep_timeout{25000000};  // 40Hz
   rcutils_time_point_value_t ros_start_time = 0;
 };
 
 TEST_F(TimeControllerClockTest, steadytime_precision)
 {
   const double playback_rate = 1.0;
-  rosbag2_cpp::TimeControllerClock pclock(ros_start_time, playback_rate, now_fn);
+  rosbag2_cpp::TimeControllerClock pclock(ros_start_time, sleep_timeout, playback_rate, now_fn);
 
   const SteadyTimePoint begin_time(std::chrono::seconds(0));
   return_time = begin_time;
@@ -75,7 +90,7 @@ TEST_F(TimeControllerClockTest, nonzero_start_time)
 {
   ros_start_time = 1234567890LL;
   const double playback_rate = 1.0;
-  rosbag2_cpp::TimeControllerClock pclock(ros_start_time, playback_rate, now_fn);
+  rosbag2_cpp::TimeControllerClock pclock(ros_start_time, sleep_timeout, playback_rate, now_fn);
 
   const SteadyTimePoint begin_time(std::chrono::seconds(0));
   return_time = begin_time;
@@ -88,7 +103,7 @@ TEST_F(TimeControllerClockTest, nonzero_start_time)
 TEST_F(TimeControllerClockTest, fast_rate)
 {
   const double playback_rate = 2.5;
-  rosbag2_cpp::TimeControllerClock pclock(ros_start_time, playback_rate, now_fn);
+  rosbag2_cpp::TimeControllerClock pclock(ros_start_time, sleep_timeout, playback_rate, now_fn);
 
   const SteadyTimePoint begin_time(std::chrono::seconds(0));
   return_time = begin_time;
@@ -102,7 +117,7 @@ TEST_F(TimeControllerClockTest, fast_rate)
 TEST_F(TimeControllerClockTest, slow_rate)
 {
   const double playback_rate = 0.4;
-  rosbag2_cpp::TimeControllerClock pclock(ros_start_time, playback_rate, now_fn);
+  rosbag2_cpp::TimeControllerClock pclock(ros_start_time, sleep_timeout, playback_rate, now_fn);
 
   const SteadyTimePoint begin_time(std::chrono::seconds(0));
   return_time = begin_time;
@@ -112,3 +127,40 @@ TEST_F(TimeControllerClockTest, slow_rate)
   return_time = some_time;
   EXPECT_EQ(pclock.now(), as_nanos(some_time) * playback_rate);
 }
+
+TEST_F(TimeControllerClockTest, sleep_rate_checking)
+{
+  sleep_timeout = std::chrono::nanoseconds{0};
+  EXPECT_THROW(
+    rosbag2_cpp::TimeControllerClock(
+      ros_start_time, sleep_timeout, playback_rate, now_fn), std::runtime_error);
+
+  sleep_timeout = std::chrono::nanoseconds{-5};
+  EXPECT_THROW(
+    rosbag2_cpp::TimeControllerClock(
+      ros_start_time, sleep_timeout, playback_rate, now_fn), std::runtime_error);
+}
+
+TEST_F(TimeControllerClockTest, sleep_rate_control)
+{
+  const rcutils_time_point_value_t one_second = RCUTILS_S_TO_NS(1);
+  const unsigned int call_count_goal = 10;
+  sleep_timeout = std::chrono::nanoseconds{RCUTILS_S_TO_NS(1) / 40};  // 40Hz
+  const auto expected_elapsed = sleep_timeout * call_count_goal;
+  // Giving 10% error tolerance because sleeping is not exact.
+  // An incorrect result would be orders of magnitude off (free loop with no wait).
+  const auto error_tolerance = static_cast<rcutils_duration_value_t>(
+    as_nanos(expected_elapsed) * 0.1);
+
+  // Don't override now_fn, we need to use the real clock in order to have sleep work
+  rosbag2_cpp::TimeControllerClock clock(ros_start_time, sleep_timeout);
+
+  const auto start_time = std::chrono::steady_clock::now();
+  for (unsigned int call_count = 0; call_count < call_count_goal; call_count++) {
+    EXPECT_FALSE(clock.sleep_until(one_second));
+  }
+  const auto elapsed_time = std::chrono::steady_clock::now() - start_time;
+
+  const auto time_error = abs_diff(elapsed_time, expected_elapsed);
+  EXPECT_LE(time_error, error_tolerance);
+}

rosbag2_transport/src/rosbag2_transport/player.cpp

@@ -217,7 +217,8 @@ void Player::prepare_publishers(const PlayOptions & options)
 void Player::prepare_clock(const PlayOptions & options, rcutils_time_point_value_t starting_time)
 {
   double rate = options.rate > 0.0 ? options.rate : 1.0;
-  clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(starting_time, rate);
+  std::chrono::nanoseconds sleep_timeout{RCUTILS_S_TO_NS(1)};  // default 1Hz spin on paused
+  clock_ = std::make_unique<rosbag2_cpp::TimeControllerClock>(starting_time, sleep_timeout, rate);
 }
 
 }  // namespace rosbag2_transport