Fix Rate::sleep not resyncing after a backward time jump

rclcpp/src/rclcpp/rate.cpp

@@ -52,7 +52,7 @@ Rate::sleep()
     next_interval = now + period_;
   }
   // Update the interval
-  last_interval_ += period_;
+  last_interval_ = next_interval;
   // If the time_to_sleep is negative or zero, don't sleep
   if (next_interval <= now) {
     // If an entire cycle was missed then reset next interval.

rclcpp/test/rclcpp/test_rate.cpp

@@ -14,9 +14,16 @@
 
 #include <gtest/gtest.h>
 
+#include <atomic>
+#include <memory>
+#include <mutex>
 #include <stdexcept>
 #include <string>
+#include <thread>
 
+#include "rcl/time.h"
+
+#include "rclcpp/clock.hpp"
 #include "rclcpp/rate.hpp"
 #include "rclcpp/utilities.hpp"
 
@@ -119,6 +126,47 @@ TEST_F(TestRate, wall_rate_basics) {
   EXPECT_GT(epsilon, delta);
 }
 
+TEST_F(TestRate, resync_after_backward_time_jump) {
+  using namespace std::chrono_literals;
+  auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  rcl_clock_t * clock_handle = clock->get_clock_handle();
+  ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(clock_handle));
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock_handle, RCL_S_TO_NS(100)));
+
+  rclcpp::Rate rate(rclcpp::Duration(1s), clock);
+
+  // Jump back in time, then advance ROS time by 10 ms every 1 ms of wall time.
+  std::atomic<rcl_time_point_value_t> ros_time{RCL_S_TO_NS(50)};
+  ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock_handle, ros_time));
+  std::atomic<bool> done{false};
+  std::thread ticker([&]() {
+      while (!done) {
+        std::this_thread::sleep_for(1ms);
+        ros_time += RCL_MS_TO_NS(10);
+        // Like TimeSource, hold the clock mutex: setting the time runs the jump
+        // callbacks, which Rate::sleep() adds and removes concurrently.
+        std::lock_guard<std::mutex> clock_guard(clock->get_clock_mutex());
+        EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock_handle, ros_time));
+      }
+    });
+
+  ASSERT_TRUE(rate.sleep());
+  const auto first_wakeup = clock->now();
+
+  // Simulate work that takes half a period.
+  while (clock->now() < first_wakeup + rclcpp::Duration(500ms)) {
+    std::this_thread::sleep_for(1ms);
+  }
+
+  // The next wakeup is one period after the previous one, not one period after now.
+  ASSERT_TRUE(rate.sleep());
+  const auto second_wakeup = clock->now();
+  done = true;
+  ticker.join();
+
+  EXPECT_LT((second_wakeup - first_wakeup).nanoseconds(), RCL_MS_TO_NS(1250));
+}
+
 TEST_F(TestRate, from_double) {
   {
     rclcpp::Rate rate(1.0);