Rewrite TimeControllerClockTest.unpaused_sleep_returns_true to be cor…

…rect (#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 38e9c96)

# Conflicts:
#	rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp

rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp

@@ -145,13 +145,41 @@ 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));
 
+<<<<<<< HEAD
   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));
+    }
+  }
+>>>>>>> 38e9c96 (Rewrite TimeControllerClockTest.unpaused_sleep_returns_true to be correct (#1384))
 }
 
 TEST_F(TimeControllerClockTest, paused_sleep_returns_false_quickly)