niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Trigger guard condition when timer is reset (#589)

Browse source 
Signed-off-by: Donghee Ye <donghee.ye@samsung.com>
This commit is contained in:
DongheeYe 2020-03-14 02:23:49 +09:00 • committed by GitHub
parent b463168528
commit 069d1f0729
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
2 changed files with 19 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

9
rcl/src/rcl/timer.c
View file

@ -33,7 +33,8 @@ typedef struct rcl_timer_impl_t
rcl_clock_t * clock; rcl_clock_t * clock;
// The associated context. // The associated context.
rcl_context_t * 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; rcl_guard_condition_t guard_condition;
// The user supplied callback. // The user supplied callback.
atomic_uintptr_t callback; atomic_uintptr_t callback;
@ -153,12 +154,12 @@ rcl_timer_init(
impl.clock = clock; impl.clock = clock;
impl.context = context; impl.context = context;
impl.guard_condition = rcl_get_zero_initialized_guard_condition(); 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_guard_condition_options_t options = rcl_guard_condition_get_default_options();
rcl_ret_t ret = rcl_guard_condition_init(&(impl.guard_condition), context, options); rcl_ret_t ret = rcl_guard_condition_init(&(impl.guard_condition), context, options);
if (RCL_RET_OK != ret) { if (RCL_RET_OK != ret) {
return ret; return ret;
} }
if (RCL_ROS_TIME == impl.clock->type) {
rcl_jump_threshold_t threshold; rcl_jump_threshold_t threshold;
threshold.on_clock_change = true; threshold.on_clock_change = true;
threshold.min_forward.nanoseconds = 1; 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); 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->next_call_time, now + period);
rcutils_atomic_store(&timer->impl->canceled, false); 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"); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset");
return RCL_RET_OK; return RCL_RET_OK;
} }

16
rcl/src/rcl/wait.c
View file

@ -547,6 +547,14 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
if (!wait_set->timers[i]) { if (!wait_set->timers[i]) {
continue; // Skip NULL timers. 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; bool is_canceled = false;
rcl_ret_t ret = rcl_timer_is_canceled(wait_set->timers[i], &is_canceled); rcl_ret_t ret = rcl_timer_is_canceled(wait_set->timers[i], &is_canceled);
if (ret != RCL_RET_OK) { 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; wait_set->timers[i] = NULL;
continue; 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 // use timer time to to set the rmw_wait timeout
// TODO(sloretz) fix spurious wake-ups on ROS_TIME timers with ROS_TIME enabled // TODO(sloretz) fix spurious wake-ups on ROS_TIME timers with ROS_TIME enabled
int64_t timer_timeout = INT64_MAX; int64_t timer_timeout = INT64_MAX;