diff --git a/rclcpp/src/rclcpp/rate.cpp b/rclcpp/src/rclcpp/rate.cpp index 9f31dbb1fc..bec471fcc5 100644 --- a/rclcpp/src/rclcpp/rate.cpp +++ b/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. diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index bbaf1beb4d..4cf9dbcb75 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -14,9 +14,16 @@ #include +#include +#include +#include #include #include +#include +#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(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 ros_time{RCL_S_TO_NS(50)}; + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock_handle, ros_time)); + std::atomic 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 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);