From 7351423884934e431f56e180015c5489eed5ef81 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Mon, 12 Jun 2023 14:16:30 -0700
Subject: [PATCH] Rewrite TimeControllerClockTest.unpaused_sleep_returns_true
 to be correct (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 <emerson.b.knapp@gmail.com>
(cherry picked from commit 38e9c964e1c7473099cb4c7be978b761a841ccd4)
---
 .../clocks/time_controller_clock.hpp          | 10 ++++++
 .../clocks/time_controller_clock.cpp          |  7 ++++
 .../test_time_controller_clock.cpp            | 32 ++++++++++++++++---
 3 files changed, 44 insertions(+), 5 deletions(-)

diff --git a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp b/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp
index a0c971fa8e..692972b93a 100644
--- a/rosbag2_cpp/include/rosbag2_cpp/clocks/time_controller_clock.hpp
+++ b/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_;
 };
diff --git a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp b/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp
index d951569751..692d9ae890 100644
--- a/rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp
+++ b/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
diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp
index f9fa002a0b..66ae791cd5 100644
--- a/rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp
+++ b/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)