Rewrite TimeControllerClockTest.unpaused_sleep_returns_true to be cor…

…rect (backport #1384) (#1390)

* Rewrite TimeControllerClockTest.unpaused_sleep_returns_true to be correct (#1384)

Previous fix attempt's loop over spurious wakeups was never entered.

Signed-off-by: Emerson Knapp <[email protected]>
(cherry picked from commit 38e9c96)

rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp

@@ -136,6 +136,16 @@ class TimeControllerClock : public PlayerClock
     rclcpp::JumpHandler::post_callback_t post_callback,
     const rcl_jump_threshold_t & threshold) override;
 
+  /**
+   * @brief Convert an arbitrary ROSTime to a SteadyTime, based on the current reference snapshot.
+   * @note Backported as non-virtual to Humble to maintain ABI
+   * @param ros_time - time point in ROSTime
+   * @return time point in steady clock i.e. std::chrono::steady_clock
+   */
+  ROSBAG2_CPP_PUBLIC
+  std::chrono::steady_clock::time_point
+  ros_to_steady(rcutils_time_point_value_t ros_time) const;
+
 private:
   std::unique_ptr<TimeControllerClockImpl> impl_;
 };

rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp

@@ -259,4 +259,11 @@ rclcpp::JumpHandler::SharedPtr TimeControllerClock::create_jump_callback(
   return nullptr;
 }
 
+std::chrono::steady_clock::time_point
+TimeControllerClock::ros_to_steady(rcutils_time_point_value_t ros_time) const
+{
+  std::lock_guard<std::mutex> lock(impl_->state_mutex);
+  return impl_->ros_to_steady(ros_time);
+}
+
 }  // namespace rosbag2_cpp

rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp

@@ -145,13 +145,35 @@ TEST_F(TimeControllerClockTest, is_paused)
 
 TEST_F(TimeControllerClockTest, unpaused_sleep_returns_true)
 {
+  const float error_ratio = 1.2f;
+  const std::chrono::nanoseconds test_timeout{RCUTILS_S_TO_NS(2)};
+  const std::chrono::nanoseconds sleep_duration{RCUTILS_S_TO_NS(1)};
   rosbag2_cpp::TimeControllerClock clock(ros_start_time);
-  clock.resume();
-  EXPECT_TRUE(clock.sleep_until(clock.now() + 100));
 
-  clock.pause();
-  clock.resume();
-  EXPECT_TRUE(clock.sleep_until(clock.now() + RCUTILS_S_TO_NS(1)));
+  // Run the whole thing twice to make sure everything also works properly after pause and resume
+  for (int i = 0; i < 2; i++) {
+    clock.resume();
+    auto ros_start = clock.now();
+    auto sleep_until_timestamp = ros_start + sleep_duration.count();
+    auto steady_start = clock.ros_to_steady(ros_start);
+    bool sleep_result = false;
+    while (!sleep_result && (std::chrono::steady_clock::now() - steady_start) < test_timeout) {
+      sleep_result = clock.sleep_until(sleep_until_timestamp);
+    }
+    const auto ros_end = clock.now();
+    const auto steady_end = clock.ros_to_steady(ros_end);
+    EXPECT_TRUE(sleep_result);
+    EXPECT_GE(steady_end - steady_start, sleep_duration);
+    EXPECT_LT(steady_end - steady_start, sleep_duration * error_ratio);
+    EXPECT_GE(ros_end - ros_start, sleep_duration.count());
+    EXPECT_LT(ros_end - ros_start, sleep_duration.count() * error_ratio);
+
+    clock.pause();
+    // Sleep long enough between runs that ros time and steady time are diverged by an error bound
+    if (i == 0) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(200));
+    }
+  }
 }
 
 TEST_F(TimeControllerClockTest, paused_sleep_returns_false_quickly)