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

Make timer callback optional and fix timer logic

Browse source 
Make timer callback optional and fix wait bugs
This commit is contained in:
Jackie Kay 2016-04-01 18:14:12 -07:00
parent 087315b4c6
commit 3f886e075f
4 changed files with 55 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

15
rcl/include/rcl/timer.h
View file

@ -77,7 +77,12 @@ rcl_get_zero_initialized_timer(void);
* The period is a duration (rather an absolute time in the future).
* If the period is 0 then it will always be ready.
*
* The callback must be a function which returns void and takes two arguments,
* The callback is an optional argument.
* Valid inputs are either a pointer to the function callback, or NULL to
* indicate that no callback will be stored in rcl.
* If the callback is null, the caller client library is responsible for
* firing the timer callback.
* Else, it must be a function which returns void and takes two arguments,
* the first being a pointer to the associated timer, and the second a uint64_t
* which is the time since the previous call, or since the timer was created
* if it is the first call to the callback.
@ -148,6 +153,10 @@ rcl_timer_fini(rcl_timer_t * timer);
* the timer's period has not yet elapsed.
* It is up to the calling code to make sure the period has elapsed by first
* calling rcl_timer_is_ready().
* If the callback pointer is null (either set in init or exchanged after
* initialized), no callback is fired.
* However, this function should still be called by the client library to
* update the state of the timer.
* The order of operations in this command are as follows:
*
* - Ensure the timer has not been canceled.
@ -326,7 +335,9 @@ rcl_timer_get_callback(const rcl_timer_t * timer);
/* This function can fail, and therefore return NULL, if:
* - timer is NULL
* - timer has not been initialized (the implementation is invalid)
* - the new_callback argument is NULL
*
* This function can set callback to null, in which case the callback is
* ignored when rcl_timer_call is called.
*
* This function is thread-safe.
* This function is lock-free so long as the C11's stdatomic.h function

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

@ -51,7 +51,6 @@ rcl_timer_init(
rcl_allocator_t allocator)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT);
RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT);
if (timer->impl) {
RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized");
return RCL_RET_ALREADY_INIT;
@ -106,10 +105,13 @@ rcl_timer_call(rcl_timer_t * timer)
}
rcl_time_point_value_t previous_ns =
rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady);
uint64_t since_last_call = now_steady - previous_ns;
rcl_timer_callback_t typed_callback =
(rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
typed_callback(timer, since_last_call);
if (typed_callback != NULL) {
uint64_t since_last_call = now_steady - previous_ns;
typed_callback(timer, since_last_call);
}
return RCL_RET_OK;
}
@ -195,7 +197,6 @@ rcl_timer_callback_t
rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback)
{
RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL);
RCL_CHECK_ARGUMENT_FOR_NULL(new_callback, NULL);
RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL);
return (rcl_timer_callback_t)rcl_atomic_exchange_uintptr_t(
&timer->impl->callback, (uintptr_t)new_callback);

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

@ -512,44 +512,49 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
return RCL_RET_WAIT_SET_EMPTY;
}
// Calculate the timeout argument.
rmw_time_t * timeout_argument;
// By default, set the timer to block indefinitely if none of the below conditions are met.
rmw_time_t * timeout_argument = NULL;
rmw_time_t temporary_timeout_storage;
if (timeout < 0) {
// Pass NULL to wait to indicate block indefinitely.
timeout_argument = NULL;
}
if (timeout > 0) {
// Determine the nearest timeout (given or a timer).
uint64_t min_timeout = timeout;
// If min_timeout is > 0, then compare it to the time until each timer.
// Take the lowest and use that for the wait timeout.
if (min_timeout > 0) {
size_t i;
for (i = 0; i < wait_set->size_of_timers; ++i) {
if (!wait_set->timers[i]) {
continue; // Skip NULL timers.
}
int64_t timer_timeout;
rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout);
if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set.
}
if ((uint64_t)timer_timeout < min_timeout) {
min_timeout = timer_timeout;
}
}
}
// Set that in the temporary storage and point to that for the argument.
temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout);
temporary_timeout_storage.nsec = min_timeout % 1000000000;
timeout_argument = &temporary_timeout_storage;
}
if (timeout == 0) {
// Then it is non-blocking, so set the temporary storage to 0, 0 and pass it.
temporary_timeout_storage.sec = 0;
temporary_timeout_storage.nsec = 0;
timeout_argument = &temporary_timeout_storage;
} else {
int64_t min_timeout = INT64_MAX;
if (timeout > 0) {
// Compare the timeout to the time until next callback for each timer.
min_timeout = timeout;
}
// Take the lowest and use that for the wait timeout.
uint64_t i = 0;
for (i = 0; i < wait_set->size_of_timers; ++i) {
if (!wait_set->timers[i]) {
continue; // Skip NULL timers.
}
int64_t timer_timeout;
rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout);
if (ret != RCL_RET_OK) {
return ret; // The rcl error state should already be set.
}
if (timer_timeout < min_timeout) {
min_timeout = timer_timeout;
}
}
if (min_timeout == INT64_MAX) {
timeout_argument = NULL;
} else {
// If min_timeout was negative, we need to wake up immediately.
if (min_timeout < 0) {
min_timeout = 0;
}
temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout);
temporary_timeout_storage.nsec = min_timeout % 1000000000;
timeout_argument = &temporary_timeout_storage;
}
}
// Wait.
rmw_ret_t ret = rmw_wait(
&wait_set->impl->rmw_subscriptions,