From ed9a7f9ea464b803fac74ca86eaf55cfb63a7455 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 17 Aug 2018 15:17:45 -0700 Subject: [PATCH 1/3] rcl_wait() on timers with ROS clock Timers handle time jump callbacks rcl_wait() wakes when ros time gets a time after timer's next call time --- rcl/include/rcl/timer.h | 20 ++++ rcl/src/rcl/timer.c | 153 ++++++++++++++++++++++--- rcl/src/rcl/wait.c | 97 ++++++++++------ rcl/test/rcl/test_timer.cpp | 222 ++++++++++++++++++++++++++++++++++++ 4 files changed, 443 insertions(+), 49 deletions(-) diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index 7a57dfbcb..35238e64f 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -23,6 +23,7 @@ extern "C" #include #include "rcl/allocator.h" +#include "rcl/guard_condition.h" #include "rcl/macros.h" #include "rcl/time.h" #include "rcl/types.h" @@ -564,6 +565,25 @@ rcl_timer_reset(rcl_timer_t * timer); const rcl_allocator_t * rcl_timer_get_allocator(const rcl_timer_t * timer); +/// Retrieve a guard condition used by the timer to wake the waitset when using ROSTime. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] timer the timer to be queried + * \return `NULL` if the timer is invalid or does not have a guard condition, or + * \return a guard condition pointer. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_guard_condition_t * +rcl_timer_get_guard_condition(const rcl_timer_t * timer); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index ccad04739..b07f02554 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -30,14 +30,18 @@ typedef struct rcl_timer_impl_t { // The clock providing time. rcl_clock_t * clock; + // A guard condition used to wake a wait set if using ROSTime, else zero initialized. + rcl_guard_condition_t guard_condition; // The user supplied callback. atomic_uintptr_t callback; // This is a duration in nanoseconds. atomic_uint_least64_t period; // This is a time in nanoseconds since an unspecified time. - atomic_uint_least64_t last_call_time; + atomic_int_least64_t last_call_time; // This is a time in nanoseconds since an unspecified time. - atomic_uint_least64_t next_call_time; + atomic_int_least64_t next_call_time; + // Credit for time elapsed before ROS time is activated or deactivated. + atomic_int_least64_t time_credit; // A flag which indicates if the timer is canceled. atomic_bool canceled; // The user supplied allocator. @@ -51,6 +55,70 @@ rcl_get_zero_initialized_timer() return null_timer; } +void _rcl_timer_time_jump( + const struct rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data) +{ + rcl_timer_t * timer = (rcl_timer_t *)user_data; + + if (before_jump) { + if (RCL_ROS_TIME_ACTIVATED == time_jump->clock_change || + RCL_ROS_TIME_DEACTIVATED == time_jump->clock_change) + { + rcl_time_point_value_t now; + if (RCL_RET_OK != rcl_clock_get_now(timer->impl->clock, &now)) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get current time in jump callback"); + return; + } + // Source of time is changing, but the timer has ellapsed some portion of its period + // Save ellapsed duration pre jump so the timer only waits the remainder in the new epoch + if (0 == now) { + // No time credit if clock is uninitialized + return; + } + const int64_t next_call_time = rcl_atomic_load_int64_t(&timer->impl->next_call_time); + rcl_atomic_store(&timer->impl->time_credit, next_call_time - now); + } + } else { + rcl_time_point_value_t now; + if (RCL_RET_OK != rcl_clock_get_now(timer->impl->clock, &now)) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get current time in jump callback"); + return; + } + const int64_t last_call_time = rcl_atomic_load_int64_t(&timer->impl->last_call_time); + const int64_t next_call_time = rcl_atomic_load_int64_t(&timer->impl->next_call_time); + const int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); + if (RCL_ROS_TIME_ACTIVATED == time_jump->clock_change || + RCL_ROS_TIME_DEACTIVATED == time_jump->clock_change) + { + // ROS time activated or deactivated + if (0 == now) { + // Can't apply time credit if clock is uninitialized + return; + } + int64_t time_credit = rcl_atomic_exchange_int64_t(&timer->impl->time_credit, 0); + if (time_credit) { + // set times in new epoch so timer only waits the remainder of the period + rcl_atomic_store(&timer->impl->next_call_time, now - time_credit + period); + rcl_atomic_store(&timer->impl->last_call_time, now - time_credit); + } + } else if (next_call_time <= now) { + // Post Forward jump and timer is ready + if (RCL_RET_OK != rcl_trigger_guard_condition(&timer->impl->guard_condition)) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to get trigger guard condition in jump callback"); + } + } else if (now < last_call_time) { + // Post backwards time jump that went further back than 1 period + // next callback should happen after 1 period + rcl_atomic_store(&timer->impl->next_call_time, now + period); + rcl_atomic_store(&timer->impl->last_call_time, now); + return; + } + } +} + rcl_ret_t rcl_timer_init( rcl_timer_t * timer, @@ -78,15 +146,48 @@ rcl_timer_init( } rcl_timer_impl_t impl; impl.clock = clock; + impl.guard_condition = rcl_get_zero_initialized_guard_condition(); + if (RCL_ROS_TIME == impl.clock->type) { + rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); + rcl_ret_t ret = rcl_guard_condition_init(&(impl.guard_condition), options); + if (RCL_RET_OK != ret) { + return ret; + } + rcl_jump_threshold_t threshold; + threshold.on_clock_change = true; + threshold.min_forward.nanoseconds = 1; + threshold.min_backward.nanoseconds = -1; + ret = rcl_clock_add_jump_callback(clock, threshold, _rcl_timer_time_jump, timer); + if (RCL_RET_OK != ret) { + if (RCL_RET_OK != rcl_guard_condition_fini(&(impl.guard_condition))) { + // Should be impossible + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to fini guard condition after failing to add jump callback"); + } + return ret; + } + } atomic_init(&impl.callback, (uintptr_t)callback); atomic_init(&impl.period, period); + atomic_init(&impl.time_credit, 0); atomic_init(&impl.last_call_time, now); atomic_init(&impl.next_call_time, now + period); atomic_init(&impl.canceled, false); impl.allocator = allocator; timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state); - RCL_CHECK_FOR_NULL_WITH_MSG( - timer->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC, allocator); + if (NULL == timer->impl) { + if (RCL_RET_OK != rcl_guard_condition_fini(&(impl.guard_condition))) { + // Should be impossible + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini guard condition after bad alloc"); + } + if (RCL_RET_OK != rcl_clock_remove_jump_callback(clock, _rcl_timer_time_jump, timer)) { + // Should be impossible + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to remove callback after bad alloc"); + } + + RCL_SET_ERROR_MSG("allocating memory failed", allocator); + return RCL_RET_BAD_ALLOC; + } *timer->impl = impl; return RCL_RET_OK; } @@ -100,6 +201,16 @@ rcl_timer_fini(rcl_timer_t * timer) // Will return either RCL_RET_OK or RCL_RET_ERROR since the timer is valid. rcl_ret_t result = rcl_timer_cancel(timer); rcl_allocator_t allocator = timer->impl->allocator; + rcl_ret_t fail_ret = rcl_guard_condition_fini(&(timer->impl->guard_condition)); + if (RCL_RET_OK != fail_ret) { + RCL_SET_ERROR_MSG("Failure to fini guard condition", allocator); + } + if (RCL_ROS_TIME == timer->impl->clock->type) { + fail_ret = rcl_clock_remove_jump_callback(timer->impl->clock, _rcl_timer_time_jump, timer); + if (RCL_RET_OK != fail_ret) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to remove timer jump callback"); + } + } allocator.deallocate(timer->impl, allocator.state); return result; } @@ -138,28 +249,27 @@ rcl_timer_call(rcl_timer_t * timer) RCL_SET_ERROR_MSG("clock now returned negative time point value", *allocator); return RCL_RET_ERROR; } - uint64_t unow = (uint64_t)now; rcl_time_point_value_t previous_ns = - rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, unow); + rcl_atomic_exchange_int64_t(&timer->impl->last_call_time, now); rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback); - uint64_t next_call_time = rcl_atomic_load_uint64_t(&timer->impl->next_call_time); - uint64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); + int64_t next_call_time = rcl_atomic_load_int64_t(&timer->impl->next_call_time); + int64_t period = rcl_atomic_load_uint64_t(&timer->impl->period); // always move the next call time by exactly period forward // don't use now as the base to avoid extending each cycle by the time // between the timer being ready and the callback being triggered next_call_time += period; // in case the timer has missed at least once cycle - if (next_call_time < unow) { + if (next_call_time < now) { if (0 == period) { // a timer with a period of zero is considered always ready - next_call_time = unow; + next_call_time = now; } else { // move the next call time forward by as many periods as necessary - uint64_t now_ahead = unow - next_call_time; + int64_t now_ahead = now - next_call_time; // rounding up without overflow - uint64_t periods_ahead = 1 + (now_ahead - 1) / period; + int64_t periods_ahead = 1 + (now_ahead - 1) / period; next_call_time += periods_ahead * period; } } @@ -200,12 +310,12 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt } RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT, *allocator); rcl_time_point_value_t now; - rcl_ret_t ret = rcutils_steady_time_now(&now); + rcl_ret_t ret = rcl_clock_get_now(timer->impl->clock, &now); if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } *time_until_next_call = - rcl_atomic_load_uint64_t(&timer->impl->next_call_time) - now; + rcl_atomic_load_int64_t(&timer->impl->next_call_time) - now; return RCL_RET_OK; } @@ -221,12 +331,12 @@ rcl_timer_get_time_since_last_call( } RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT, *allocator); rcl_time_point_value_t now; - rcl_ret_t ret = rcutils_steady_time_now(&now); + rcl_ret_t ret = rcl_clock_get_now(timer->impl->clock, &now); if (ret != RCL_RET_OK) { return ret; // rcl error state should already be set. } *time_since_last_call = - now - rcl_atomic_load_uint64_t(&timer->impl->last_call_time); + now - rcl_atomic_load_int64_t(&timer->impl->last_call_time); return RCL_RET_OK; } @@ -310,7 +420,7 @@ rcl_timer_reset(rcl_timer_t * timer) RCL_CHECK_FOR_NULL_WITH_MSG( timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID, rcl_get_default_allocator()); rcl_time_point_value_t now; - rcl_ret_t now_ret = rcutils_steady_time_now(&now); + rcl_ret_t now_ret = rcl_clock_get_now(timer->impl->clock, &now); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } @@ -330,6 +440,15 @@ rcl_timer_get_allocator(const rcl_timer_t * timer) return &timer->impl->allocator; } +rcl_guard_condition_t * +rcl_timer_get_guard_condition(const rcl_timer_t * timer) +{ + if (NULL == timer || NULL == timer->impl || NULL == timer->impl->guard_condition.impl) { + return NULL; + } + return &timer->impl->guard_condition; +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 4d7b73493..abc1f60b8 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -354,16 +354,39 @@ rcl_wait_set_resize( SET_RESIZE_RMW_REALLOC( subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count) ); - SET_RESIZE( - guard_condition, - SET_RESIZE_RMW_DEALLOC( - rmw_guard_conditions.guard_conditions, - rmw_guard_conditions.guard_condition_count), - SET_RESIZE_RMW_REALLOC( - guard_condition, - rmw_guard_conditions.guard_conditions, - rmw_guard_conditions.guard_condition_count) - ); + // Guard condition RCL size is the resize amount given + SET_RESIZE(guard_condition,;,;); // NOLINT + + // Guard condition RMW size needs to be guard conditions + timers + rmw_guard_conditions_t * rmw_gcs = &(wait_set->impl->rmw_guard_conditions); + const size_t num_rmw_gc = guard_conditions_size + timers_size; + // Clear added guard conditions + rmw_gcs->guard_condition_count = 0u; + if (0u == num_rmw_gc) { + if (rmw_gcs->guard_conditions) { + wait_set->impl->allocator.deallocate( + (void *)rmw_gcs->guard_conditions, wait_set->impl->allocator.state); + rmw_gcs->guard_conditions = NULL; + } + } else { + rmw_gcs->guard_conditions = (void **)wait_set->impl->allocator.reallocate( + rmw_gcs->guard_conditions, sizeof(void *) * num_rmw_gc, wait_set->impl->allocator.state); + if (!rmw_gcs->guard_conditions) { + // Deallocate rcl arrays to match unallocated rmw guard conditions + wait_set->impl->allocator.deallocate( + (void *)wait_set->guard_conditions, wait_set->impl->allocator.state); + wait_set->size_of_guard_conditions = 0u; + wait_set->guard_conditions = NULL; + wait_set->impl->allocator.deallocate( + (void *)wait_set->timers, wait_set->impl->allocator.state); + wait_set->size_of_timers = 0u; + wait_set->timers = NULL; + RCL_SET_ERROR_MSG("allocating memory failed", wait_set->impl->allocator); + return RCL_RET_BAD_ALLOC; + } + memset(rmw_gcs->guard_conditions, 0, sizeof(void *) * num_rmw_gc); + } + SET_RESIZE(timer,;,;); // NOLINT SET_RESIZE(client, SET_RESIZE_RMW_DEALLOC( @@ -397,6 +420,16 @@ rcl_wait_set_add_timer( const rcl_timer_t * timer) { SET_ADD(timer) + // Add timer guard conditions to end of rmw guard condtion set. + rcl_guard_condition_t * guard_condition = rcl_timer_get_guard_condition(timer); + if (NULL != guard_condition) { + // rcl_wait() will take care of moving these backwards and setting guard_condition_count. + const size_t index = wait_set->size_of_guard_conditions + (wait_set->impl->timer_index - 1); + rmw_guard_condition_t * rmw_handle = rcl_guard_condition_get_rmw_handle(guard_condition); + RCL_CHECK_FOR_NULL_WITH_MSG( + rmw_handle, rcl_get_error_string_safe(), return RCL_RET_ERROR, wait_set->impl->allocator); + wait_set->impl->rmw_guard_conditions.guard_conditions[index] = rmw_handle->data; + } return RCL_RET_OK; } @@ -443,6 +476,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) 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; // 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 @@ -460,38 +495,36 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) if (is_canceled) { number_of_valid_timers--; wait_set->timers[i] = NULL; + continue; + } + 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); + } else { + // No guard condition, instead use 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); + 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; + } } } } - 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; - 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; diff --git a/rcl/test/rcl/test_timer.cpp b/rcl/test/rcl/test_timer.cpp index 66061a8bf..eea790620 100644 --- a/rcl/test/rcl/test_timer.cpp +++ b/rcl/test/rcl/test_timer.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include "rcl/timer.h" @@ -246,3 +248,223 @@ TEST_F(TestTimerFixture, test_canceled_timer) { ret = rcl_clock_fini(&clock); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); } + +TEST_F(TestTimerFixture, test_rostime_time_until_next_call) { + rcl_ret_t ret; + const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; + int64_t time_until = 0; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe(); + }); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe(); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe(); + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe(); + ret = rcl_timer_get_time_until_next_call(&timer, &time_until); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + EXPECT_EQ(sec_5 - 1, time_until); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5)) << rcl_get_error_string_safe(); + ret = rcl_timer_get_time_until_next_call(&timer, &time_until); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + EXPECT_EQ(0, time_until); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5 + 1)) << + rcl_get_error_string_safe(); + ret = rcl_timer_get_time_until_next_call(&timer, &time_until); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + EXPECT_EQ(-1, time_until); +} + +TEST_F(TestTimerFixture, test_system_time_to_ros_time) { + rcl_ret_t ret; + const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe(); + }); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe(); + }); + + int64_t time_until_pre = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until_pre)) << + rcl_get_error_string_safe(); + ASSERT_LT(0, time_until_pre); + ASSERT_GT(sec_5, time_until_pre); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe(); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe(); + + int64_t time_until = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) << + rcl_get_error_string_safe(); + // Because of time credit the time until next call should be less than before + EXPECT_GT(time_until_pre, time_until); + EXPECT_LT(0, time_until); +} + +TEST_F(TestTimerFixture, test_ros_time_to_system_time) { + rcl_ret_t ret; + const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; + const int64_t sec_1 = 1000LL * 1000LL * 1000LL; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe(); + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe(); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe(); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe(); + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe(); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe(); + + int64_t time_until_pre = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until_pre)) << + rcl_get_error_string_safe(); + ASSERT_EQ(sec_5 - (sec_1 - 1), time_until_pre); + + ASSERT_EQ(RCL_RET_OK, rcl_disable_ros_time_override(&clock)) << rcl_get_error_string_safe(); + + int64_t time_until = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) << + rcl_get_error_string_safe(); + // Because of time credit the time until next call should be less than before + EXPECT_GT(time_until_pre, time_until); + EXPECT_LT(0, time_until); +} + +TEST_F(TestTimerFixture, test_ros_time_backwards_jump) { + rcl_ret_t ret; + const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; + const int64_t sec_3 = 3LL * 1000LL * 1000LL * 1000LL; + const int64_t sec_2 = 2LL * 1000LL * 1000LL * 1000LL; + const int64_t sec_1 = 1000LL * 1000LL * 1000LL; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe(); + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_2)) << rcl_get_error_string_safe(); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe(); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe(); + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_3)) << rcl_get_error_string_safe(); + { + // Moved forward a little bit, timer should be closer to being ready + int64_t time_until = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) << + rcl_get_error_string_safe(); + EXPECT_EQ(sec_5 - (sec_3 - sec_2), time_until); + } + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe(); + { + // Jumped back before timer was created, so last_call_time should be 1 period + int64_t time_until = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) << + rcl_get_error_string_safe(); + EXPECT_EQ(sec_5, time_until); + } +} + +TEST_F(TestTimerFixture, test_ros_time_wakes_wait) { + const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; + const int64_t sec_1 = 1000LL * 1000LL * 1000LL; + const int64_t sec_1_5 = 1500LL * 1000LL * 1000LL; + + rcl_ret_t ret; + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ASSERT_EQ(RCL_RET_OK, rcl_clock_init(RCL_ROS_TIME, &clock, &allocator)) << + rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe(); + }); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe(); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe(); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init(&timer, &clock, sec_1, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe(); + }); + + bool timer_was_ready = false; + + std::thread wait_thr([&](void) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + + ASSERT_EQ(RCL_RET_OK, rcl_wait_set_add_timer(&wait_set, &timer)) << + rcl_get_error_string_safe(); + // *INDENT-OFF* (Uncrustify wants strange un-indentation here) + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << + rcl_get_error_string_safe(); + }); + // *INDENT-ON* + + ret = rcl_wait(&wait_set, sec_5); + // set some flag indicating wait was exited + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); + if (wait_set.timers[0] != NULL) { + timer_was_ready = true; + } + }); + + // Timer not exceeded, should not wake + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1_5)) << + rcl_get_error_string_safe(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_FALSE(timer_was_ready); + + // Timer exceeded, should wake + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5)) << rcl_get_error_string_safe(); + auto start = std::chrono::steady_clock::now(); + wait_thr.join(); + auto finish = std::chrono::steady_clock::now(); + EXPECT_TRUE(timer_was_ready); + EXPECT_LT(finish - start, std::chrono::milliseconds(100)); +} From ae43ec8029023a537d04daa31feea793efd88c94 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Fri, 7 Sep 2018 14:49:57 -0700 Subject: [PATCH 2/3] Set timer impl to NULL after fini --- rcl/src/rcl/timer.c | 1 + 1 file changed, 1 insertion(+) diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index b07f02554..d2fd1eb27 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -212,6 +212,7 @@ rcl_timer_fini(rcl_timer_t * timer) } } allocator.deallocate(timer->impl, allocator.state); + timer->impl = NULL; return result; } From 0520282a895f4cdb8dc90b8c0f6835a1fb77667a Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Wed, 12 Sep 2018 15:36:10 -0700 Subject: [PATCH 3/3] Use RCL_S_TO_NS --- rcl/test/rcl/test_timer.cpp | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/rcl/test/rcl/test_timer.cpp b/rcl/test/rcl/test_timer.cpp index eea790620..918b75c94 100644 --- a/rcl/test/rcl/test_timer.cpp +++ b/rcl/test/rcl/test_timer.cpp @@ -251,7 +251,7 @@ TEST_F(TestTimerFixture, test_canceled_timer) { TEST_F(TestTimerFixture, test_rostime_time_until_next_call) { rcl_ret_t ret; - const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; + const int64_t sec_5 = RCL_S_TO_NS(5); int64_t time_until = 0; rcl_clock_t clock; @@ -289,7 +289,7 @@ TEST_F(TestTimerFixture, test_rostime_time_until_next_call) { TEST_F(TestTimerFixture, test_system_time_to_ros_time) { rcl_ret_t ret; - const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; + const int64_t sec_5 = RCL_S_TO_NS(5); rcl_clock_t clock; rcl_allocator_t allocator = rcl_get_default_allocator(); @@ -325,8 +325,8 @@ TEST_F(TestTimerFixture, test_system_time_to_ros_time) { TEST_F(TestTimerFixture, test_ros_time_to_system_time) { rcl_ret_t ret; - const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; - const int64_t sec_1 = 1000LL * 1000LL * 1000LL; + const int64_t sec_5 = RCL_S_TO_NS(5); + const int64_t sec_1 = RCL_S_TO_NS(1); rcl_clock_t clock; rcl_allocator_t allocator = rcl_get_default_allocator(); @@ -366,10 +366,10 @@ TEST_F(TestTimerFixture, test_ros_time_to_system_time) { TEST_F(TestTimerFixture, test_ros_time_backwards_jump) { rcl_ret_t ret; - const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; - const int64_t sec_3 = 3LL * 1000LL * 1000LL * 1000LL; - const int64_t sec_2 = 2LL * 1000LL * 1000LL * 1000LL; - const int64_t sec_1 = 1000LL * 1000LL * 1000LL; + const int64_t sec_5 = RCL_S_TO_NS(5); + const int64_t sec_3 = RCL_S_TO_NS(3); + const int64_t sec_2 = RCL_S_TO_NS(2); + const int64_t sec_1 = RCL_S_TO_NS(1); rcl_clock_t clock; rcl_allocator_t allocator = rcl_get_default_allocator(); @@ -408,9 +408,9 @@ TEST_F(TestTimerFixture, test_ros_time_backwards_jump) { } TEST_F(TestTimerFixture, test_ros_time_wakes_wait) { - const int64_t sec_5 = 5LL * 1000LL * 1000LL * 1000LL; - const int64_t sec_1 = 1000LL * 1000LL * 1000LL; - const int64_t sec_1_5 = 1500LL * 1000LL * 1000LL; + const int64_t sec_5 = RCL_S_TO_NS(5); + const int64_t sec_1 = RCL_S_TO_NS(1); + const int64_t sec_1_5 = RCL_S_TO_NS(3) / 2; rcl_ret_t ret; rcl_clock_t clock;