Rcl timer with ros time (#286)
* 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
This commit is contained in:
Shane Loretz
GitHub
parent
e5e338a8e0
commit
0a6d9186e8
4 changed files with 444 additions and 49 deletions
|
|
@ -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"
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
@ -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,
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
@ -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.
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -354,16 +354,39 @@ rcl_wait_set_resize(
|
|||
SET_RESIZE_RMW_REALLOC(
|
||||
subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count)
|
||||
);
|
||||
SET_RESIZE(
|
||||
guard_condition,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
rmw_guard_conditions.guard_conditions,
|
||||
rmw_guard_conditions.guard_condition_count),
|
||||
SET_RESIZE_RMW_REALLOC(
|
||||
guard_condition,
|
||||
rmw_guard_conditions.guard_conditions,
|
||||
rmw_guard_conditions.guard_condition_count)
|
||||
);
|
||||
// Guard condition RCL size is the resize amount given
|
||||
SET_RESIZE(guard_condition,;,;); // NOLINT
|
||||
|
||||
// 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;
|
||||
// Clear added guard conditions
|
||||
rmw_gcs->guard_condition_count = 0u;
|
||||
if (0u == num_rmw_gc) {
|
||||
if (rmw_gcs->guard_conditions) {
|
||||
wait_set->impl->allocator.deallocate(
|
||||
(void *)rmw_gcs->guard_conditions, wait_set->impl->allocator.state);
|
||||
rmw_gcs->guard_conditions = NULL;
|
||||
}
|
||||
} else {
|
||||
rmw_gcs->guard_conditions = (void **)wait_set->impl->allocator.reallocate(
|
||||
rmw_gcs->guard_conditions, sizeof(void *) * num_rmw_gc, wait_set->impl->allocator.state);
|
||||
if (!rmw_gcs->guard_conditions) {
|
||||
// Deallocate rcl arrays to match unallocated rmw guard conditions
|
||||
wait_set->impl->allocator.deallocate(
|
||||
(void *)wait_set->guard_conditions, wait_set->impl->allocator.state);
|
||||
wait_set->size_of_guard_conditions = 0u;
|
||||
wait_set->guard_conditions = NULL;
|
||||
wait_set->impl->allocator.deallocate(
|
||||
(void *)wait_set->timers, wait_set->impl->allocator.state);
|
||||
wait_set->size_of_timers = 0u;
|
||||
wait_set->timers = NULL;
|
||||
RCL_SET_ERROR_MSG("allocating memory failed", wait_set->impl->allocator);
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
memset(rmw_gcs->guard_conditions, 0, sizeof(void *) * num_rmw_gc);
|
||||
}
|
||||
|
||||
SET_RESIZE(timer,;,;); // NOLINT
|
||||
SET_RESIZE(client,
|
||||
SET_RESIZE_RMW_DEALLOC(
|
||||
|
|
@ -397,6 +420,16 @@ rcl_wait_set_add_timer(
|
|||
const rcl_timer_t * timer)
|
||||
{
|
||||
SET_ADD(timer)
|
||||
// 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_safe(), return RCL_RET_ERROR, wait_set->impl->allocator);
|
||||
wait_set->impl->rmw_guard_conditions.guard_conditions[index] = rmw_handle->data;
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
|
|
@ -443,6 +476,8 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
|
|||
rmw_time_t * timeout_argument = NULL;
|
||||
rmw_time_t temporary_timeout_storage;
|
||||
|
||||
bool is_timer_timeout = false;
|
||||
int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
|
||||
// calculate the number of valid (non-NULL and non-canceled) timers
|
||||
size_t number_of_valid_timers = wait_set->size_of_timers;
|
||||
{ // scope to prevent i from colliding below
|
||||
|
|
@ -460,27 +495,17 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
|
|||
if (is_canceled) {
|
||||
number_of_valid_timers--;
|
||||
wait_set->timers[i] = NULL;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool is_timer_timeout = false;
|
||||
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 if (timeout > 0 || number_of_valid_timers > 0) {
|
||||
int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX;
|
||||
// Compare the timeout to the time until next callback for each timer.
|
||||
// Take the lowest and use that for the wait timeout.
|
||||
uint64_t i = 0;
|
||||
for (i = 0; i < wait_set->impl->timer_index; ++i) {
|
||||
if (!wait_set->timers[i]) {
|
||||
continue; // Skip NULL timers.
|
||||
}
|
||||
// at this point we know any non-NULL timers are also not canceled
|
||||
|
||||
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);
|
||||
} else {
|
||||
// No guard condition, instead use to set the rmw_wait timeout
|
||||
int64_t timer_timeout = INT64_MAX;
|
||||
rcl_ret_t ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout);
|
||||
if (ret != RCL_RET_OK) {
|
||||
|
|
@ -491,7 +516,15 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout)
|
|||
min_timeout = timer_timeout;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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 if (timeout > 0 || number_of_valid_timers > 0) {
|
||||
// If min_timeout was negative, we need to wake up immediately.
|
||||
if (min_timeout < 0) {
|
||||
min_timeout = 0;
|
||||
|
|
|
|||
|
|
@ -13,6 +13,8 @@
|
|||
// limitations under the License.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "rcl/timer.h"
|
||||
|
||||
|
|
@ -246,3 +248,223 @@ TEST_F(TestTimerFixture, test_canceled_timer) {
|
|||
ret = rcl_clock_fini(&clock);
|
||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
}
|
||||
|
||||
TEST_F(TestTimerFixture, test_rostime_time_until_next_call) {
|
||||
rcl_ret_t ret;
|
||||
const int64_t sec_5 = RCL_S_TO_NS(5);
|
||||
int64_t time_until = 0;
|
||||
|
||||
rcl_clock_t clock;
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
|
||||
|
||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||
ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator());
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe();
|
||||
ret = rcl_timer_get_time_until_next_call(&timer, &time_until);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
EXPECT_EQ(sec_5 - 1, time_until);
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5)) << rcl_get_error_string_safe();
|
||||
ret = rcl_timer_get_time_until_next_call(&timer, &time_until);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
EXPECT_EQ(0, time_until);
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5 + 1)) <<
|
||||
rcl_get_error_string_safe();
|
||||
ret = rcl_timer_get_time_until_next_call(&timer, &time_until);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
EXPECT_EQ(-1, time_until);
|
||||
}
|
||||
|
||||
TEST_F(TestTimerFixture, test_system_time_to_ros_time) {
|
||||
rcl_ret_t ret;
|
||||
const int64_t sec_5 = RCL_S_TO_NS(5);
|
||||
|
||||
rcl_clock_t clock;
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||
ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator());
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
int64_t time_until_pre = 0;
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until_pre)) <<
|
||||
rcl_get_error_string_safe();
|
||||
ASSERT_LT(0, time_until_pre);
|
||||
ASSERT_GT(sec_5, time_until_pre);
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
|
||||
|
||||
int64_t time_until = 0;
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) <<
|
||||
rcl_get_error_string_safe();
|
||||
// Because of time credit the time until next call should be less than before
|
||||
EXPECT_GT(time_until_pre, time_until);
|
||||
EXPECT_LT(0, time_until);
|
||||
}
|
||||
|
||||
TEST_F(TestTimerFixture, test_ros_time_to_system_time) {
|
||||
rcl_ret_t ret;
|
||||
const int64_t sec_5 = RCL_S_TO_NS(5);
|
||||
const int64_t sec_1 = RCL_S_TO_NS(1);
|
||||
|
||||
rcl_clock_t clock;
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
|
||||
|
||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||
ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator());
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
|
||||
|
||||
int64_t time_until_pre = 0;
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until_pre)) <<
|
||||
rcl_get_error_string_safe();
|
||||
ASSERT_EQ(sec_5 - (sec_1 - 1), time_until_pre);
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_disable_ros_time_override(&clock)) << rcl_get_error_string_safe();
|
||||
|
||||
int64_t time_until = 0;
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) <<
|
||||
rcl_get_error_string_safe();
|
||||
// Because of time credit the time until next call should be less than before
|
||||
EXPECT_GT(time_until_pre, time_until);
|
||||
EXPECT_LT(0, time_until);
|
||||
}
|
||||
|
||||
TEST_F(TestTimerFixture, test_ros_time_backwards_jump) {
|
||||
rcl_ret_t ret;
|
||||
const int64_t sec_5 = RCL_S_TO_NS(5);
|
||||
const int64_t sec_3 = RCL_S_TO_NS(3);
|
||||
const int64_t sec_2 = RCL_S_TO_NS(2);
|
||||
const int64_t sec_1 = RCL_S_TO_NS(1);
|
||||
|
||||
rcl_clock_t clock;
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator);
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_2)) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
|
||||
|
||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||
ret = rcl_timer_init(&timer, &clock, sec_5, nullptr, rcl_get_default_allocator());
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_3)) << rcl_get_error_string_safe();
|
||||
{
|
||||
// Moved forward a little bit, timer should be closer to being ready
|
||||
int64_t time_until = 0;
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) <<
|
||||
rcl_get_error_string_safe();
|
||||
EXPECT_EQ(sec_5 - (sec_3 - sec_2), time_until);
|
||||
}
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe();
|
||||
{
|
||||
// Jumped back before timer was created, so last_call_time should be 1 period
|
||||
int64_t time_until = 0;
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) <<
|
||||
rcl_get_error_string_safe();
|
||||
EXPECT_EQ(sec_5, time_until);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(TestTimerFixture, test_ros_time_wakes_wait) {
|
||||
const int64_t sec_5 = RCL_S_TO_NS(5);
|
||||
const int64_t sec_1 = RCL_S_TO_NS(1);
|
||||
const int64_t sec_1_5 = RCL_S_TO_NS(3) / 2;
|
||||
|
||||
rcl_ret_t ret;
|
||||
rcl_clock_t clock;
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_clock_init(RCL_ROS_TIME, &clock, &allocator)) <<
|
||||
rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string_safe();
|
||||
|
||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||
ret = rcl_timer_init(&timer, &clock, sec_1, nullptr, rcl_get_default_allocator());
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
bool timer_was_ready = false;
|
||||
|
||||
std::thread wait_thr([&](void) {
|
||||
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
|
||||
ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator());
|
||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_wait_set_add_timer(&wait_set, &timer)) <<
|
||||
rcl_get_error_string_safe();
|
||||
// *INDENT-OFF* (Uncrustify wants strange un-indentation here)
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) <<
|
||||
rcl_get_error_string_safe();
|
||||
});
|
||||
// *INDENT-ON*
|
||||
|
||||
ret = rcl_wait(&wait_set, sec_5);
|
||||
// set some flag indicating wait was exited
|
||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
if (wait_set.timers[0] != NULL) {
|
||||
timer_was_ready = true;
|
||||
}
|
||||
});
|
||||
|
||||
// Timer not exceeded, should not wake
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1_5)) <<
|
||||
rcl_get_error_string_safe();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
EXPECT_FALSE(timer_was_ready);
|
||||
|
||||
// Timer exceeded, should wake
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5)) << rcl_get_error_string_safe();
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
wait_thr.join();
|
||||
auto finish = std::chrono::steady_clock::now();
|
||||
EXPECT_TRUE(timer_was_ready);
|
||||
EXPECT_LT(finish - start, std::chrono::milliseconds(100));
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue