From 1fa4acac8ca42f61bd0f80c7d3ee5fba3d196e33 Mon Sep 17 00:00:00 2001 From: Mikael Arguedas Date: Fri, 8 Sep 2017 10:22:14 -0700 Subject: [PATCH] Timer tests (#163) * dont return timeout errcode if we didnt hit user specified timeout * update axisting test accordingly * add timer test * Revert "dont return timeout errcode if we didnt hit user specified timeout" This reverts commit f44a93b8528e87ce46594af0d1184c116ade0cb4. * alternative fix to not return timeout when timer is ready this also fixes a bug where the wait would always return when all the timers were canceled fixes rclcpp#319 * fixups * more tests * another one just for fun * remove new tests * clearer variable name * update comment --- rcl/src/rcl/wait.c | 77 ++++++++++++++++++++++++-------------- rcl/test/rcl/test_wait.cpp | 2 +- 2 files changed, 50 insertions(+), 29 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index a549017..f398190 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -528,18 +528,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) rmw_time_t * timeout_argument = NULL; rmw_time_t temporary_timeout_storage; - 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 || wait_set->size_of_timers > 0) { - int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX; - // Compare the timeout to the time until next callback for each timer. - // Take the lowest and use that for the wait timeout. + // calculate the number of valid (non-NULL and non-canceled) timers + size_t number_of_valid_timers = wait_set->size_of_timers; + { // 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]) { + number_of_valid_timers--; continue; // Skip NULL timers. } bool is_canceled = false; @@ -548,18 +543,40 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) return ret; // The rcl error state should already be set. } if (is_canceled) { - continue; // Skip canceled timers. + number_of_valid_timers--; + wait_set->timers[i] = NULL; } + } + } + + bool is_timer_timeout = false; + 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 || number_of_valid_timers > 0) { + int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX; + // Compare the timeout to the time until next callback for each timer. + // Take the lowest and use that for the wait timeout. + uint64_t i = 0; + for (i = 0; i < wait_set->impl->timer_index; ++i) { + if (!wait_set->timers[i]) { + continue; // Skip NULL timers. + } + // at this point we know any non-NULL timers are also not canceled int64_t timer_timeout = INT64_MAX; - ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); + rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); if (ret != RCL_RET_OK) { return ret; // The rcl error state should already be set. } if (timer_timeout < min_timeout) { + is_timer_timeout = true; min_timeout = timer_timeout; } } + // If min_timeout was negative, we need to wake up immediately. if (min_timeout < 0) { min_timeout = 0; @@ -577,7 +594,23 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) &wait_set->impl->rmw_clients, wait_set->impl->rmw_waitset, timeout_argument); - // Check for timeout. + // Check for ready timers next + // and set not ready timers (which includes canceled timers) to NULL. + size_t i; + for (i = 0; i < wait_set->impl->timer_index; ++i) { + if (!wait_set->timers[i]) { + continue; + } + bool is_ready = false; + rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready); + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (!is_ready) { + wait_set->timers[i] = NULL; + } + } + // Check for timeout, return RCL_RET_TIMEOUT only if it wasn't a timer. if (ret == RMW_RET_TIMEOUT) { // Assume none were set (because timeout was reached first), and clear all. rcl_ret_t rcl_ret; @@ -591,25 +624,13 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. rcl_ret = rcl_wait_set_clear_clients(wait_set); assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. - return RCL_RET_TIMEOUT; - } - // Check for error. - if (ret != RMW_RET_OK) { + if (!is_timer_timeout) { + return RCL_RET_TIMEOUT; + } + } else if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), wait_set->impl->allocator); return RCL_RET_ERROR; } - // Check for ready timers next, and set not ready timers to NULL. - size_t i; - for (i = 0; i < wait_set->impl->timer_index; ++i) { - bool is_ready = false; - rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready); - if (ret != RCL_RET_OK) { - return ret; // The rcl error state should already be set. - } - if (!is_ready) { - wait_set->timers[i] = NULL; - } - } // Set corresponding rcl subscription handles NULL. for (i = 0; i < wait_set->size_of_subscriptions; ++i) { if (!wait_set->impl->rmw_subscriptions.subscribers[i]) { diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index d45cb8e..ec0c8bc 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -123,7 +123,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) { ret = rcl_wait(&wait_set, timeout); std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); // We expect a timeout here (timer value reached) - ASSERT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string_safe(); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); // Check time int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); EXPECT_LE(diff, RCL_MS_TO_NS(10) + TOLERANCE);