From 1f6fb69d1adc461f2e6b9876365142a68ed3256f Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 7 Mar 2024 20:11:08 +0100 Subject: [PATCH 1/4] feat: Allow usage of rcl_timer_clock with const rcl_timer_t* Signed-off-by: Janosch Machowinski --- rcl/include/rcl/timer.h | 2 +- rcl/src/rcl/timer.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index c2f7bc1fb..179ae2465 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -284,7 +284,7 @@ rcl_timer_call(rcl_timer_t * timer); RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock); +rcl_timer_clock(const rcl_timer_t * timer, rcl_clock_t ** clock); /// Calculates whether or not the timer should be called. /** diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 8030a653e..75d6e442d 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -254,7 +254,7 @@ rcl_timer_fini(rcl_timer_t * timer) RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock) +rcl_timer_clock(const rcl_timer_t * timer, rcl_clock_t ** clock) { RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); From 143af3f027087c0fd8abc7973fa2ad748560c0e7 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 7 Mar 2024 20:14:14 +0100 Subject: [PATCH 2/4] fix: Fixed purious wake-ups on ROS_TIME timers with ROS_TIME enabled Added special handling for timers with a clock that has time override enabled. For theses timer we should not compute a timeout, as the waitset is waken up by the associated guard condition. Before this change, the waitset could wait up, because of an expected ready timer, that was acutally not ready, as the time update to the ROS_TIME had not yet arrived. Signed-off-by: Janosch Machowinski --- rcl/src/rcl/wait.c | 23 +++++++- rcl/test/rcl/test_wait.cpp | 118 +++++++++++++++++++++++++++++++++++++ 2 files changed, 140 insertions(+), 1 deletion(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 1f79a6a54..8f46ee8f5 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -558,8 +558,29 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) rmw_gcs->guard_conditions[gc_idx]; ++(rmw_gcs->guard_condition_count); } + + rcl_clock_t * clock; + if (rcl_timer_clock(wait_set->timers[i], &clock) != RCL_RET_OK) { + //should never happen + return RCL_RET_ERROR; + } + + if (clock->type == RCL_ROS_TIME) { + bool timer_override_active = false; + if (rcl_is_enabled_ros_time_override(clock, &timer_override_active) != RCL_RET_OK) { + //should never happen + return RCL_RET_ERROR; + } + + if (timer_override_active) { + // if the timer override is active, there is no point in computing a wait time, + // as it might be on a total wrong time basis. In case this timer becomes ready, + // the guard_condition above will wake us. + continue; + } + } + // use timer time to to set the rmw_wait timeout - // TODO(sloretz) fix spurious wake-ups on ROS_TIME timers with ROS_TIME enabled int64_t timer_timeout = INT64_MAX; rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); if (ret == RCL_RET_TIMER_CANCELED) { diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 626d047dc..b69c89415 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -321,6 +321,124 @@ TEST_F(WaitSetTestFixture, zero_timeout_overrun_timer) { EXPECT_LE(diff, RCL_MS_TO_NS(50)); } +// Test rcl_wait with a timeout value and an overrun timer +TEST_F(WaitSetTestFixture, no_wakeup_on_override_timer) { + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_enable_ros_time_override(&clock); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init2( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator(), + true); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Time spent during wait should be negligible, definitely less than the given timeout + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(100)); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // We don't expect a timeout here (since the guard condition had already been triggered) + ASSERT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_GE(diff, RCL_MS_TO_NS(100)); +} + +// Test rcl_wait with a timeout value and an overrun timer +TEST_F(WaitSetTestFixture, wakeup_on_override_timer) { + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_enable_ros_time_override(&clock); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init2( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator(), + true); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Time spent during wait should be negligible, definitely less than the given timeout + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + + rcl_ret_t wait_ret = RCL_RET_ERROR; + std::atomic_bool wait_done = false; + std::thread t([&wait_ret, &wait_set, &wait_done]() { + wait_ret = rcl_wait(&wait_set, RCL_MS_TO_NS(100)); + wait_done = true; + }); + + ret = rcl_set_ros_time_override(&clock, RCL_MS_TO_NS(5)); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + std::this_thread::sleep_for(std::chrono::milliseconds(15)); + EXPECT_EQ(wait_done, false) << "Error, wait terminated to early"; + + ret = rcl_set_ros_time_override(&clock, RCL_MS_TO_NS(15)); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + EXPECT_EQ(wait_done, true) << "Error, wait waken up by time jump"; + + t.join(); + + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // We don't expect a timeout here (since the guard condition had already been triggered) + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LT(diff, RCL_MS_TO_NS(100)); +} + // Check that a canceled timer doesn't wake up rcl_wait TEST_F(WaitSetTestFixture, canceled_timer) { rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); From 1b7b743cc4f18446dd97c3e7cae9dd21c63fe012 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 7 Mar 2024 20:43:24 +0100 Subject: [PATCH 3/4] feat: Added rcl_timer_get_next_call_time Signed-off-by: Janosch Machowinski --- rcl/include/rcl/timer.h | 24 ++++++++++++++++++++++++ rcl/src/rcl/timer.c | 16 ++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index 179ae2465..c7f48ebb0 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -352,6 +352,30 @@ RCL_WARN_UNUSED rcl_ret_t rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); +/// Retrieve the time when the next call to rcl_timer_call() shall occur. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[in] timer the handle to the timer that is being queried + * \param[out] next_call_time the output variable for the result + * \return #RCL_RET_OK if the timer until next call was successfully calculated, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_TIMER_CANCELED if the timer is canceled, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_get_next_call_time(const rcl_timer_t * timer, int64_t * next_call_time); + /// Retrieve the time since the previous call to rcl_timer_call() occurred. /** * This function calculates the time since the last call and copies it into diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 75d6e442d..cdf7a238b 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -333,6 +333,22 @@ rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready) return RCL_RET_OK; } +rcl_ret_t +rcl_timer_get_next_call_time(const rcl_timer_t * timer, int64_t * next_call_time) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(next_call_time, RCL_RET_INVALID_ARGUMENT); + + if (rcutils_atomic_load_bool(&timer->impl->canceled)) { + return RCL_RET_TIMER_CANCELED; + } + + *next_call_time = + rcutils_atomic_load_int64_t(&timer->impl->next_call_time); + return RCL_RET_OK; +} + rcl_ret_t rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call) { From dd8ab5744cb8aa0b4ba8468b14bbcf0f84964521 Mon Sep 17 00:00:00 2001 From: Janosch Machowinski Date: Thu, 7 Mar 2024 20:45:25 +0100 Subject: [PATCH 4/4] fix(rcl_wait): Improved timeout computation in case of many timers This commit changes the computation of the timer timeout, to be more precise, in the case, of many registered timers. Signed-off-by: Janosch Machowinski --- rcl/src/rcl/wait.c | 88 ++++++++++++++++++++++++++------------ rcl/test/rcl/test_wait.cpp | 2 - 2 files changed, 60 insertions(+), 30 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 8f46ee8f5..7fc56c5a0 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -541,34 +541,50 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) // By default, set the timer to block indefinitely if none of the below conditions are met. rmw_time_t * timeout_argument = NULL; rmw_time_t temporary_timeout_storage; - bool is_timer_timeout = false; - int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX; - { // scope to prevent i from colliding below - uint64_t i = 0; - for (i = 0; i < wait_set->impl->timer_index; ++i) { - if (!wait_set->timers[i]) { + + for (uint64_t t_idx = 0; t_idx < wait_set->impl->timer_index; ++t_idx) { + if (!wait_set->timers[t_idx]) { + continue; // Skip NULL timers. + } + rmw_guard_conditions_t * rmw_gcs = &(wait_set->impl->rmw_guard_conditions); + size_t gc_idx = wait_set->size_of_guard_conditions + t_idx; + if (NULL != rmw_gcs->guard_conditions[gc_idx]) { + // This timer has a guard condition, so move it to make a legal wait set. + rmw_gcs->guard_conditions[rmw_gcs->guard_condition_count] = + rmw_gcs->guard_conditions[gc_idx]; + ++(rmw_gcs->guard_condition_count); + } + } + + if (timeout == 0) { + // Then it is non-blocking, so set the temporary storage to 0, 0 and pass it. + temporary_timeout_storage.sec = 0; + temporary_timeout_storage.nsec = 0; + timeout_argument = &temporary_timeout_storage; + } else { + int64_t min_next_call_time[RCL_STEADY_TIME + 1]; + rcl_clock_t * clocks[RCL_STEADY_TIME + 1] = {0, 0, 0, 0}; + + min_next_call_time[RCL_ROS_TIME] = INT64_MAX; + min_next_call_time[RCL_SYSTEM_TIME] = INT64_MAX; + min_next_call_time[RCL_STEADY_TIME] = INT64_MAX; + + for (size_t t_idx = 0; t_idx < wait_set->impl->timer_index; ++t_idx) { + if (!wait_set->timers[t_idx]) { continue; // Skip NULL timers. } - rmw_guard_conditions_t * rmw_gcs = &(wait_set->impl->rmw_guard_conditions); - size_t gc_idx = wait_set->size_of_guard_conditions + i; - if (NULL != rmw_gcs->guard_conditions[gc_idx]) { - // This timer has a guard condition, so move it to make a legal wait set. - rmw_gcs->guard_conditions[rmw_gcs->guard_condition_count] = - rmw_gcs->guard_conditions[gc_idx]; - ++(rmw_gcs->guard_condition_count); - } rcl_clock_t * clock; - if (rcl_timer_clock(wait_set->timers[i], &clock) != RCL_RET_OK) { - //should never happen + if (rcl_timer_clock(wait_set->timers[t_idx], &clock) != RCL_RET_OK) { + // should never happen return RCL_RET_ERROR; } if (clock->type == RCL_ROS_TIME) { bool timer_override_active = false; if (rcl_is_enabled_ros_time_override(clock, &timer_override_active) != RCL_RET_OK) { - //should never happen + // should never happen return RCL_RET_ERROR; } @@ -580,29 +596,45 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) } } - // use timer time to to set the rmw_wait timeout - int64_t timer_timeout = INT64_MAX; - rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); + clocks[clock->type] = clock; + + // get the time of the next call to the timer + int64_t next_call_time = INT64_MAX; + rcl_ret_t ret = rcl_timer_get_next_call_time(wait_set->timers[t_idx], &next_call_time); if (ret == RCL_RET_TIMER_CANCELED) { - wait_set->timers[i] = NULL; + wait_set->timers[t_idx] = NULL; + continue; + } + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (next_call_time < min_next_call_time[clock->type]) { + min_next_call_time[clock->type] = next_call_time; + } + } + + int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX; + + // determine the min timeout of all clocks + for (size_t i = RCL_ROS_TIME; i <= RCL_STEADY_TIME; i++) { + if (clocks[i] == 0) { continue; } + + int64_t cur_time; + rmw_ret_t ret = rcl_clock_get_now(clocks[i], &cur_time); if (ret != RCL_RET_OK) { return ret; // The rcl error state should already be set. } + + int64_t timer_timeout = min_next_call_time[i] - cur_time; + if (timer_timeout < min_timeout) { is_timer_timeout = true; min_timeout = timer_timeout; } } - } - if (timeout == 0) { - // Then it is non-blocking, so set the temporary storage to 0, 0 and pass it. - temporary_timeout_storage.sec = 0; - temporary_timeout_storage.nsec = 0; - timeout_argument = &temporary_timeout_storage; - } else if (timeout > 0 || is_timer_timeout) { // If min_timeout was negative, we need to wake up immediately. if (min_timeout < 0) { min_timeout = 0; diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index b69c89415..fe5015f9d 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -323,7 +323,6 @@ TEST_F(WaitSetTestFixture, zero_timeout_overrun_timer) { // Test rcl_wait with a timeout value and an overrun timer TEST_F(WaitSetTestFixture, no_wakeup_on_override_timer) { - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); @@ -371,7 +370,6 @@ TEST_F(WaitSetTestFixture, no_wakeup_on_override_timer) { // Test rcl_wait with a timeout value and an overrun timer TEST_F(WaitSetTestFixture, wakeup_on_override_timer) { - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator());