From 22761b65964f1d4558e76eed42e8ec2236271d34 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 4 Dec 2015 15:03:18 -0800 Subject: [PATCH] implement timer.c --- rcl/CMakeLists.txt | 1 + rcl/include/rcl/time.h | 17 ++- rcl/include/rcl/timer.h | 78 +++++++++++-- rcl/src/rcl/time.c | 9 ++ rcl/src/rcl/timer.c | 236 ++++++++++++++++++++++++++++++++++++++++ 5 files changed, 328 insertions(+), 13 deletions(-) create mode 100644 rcl/src/rcl/timer.c diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 0f16659..46f65b2 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -26,6 +26,7 @@ set(${PROJECT_NAME}_sources src/rcl/subscription.c src/rcl/wait.c src/rcl/time.c + src/rcl/timer.c ) add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_sources}) ament_target_dependencies(${PROJECT_NAME} diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index 36d0d71..bffb8be 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -23,13 +23,13 @@ extern "C" #include "rcl/types.h" #include "rmw/rmw.h" -#define RCL_S_TO_NS(seconds) seconds * 1000000000 -#define RCL_MS_TO_NS(milliseconds) milliseconds * 1000000 -#define RCL_US_TO_NS(microseconds) microseconds * 1000 +#define RCL_S_TO_NS(seconds) (seconds * 1000000000) +#define RCL_MS_TO_NS(milliseconds) (milliseconds * 1000000) +#define RCL_US_TO_NS(microseconds) (microseconds * 1000) -#define RCL_NS_TO_S(nanoseconds) nanoseconds / 1000000000 -#define RCL_NS_TO_MS(nanoseconds) nanoseconds / 1000000 -#define RCL_NS_TO_US(nanoseconds) nanoseconds / 1000 +#define RCL_NS_TO_S(nanoseconds) (nanoseconds / 1000000000) +#define RCL_NS_TO_MS(nanoseconds) (nanoseconds / 1000000) +#define RCL_NS_TO_US(nanoseconds) (nanoseconds / 1000) typedef rmw_time_t rcl_time_t; @@ -41,6 +41,11 @@ rcl_time_from_int64_t_nanoseconds(int64_t nanoseconds); rcl_time_t rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds); +/// Convert a rcl_time_t struct into an unsigned number of nanoseconds. +/* This function will return 0 if the time argument is NULL. */ +uint64_t +rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * time); + /// Retrieve the current time as a rcl_time_t struct. /* The now argument must point to an allocated rcl_time_t struct, as the * result is copied into this variable. diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index 1001851..d273ed5 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -32,7 +32,7 @@ struct rcl_timer_impl_t; /// Handle for a ROS timer. typedef struct rcl_timer_t { /// Private implementation pointer. - rcl_timer_impl_t * impl; + struct rcl_timer_impl_t * impl; } rcl_timer_t; /// User callback signature for timers. @@ -77,7 +77,25 @@ rcl_get_zero_initialized_timer(); * struct which is the time of the previous call or when the timer was created * if it is the first call to the callback. * + * Expected usage: + * + * #include + * + * void my_timer_callback(rcl_timer_t * timer, rcl_time_t last_call_time) + * { + * // Do timer work... + * // Optionally reconfigure, cancel, or reset the timer... + * } + * + * rcl_timer_t timer = rcl_get_zero_initialized_timer(); + * rcl_ret_t ret = + * rcl_timer_init(&timer, RCL_MS_TO_NS(100), my_timer_callback, rcl_get_default_allocator()); + * // ... error handling, use the timer with a wait set, or poll it manually, then cleanup + * ret = rcl_timer_fini(&timer); + * // ... error handling + * * This function is not thread-safe. + * This function is lock-free. * * \param[inout] timer the timer handle to be initialized * \param[in] period the duration between calls to the callback in nanoseconds @@ -85,6 +103,7 @@ rcl_get_zero_initialized_timer(); * \param[in] allocator the allocator to use for allocations * \return RCL_RET_OK if the timer was initialized successfully, or * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_ALREADY_INIT if the timer was already initialized, or * RCL_RET_BAD_ALLOC if allocating memory failed, or * RCL_RET_ERROR an unspecified error occur. */ @@ -120,7 +139,8 @@ rcl_timer_fini(rcl_timer_t * timer); * - Ensure the timer has not been canceled. * - Get the current time into a temporary rcl_time_t. * - Exchange the current time with the last call time of the timer. - * - Call the callback, with this timer and the exchanged last call time. + * - Call the callback, with this timer and the last call time as arguments. + * - Return after the callback has completed. * * During the callback the timer can be canceled or have its period and/or * callback modified. @@ -213,6 +233,48 @@ rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_unt rcl_ret_t rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time); +/// Retrieve the period of the timer. +/* This function retrieves the period and copies it into the give variable. + * + * The period argument must be a pointer to an already allocated uint64_t. + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[in] timer the handle to the timer which is being queried + * \param[out] period the uint64_t in which the period is stored + * \return RCL_RET_OK if the period was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period); + +/// Exchange the period of the timer and return the previous period. +/* This function exchanges the period in the timer and copies the old one into + * the give variable. + * + * Exchanging (changing) the period will not affect already waiting wait sets. + * + * The old_period argument must be a pointer to an already allocated uint64_t. + * + * This function is thread-safe. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_int_least64_t. + * + * \param[in] timer the handle to the timer which is being modified + * \param[out] new_period the uint64_t to exchange into the timer + * \param[out] old_period the uint64_t in which the previous period is stored + * \return RCL_RET_OK if the period was retrieved successfully, or + * RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * RCL_RET_TIMER_INVALID if the timer is invalid, or + * RCL_RET_ERROR an unspecified error occur. + */ +rcl_ret_t +rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period); + /// Return the current timer callback. /* This function can fail, and therefore return NULL, if: * - timer is NULL @@ -238,12 +300,12 @@ rcl_timer_get_callback(const rcl_timer_t * timer); * This function is lock-free so long as the C11's stdatomic.h function * atomic_is_lock_free() returns true for atomic_uintptr_t. * - * \param[inout] timer handle to the timer from the callback should be returned - * \param[in] timer handle to the timer from the callback should be returned + * \param[inout] timer handle to the timer from the callback should be exchanged + * \param[in] new_callback the callback to be exchanged into the timer * \return function pointer to the old callback, or NULL if an error occurred */ rcl_timer_callback_t -rcl_timer_exchange_callback(rcl_time_t * timer, const rcl_timer_callback_t new_callback); +rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback); /// Cancel a timer. /* When a timer is canceled, rcl_timer_is_ready() will return false for that @@ -253,7 +315,8 @@ rcl_timer_exchange_callback(rcl_time_t * timer, const rcl_timer_callback_t new_c * Calling this function on an already canceled timer will succeed. * * This function is thread-safe. - * This function is lock-free. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_bool. * * \param[inout] timer the timer to be canceled * \return RCL_RET_OK if the last call time was retrieved successfully, or @@ -272,7 +335,8 @@ rcl_timer_cancel(rcl_timer_t * timer); * copied into this variable. * * This function is thread-safe. - * This function is lock-free. + * This function is lock-free so long as the C11's stdatomic.h function + * atomic_is_lock_free() returns true for atomic_bool. * * \param[in] timer the timer to be queried * \return RCL_RET_OK if the last call time was retrieved successfully, or diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index eab5fdf..be4c557 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -57,6 +57,15 @@ rcl_time_from_uint64_t_nanoseconds(uint64_t nanoseconds) return result; } +uint64_t +rcl_time_to_uint64_t_nanoseconds(const rcl_time_t * rcl_time) +{ + if (!rcl_time) { + return 0; + } + return RCL_S_TO_NS(rcl_time->sec) + rcl_time->nsec; +} + static void __timespec_get_now(struct timespec * timespec_now) { diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c new file mode 100644 index 0000000..a8df7ad --- /dev/null +++ b/rcl/src/rcl/timer.c @@ -0,0 +1,236 @@ +// Copyright 2015 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if __cplusplus +extern "C" +{ +#endif + +#include "rcl/timer.h" + +#include + +#include "./common.h" + +typedef struct rcl_timer_impl_t { + // The user supplied callback. + atomic_uintptr_t callback; + // This is a duration in nanoseconds. + atomic_uint_least64_t period; + // This is an absolute time in nanoseconds since the unix epoch. + atomic_uint_least64_t last_call_time; + // A flag which indicates if the timer is canceled. + atomic_bool canceled; + // The user supplied allocator. + rcl_allocator_t allocator; +} rcl_timer_impl_t; + +rcl_timer_t +rcl_get_zero_initialized_timer() +{ + static rcl_timer_t null_timer = {0}; + return null_timer; +} + +rcl_ret_t +rcl_timer_init( + rcl_timer_t * timer, + uint64_t period, + const rcl_timer_callback_t callback, + 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; + } + rcl_time_t now; + rcl_ret_t now_ret = rcl_time_now(&now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + rcl_timer_impl_t impl = { + .callback = ATOMIC_VAR_INIT((uintptr_t)callback), + .period = ATOMIC_VAR_INIT(period), + .last_call_time = ATOMIC_VAR_INIT(rcl_time_to_uint64_t_nanoseconds(&now)), + .canceled = ATOMIC_VAR_INIT(false), + .allocator = allocator, + }; + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.allocate, "allocate not set", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + allocator.deallocate, "deallocate not set", return RCL_RET_INVALID_ARGUMENT); + 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); + *timer->impl = impl; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_fini(rcl_timer_t * timer) +{ + if (!timer || !timer->impl) { + return RCL_RET_OK; + } + // 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; + allocator.deallocate(timer->impl, allocator.state); + return result; +} + +rcl_ret_t +rcl_timer_call(rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + if (atomic_load(&timer->impl->canceled)) { + RCL_SET_ERROR_MSG("timer is canceled"); + return RCL_RET_TIMER_CANCELED; + } + rcl_time_t now; + rcl_ret_t now_ret = rcl_time_now(&now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + uint64_t previous_ns = + atomic_exchange(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now)); + rcl_time_t previous = rcl_time_from_uint64_t_nanoseconds(previous_ns); + rcl_timer_callback_t typed_callback = (rcl_timer_callback_t)atomic_load(&timer->impl->callback); + typed_callback(timer, previous); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_ready, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + int64_t time_until_next_call; + rcl_ret_t ret = rcl_timer_get_time_until_next_call(timer, &time_until_next_call); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + *is_ready = (time_until_next_call <= 0) && !atomic_load(&timer->impl->canceled); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + rcl_time_t now, last_call_time; + rcl_ret_t ret = rcl_time_now(&now); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + ret = rcl_timer_get_last_call_time(timer, &last_call_time); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + uint64_t period = atomic_load(&timer->impl->period); + int64_t next_call = rcl_time_to_uint64_t_nanoseconds(&last_call_time) + period; + *time_until_next_call = next_call - rcl_time_to_uint64_t_nanoseconds(&now); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_last_call_time(const rcl_timer_t * timer, rcl_time_t * last_call_time) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(last_call_time, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + uint64_t last_call_time_ns = atomic_load(&timer->impl->last_call_time); + *last_call_time = rcl_time_from_uint64_t_nanoseconds(last_call_time_ns); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_period(const rcl_timer_t * timer, uint64_t * period) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + *period = atomic_load(&timer->impl->period); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_exchange_period(const rcl_timer_t * timer, uint64_t new_period, uint64_t * old_period) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + *old_period = atomic_exchange(&timer->impl->period, new_period); + return RCL_RET_OK; +} + +rcl_timer_callback_t +rcl_timer_get_callback(const rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); + return (rcl_timer_callback_t)atomic_load(&timer->impl->callback); +} + +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)atomic_exchange(&timer->impl->callback, (uintptr_t)new_callback); +} + +rcl_ret_t +rcl_timer_cancel(rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + atomic_store(&timer->impl->canceled, true); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + *is_canceled = atomic_load(&timer->impl->canceled); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_reset(rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + rcl_time_t now; + rcl_ret_t now_ret = rcl_time_now(&now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + atomic_store(&timer->impl->last_call_time, rcl_time_to_uint64_t_nanoseconds(&now)); + atomic_store(&timer->impl->canceled, false); + return RCL_RET_OK; +} + +#if __cplusplus +} +#endif