Add PlayerClock::is_sleeping()

Signed-off-by: Christophe Bedard <[email protected]>

rosbag2_cpp/include/rosbag2_cpp/clocks/player_clock.hpp

@@ -77,6 +77,11 @@ class PlayerClock
   ROSBAG2_CPP_PUBLIC
   virtual bool sleep_until(rclcpp::Time time) = 0;
 
+  /**
+   * \return whether the clock is currently sleeping.
+   */
+  virtual bool is_sleeping() = 0;
+
   /**
    * \brief Wake up the clock if it is sleeping.
    * \note This will wake any waiting `sleep_until`.

rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp

@@ -81,6 +81,9 @@ class TimeControllerClock : public PlayerClock
   ROSBAG2_CPP_PUBLIC
   bool sleep_until(rclcpp::Time until) override;
 
+  ROSBAG2_CPP_PUBLIC
+  bool is_sleeping() override;
+
   ROSBAG2_CPP_PUBLIC
   void wakeup() override;
 

rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp

@@ -123,6 +123,7 @@ class TimeControllerClockImpl
   std::condition_variable cv RCPPUTILS_TSA_GUARDED_BY(state_mutex);
   double rate RCPPUTILS_TSA_GUARDED_BY(state_mutex) = 1.0;
   bool paused RCPPUTILS_TSA_GUARDED_BY(state_mutex) = false;
+  bool is_sleeping RCPPUTILS_TSA_GUARDED_BY(state_mutex) = false;
   TimeReference reference RCPPUTILS_TSA_GUARDED_BY(state_mutex);
 };
 
@@ -165,12 +166,16 @@ bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until)
   {
     rcpputils::unique_lock<std::mutex> lock(impl_->state_mutex);
     if (impl_->paused) {
+      impl_->is_sleeping = true;
       impl_->cv.wait_for(lock, impl_->sleep_time_while_paused);
+      impl_->is_sleeping = false;
     } else {
       const auto steady_until = impl_->ros_to_steady(until);
       // wait only if necessary for performance
       if (steady_until > impl_->now_fn()) {
+        impl_->is_sleeping = true;
         impl_->cv.wait_until(lock, steady_until);
+        impl_->is_sleeping = false;
       }
     }
     if (impl_->paused) {
@@ -187,6 +192,12 @@ bool TimeControllerClock::sleep_until(rclcpp::Time until)
   return sleep_until(until.nanoseconds());
 }
 
+bool TimeControllerClock::is_sleeping()
+{
+  std::lock_guard<std::mutex> lock(impl_->state_mutex);
+  return impl_->is_sleeping;
+}
+
 void TimeControllerClock::wakeup()
 {
   impl_->cv.notify_all();

rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp

@@ -217,17 +217,21 @@ TEST_F(TimeControllerClockTest, wakeup_sleep_returns_immediately)
   auto sleep_time_while_paused = std::chrono::seconds(30);
   rosbag2_cpp::TimeControllerClock clock(ros_start_time, now_fn, sleep_time_while_paused);
   clock.pause();
+  EXPECT_FALSE(clock.is_sleeping());
   auto steady_start = std::chrono::steady_clock::now();
   std::atomic_bool thread_sleep_result{true};
   auto sleep_long_thread = std::thread(
     [&clock, &thread_sleep_result]() {
       bool sleep_result = clock.sleep_until(clock.now() + RCUTILS_S_TO_NS(10));
       thread_sleep_result.store(sleep_result);
     });
-  // Wait enough time for the thread to start sleeping before we try to wake it up
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  // Wait for the thread to be sleeping for the purposes of this test
+  while (!clock.is_sleeping()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
   clock.wakeup();
   sleep_long_thread.join();
+  EXPECT_FALSE(clock.is_sleeping());
   // Check that the clock slept less than the sleep_time_while_paused value
   EXPECT_LT(std::chrono::steady_clock::now() - steady_start, sleep_time_while_paused);
   EXPECT_FALSE(thread_sleep_result);