Add tests, fix functionality problems revealed in test

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

rosbag2_cpp/src/rosbag2_cpp/clocks/time_controller_clock.cpp

@@ -63,30 +63,39 @@ class TimeControllerClockImpl
   virtual ~TimeControllerClockImpl() = default;
 
   template<typename T>
-  rcutils_duration_value_t duration_nanos(const T & duration)
+  rcutils_duration_value_t duration_nanos(const T & duration) const
   {
     return std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count();
   }
 
-  rcutils_time_point_value_t steady_to_ros(std::chrono::steady_clock::time_point steady_time)
+  rcutils_time_point_value_t steady_to_ros(std::chrono::steady_clock::time_point steady_time) const
   RCPPUTILS_TSA_REQUIRES(state_mutex)
   {
     return reference.ros + static_cast<rcutils_duration_value_t>(
       rate * duration_nanos(steady_time - reference.steady));
   }
 
-  std::chrono::steady_clock::time_point ros_to_steady(rcutils_time_point_value_t ros_time)
+  std::chrono::steady_clock::time_point ros_to_steady(rcutils_time_point_value_t ros_time) const
   RCPPUTILS_TSA_REQUIRES(state_mutex)
   {
     const auto diff_nanos = static_cast<rcutils_duration_value_t>(
       (ros_time - reference.ros) / rate);
     return reference.steady + std::chrono::nanoseconds(diff_nanos);
   }
 
-  void snapshot(rcutils_time_point_value_t ros_time)
+  rcutils_time_point_value_t ros_now() const
   RCPPUTILS_TSA_REQUIRES(state_mutex)
   {
-    reference.ros = ros_time;
+    if (paused) {
+      return reference.ros;
+    }
+    return steady_to_ros(now_fn());
+  }
+
+  void snapshot()
+  RCPPUTILS_TSA_REQUIRES(state_mutex)
+  {
+    reference.ros = ros_now();
     reference.steady = now_fn();
   }
 
@@ -119,7 +128,7 @@ TimeControllerClock::~TimeControllerClock()
 rcutils_time_point_value_t TimeControllerClock::now() const
 {
   std::lock_guard<std::mutex> lock(impl_->state_mutex);
-  return impl_->steady_to_ros(impl_->now_fn());
+  return impl_->ros_now();
 }
 
 bool TimeControllerClock::sleep_until(rcutils_time_point_value_t until)
@@ -144,29 +153,25 @@ double TimeControllerClock::get_rate() const
 
 void TimeControllerClock::pause()
 {
-  {
-    std::lock_guard<std::mutex> lock(impl_->state_mutex);
-    if (impl_->paused) {
-      return;
-    }
-    // Note: needs to not be paused when taking snapshot, otherwise it will use last ros ref
-    impl_->snapshot(now());
-    impl_->paused = true;
+  std::lock_guard<std::mutex> lock(impl_->state_mutex);
+  if (impl_->paused) {
+    return;
   }
+  // Note: needs to not be paused when taking snapshot, otherwise it will use last ros ref
+  impl_->snapshot();
+  impl_->paused = true;
   impl_->cv.notify_all();
 }
 
 void TimeControllerClock::resume()
 {
-  {
-    std::lock_guard<std::mutex> lock(impl_->state_mutex);
-    if (!impl_->paused) {
-      return;
-    }
-    // Note: needs to not be paused when taking snapshot, otherwise it will use last ros ref
-    impl_->paused = false;
-    impl_->snapshot(now());
+  std::lock_guard<std::mutex> lock(impl_->state_mutex);
+  if (!impl_->paused) {
+    return;
   }
+  // Note: needs to not be paused when taking snapshot, otherwise it will use last ros ref
+  impl_->paused = false;
+  impl_->snapshot();
   impl_->cv.notify_all();
 }
 

rosbag2_cpp/test/rosbag2_cpp/test_time_controller_clock.cpp

@@ -12,6 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <atomic>
+#include <thread>
+
 #include <gmock/gmock.h>
 
 #include "rosbag2_cpp/clocks/time_controller_clock.hpp"
@@ -112,3 +115,51 @@ TEST_F(TimeControllerClockTest, slow_rate)
   return_time = some_time;
   EXPECT_EQ(pclock.now(), as_nanos(some_time) * playback_rate);
 }
+
+TEST_F(TimeControllerClockTest, basic_pause_resume)
+{
+  bool sleep_result;
+  rosbag2_cpp::TimeControllerClock clock(ros_start_time, 1.0);
+  EXPECT_FALSE(clock.is_paused());
+  clock.pause();
+  EXPECT_TRUE(clock.is_paused());
+  clock.resume();
+  EXPECT_FALSE(clock.is_paused());
+
+  // Sleep 100 nanoseconds (basically nothing)
+  clock.resume();
+  sleep_result = clock.sleep_until(clock.now() + 100);
+  EXPECT_TRUE(sleep_result);
+
+  // Try to sleep for a long time while paused, expect control to return quickly
+  clock.pause();
+  sleep_result = clock.sleep_until(clock.now() + RCUTILS_S_TO_NS(10));
+  EXPECT_FALSE(sleep_result);
+
+  // Expect now() to return the same value always, while paused.
+  clock.pause();
+  auto now_start = clock.now();
+  EXPECT_EQ(now_start, clock.now());
+  clock.sleep_until(clock.now() + RCUTILS_S_TO_NS(1));
+  EXPECT_EQ(now_start, clock.now());
+
+  // Resume time, next now() should not be the same
+  clock.resume();
+  EXPECT_NE(now_start, clock.now());
+
+  // While running, expect sleep to always make it to time (when not interrupted)
+  clock.resume();
+  sleep_result = clock.sleep_until(clock.now() + RCUTILS_S_TO_NS(1));
+  EXPECT_TRUE(sleep_result);
+
+  // Expect long sleep to be interrupted if we change pause state
+  clock.resume();
+  std::atomic_bool thread_sleep_result{true};
+  auto sleep_long_thread = std::thread(
+    [&clock, &thread_sleep_result]() {
+      thread_sleep_result.store(clock.sleep_until(clock.now() + RCUTILS_S_TO_NS(10)));
+    });
+  clock.pause();
+  sleep_long_thread.join();
+  EXPECT_FALSE(thread_sleep_result);
+}