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.

Sign up for GitHub

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

Jump to bottom

Trigger guard condition when timer is reset #589

Merged
merged 5 commits into from
Mar 13, 2020
Merged

Trigger guard condition when timer is reset #589

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
3b64150
Fix malfunction when reset canceled timer
DongheeYe Mar 9, 2020
36d9c06
Apply comment : remove notify guard condition and always add current …
DongheeYe Mar 10, 2020
17d1cec
Apply comment: Always trigger gc when timer reset
DongheeYe Mar 10, 2020
e725ff6
Apply comment: change comment of guard condition
DongheeYe Mar 11, 2020
a78d199
Fix test error: uncrustify
DongheeYe Mar 13, 2020
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: 16 additions & 0 deletions rcl/include/rcl/timer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -588,6 +588,22 @@ RCL_WARN_UNUSED
rcl_guard_condition_t *
rcl_timer_get_guard_condition(const rcl_timer_t * timer);

/// Retrieve a guard condition used by the timer to wake the waitset when rest.
/**
* <hr>
* 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_guard_condition_t *
rcl_timer_get_notify_guard_condition(const rcl_timer_t * timer);
#ifdef __cplusplus
}
#endif
Expand Down
44 changes: 44 additions & 0 deletions rcl/src/rcl/timer.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@ typedef struct rcl_timer_impl_t
atomic_bool canceled;
// The user supplied allocator.
rcl_allocator_t allocator;
// A guard condition used to wake a wait set to reset cancelled timer.
rcl_guard_condition_t notify_guard_condition;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't see a reason to use a different guard condition.
Guard conditions just wakes the wait set, it isn't used to check if the timer is ready or not.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't see a reason to use a different guard condition.
Guard conditions just wakes the wait set, it isn't used to check if the timer is ready or not.

I removed notify_guard_condition and always add a guard condition of timer to node wait set.

} rcl_timer_impl_t;

rcl_timer_t
Expand Down Expand Up @@ -173,6 +175,24 @@ rcl_timer_init(
return ret;
}
}

impl.notify_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.notify_guard_condition), context, options);
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");
}

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");
}
return ret;
}

atomic_init(&impl.callback, (uintptr_t)callback);
atomic_init(&impl.period, period);
atomic_init(&impl.time_credit, 0);
Expand All @@ -186,6 +206,10 @@ rcl_timer_init(
// Should be impossible
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini guard condition after bad alloc");
}
if (RCL_RET_OK != rcl_guard_condition_fini(&(impl.notify_guard_condition))) {
// Should be impossible
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini notify 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");
Expand All @@ -212,6 +236,10 @@ rcl_timer_fini(rcl_timer_t * timer)
if (RCL_RET_OK != fail_ret) {
RCL_SET_ERROR_MSG("Failure to fini guard condition");
}
fail_ret = rcl_guard_condition_fini(&(timer->impl->notify_guard_condition));
if (RCL_RET_OK != fail_ret) {
RCL_SET_ERROR_MSG("Failure to fini notify guard condition");
}
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) {
Expand Down Expand Up @@ -402,7 +430,14 @@ 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);
bool is_canceled;
is_canceled = rcutils_atomic_load_bool(&timer->impl->canceled);
rcutils_atomic_store(&timer->impl->canceled, false);
if (is_canceled) {
ivanpauno marked this conversation as resolved.
Show resolved Hide resolved
rcl_ret_t ret = rcl_trigger_guard_condition(&timer->impl->notify_guard_condition);
if (ret != RCL_RET_OK)
RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to trigger timer notify guard condition");
}
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset");
return RCL_RET_OK;
}
Expand All @@ -424,6 +459,15 @@ rcl_timer_get_guard_condition(const rcl_timer_t * timer)
return &timer->impl->guard_condition;
}

rcl_guard_condition_t *
rcl_timer_get_notify_guard_condition(const rcl_timer_t * timer)
{
if (NULL == timer || NULL == timer->impl || NULL == timer->impl->notify_guard_condition.impl) {
return NULL;
}
return &timer->impl->notify_guard_condition;
}

#ifdef __cplusplus
}
#endif
31 changes: 23 additions & 8 deletions rcl/src/rcl/wait.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -393,7 +393,7 @@ rcl_wait_set_resize(

// 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;
const size_t num_rmw_gc = guard_conditions_size + timers_size * 2;
// Clear added guard conditions
rmw_gcs->guard_condition_count = 0u;
if (0u == num_rmw_gc) {
Expand Down Expand Up @@ -468,15 +468,24 @@ rcl_wait_set_add_timer(
size_t * index)
{
SET_ADD(timer)
// rcl_wait() will take care of moving these backwards and setting guard_condition_count.
const size_t gc_index = wait_set->size_of_guard_conditions + (wait_set->impl->timer_index - 1) * 2;

rcl_guard_condition_t * notify_guard_condition = rcl_timer_get_notify_guard_condition(timer);
if (NULL != notify_guard_condition) {
rmw_guard_condition_t * rmw_handle = rcl_guard_condition_get_rmw_handle(notify_guard_condition);
RCL_CHECK_FOR_NULL_WITH_MSG(
rmw_handle, rcl_get_error_string().str, return RCL_RET_ERROR);
wait_set->impl->rmw_guard_conditions.guard_conditions[gc_index] = rmw_handle->data;
}

// 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().str, return RCL_RET_ERROR);
wait_set->impl->rmw_guard_conditions.guard_conditions[index] = rmw_handle->data;
wait_set->impl->rmw_guard_conditions.guard_conditions[gc_index + 1] = rmw_handle->data;
}
return RCL_RET_OK;
}
Expand Down Expand Up @@ -552,16 +561,22 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set.
}
rmw_guard_conditions_t * rmw_gcs = &(wait_set->impl->rmw_guard_conditions);
size_t gc_idx = wait_set->size_of_guard_conditions + i * 2;
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);
}
if (is_canceled) {
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]) {
if (NULL != rmw_gcs->guard_conditions[gc_idx + 1]) {
// 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_conditions[gc_idx + 1];
++(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
Expand Down