diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index df3c210370..9d856b2cf7 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -33,7 +33,8 @@ typedef struct rcl_timer_impl_t rcl_clock_t * clock; // The associated context. rcl_context_t * context; - // A guard condition used to wake a wait set if using ROSTime, else zero initialized. + // A guard condition used to wake the associated wait set, either when + // ROSTime causes the timer to expire or when the timer is reset. rcl_guard_condition_t guard_condition; // The user supplied callback. atomic_uintptr_t callback; @@ -153,12 +154,12 @@ rcl_timer_init( impl.clock = clock; impl.context = context; impl.guard_condition = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); + rcl_ret_t ret = rcl_guard_condition_init(&(impl.guard_condition), context, options); + if (RCL_RET_OK != ret) { + return ret; + } 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), context, options); - if (RCL_RET_OK != ret) { - return ret; - } rcl_jump_threshold_t threshold; threshold.on_clock_change = true; threshold.min_forward.nanoseconds = 1; @@ -403,6 +404,10 @@ rcl_timer_reset(rcl_timer_t * timer) int64_t period = rcutils_atomic_load_uint64_t(&timer->impl->period); rcutils_atomic_store(&timer->impl->next_call_time, now + period); rcutils_atomic_store(&timer->impl->canceled, false); + rcl_ret_t ret = rcl_trigger_guard_condition(&timer->impl->guard_condition); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to trigger timer guard condition"); + } RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset"); return RCL_RET_OK; } diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index f0d0d2cc3f..79556df9f4 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -547,6 +547,14 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) if (!wait_set->timers[i]) { 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); + } bool is_canceled = false; rcl_ret_t ret = rcl_timer_is_canceled(wait_set->timers[i], &is_canceled); if (ret != RCL_RET_OK) { @@ -556,14 +564,6 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) 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); - } // 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;