Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Trigger guard condition when timer is reset #589

Merged
merged 5 commits into from
Mar 13, 2020
Merged
Show file tree
Hide file tree
Changes from 4 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 10 additions & 6 deletions rcl/src/rcl/timer.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -403,6 +404,9 @@ 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;
}
Expand Down
16 changes: 8 additions & 8 deletions rcl/src/rcl/wait.c
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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);
}
wjwwood marked this conversation as resolved.
Show resolved Hide resolved
// 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;
Expand Down