Skip to content

Commit

Permalink
Rcl timer with ros time (#286)
Browse files Browse the repository at this point in the history 
* 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

* Set timer impl to NULL after fini
  • Loading branch information
@sloretz
sloretz authored Sep 13, 2018
1 parent e5e338a commit 0a6d918
Show file tree
Hide file tree
Showing 4 changed files with 444 additions and 49 deletions.
20 changes: 20 additions & 0 deletions rcl/include/rcl/timer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ extern "C"
#include <stdbool.h>

#include "rcl/allocator.h"
#include "rcl/guard_condition.h"
#include "rcl/macros.h"
#include "rcl/time.h"
#include "rcl/types.h"
Expand Down Expand Up @@ -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.
/**
* <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_PUBLIC
RCL_WARN_UNUSED
rcl_guard_condition_t *
rcl_timer_get_guard_condition(const rcl_timer_t * timer);

#ifdef __cplusplus
}
#endif
Expand Down
154 changes: 137 additions & 17 deletions rcl/src/rcl/timer.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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,
Expand Down Expand Up @@ -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;
}
Expand All @@ -100,7 +201,18 @@ 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);
timer->impl = NULL;
return result;
}

Expand Down Expand Up @@ -138,28 +250,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;
}
}
Expand Down Expand Up @@ -200,12 +311,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;
}

Expand All @@ -221,12 +332,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;
}

Expand Down Expand Up @@ -310,7 +421,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.
}
Expand All @@ -330,6 +441,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
Loading

0 comments on commit 0a6d918

Please sign in to comment.