initialize timer with clock (#272)
* initialize timer with clock * use rcl_clock_get_now * call rcl_clock_fini at the end of each test * modify rcl_clock_get_now to take a rcl_time_point_value_t * update docblock * update to pass time_point_value * add check for NULL * add rcl_timer_clock() * fix style * doc fixes * fini clock
This commit is contained in:
parent
d3a734f266
commit
b324f9de0d
7 changed files with 141 additions and 35 deletions
|
@ -272,12 +272,12 @@ rcl_ret_t
|
||||||
rcl_difference_times(
|
rcl_difference_times(
|
||||||
rcl_time_point_t * start, rcl_time_point_t * finish, rcl_duration_t * delta);
|
rcl_time_point_t * start, rcl_time_point_t * finish, rcl_duration_t * delta);
|
||||||
|
|
||||||
/// Fill the time point with the current value of the associated clock.
|
/// Fill the time point value with the current value of the associated clock.
|
||||||
/**
|
/**
|
||||||
* This function will populate the data of the time_point object with the
|
* This function will populate the data of the time_point_value object with the
|
||||||
* current value from it's associated time abstraction.
|
* current value from it's associated time abstraction.
|
||||||
* \param[in] clock The time source from which to set the value.
|
* \param[in] clock The time source from which to set the value.
|
||||||
* \param[out] time_point The time_point to populate.
|
* \param[out] time_point_value The time_point value to populate.
|
||||||
* \return `RCL_RET_OK` if the last call time was retrieved successfully, or
|
* \return `RCL_RET_OK` if the last call time was retrieved successfully, or
|
||||||
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||||
* \return `RCL_RET_ERROR` an unspecified error occur.
|
* \return `RCL_RET_ERROR` an unspecified error occur.
|
||||||
|
@ -285,7 +285,7 @@ rcl_difference_times(
|
||||||
RCL_PUBLIC
|
RCL_PUBLIC
|
||||||
RCL_WARN_UNUSED
|
RCL_WARN_UNUSED
|
||||||
rcl_ret_t
|
rcl_ret_t
|
||||||
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point);
|
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value);
|
||||||
|
|
||||||
|
|
||||||
/// Enable the ROS time abstraction override.
|
/// Enable the ROS time abstraction override.
|
||||||
|
|
|
@ -28,6 +28,7 @@ extern "C"
|
||||||
#include "rcl/types.h"
|
#include "rcl/types.h"
|
||||||
#include "rmw/rmw.h"
|
#include "rmw/rmw.h"
|
||||||
|
|
||||||
|
struct rcl_clock_t;
|
||||||
struct rcl_timer_impl_t;
|
struct rcl_timer_impl_t;
|
||||||
|
|
||||||
/// Structure which encapsulates a ROS Timer.
|
/// Structure which encapsulates a ROS Timer.
|
||||||
|
@ -60,7 +61,7 @@ rcl_get_zero_initialized_timer(void);
|
||||||
|
|
||||||
/// Initialize a timer.
|
/// Initialize a timer.
|
||||||
/**
|
/**
|
||||||
* A timer consists of a callback function and a period.
|
* A timer consists of a clock, a callback function and a period.
|
||||||
* A timer can be added to a wait set and waited on, such that the wait set
|
* A timer can be added to a wait set and waited on, such that the wait set
|
||||||
* will wake up when a timer is ready to be executed.
|
* will wake up when a timer is ready to be executed.
|
||||||
*
|
*
|
||||||
|
@ -77,6 +78,9 @@ rcl_get_zero_initialized_timer(void);
|
||||||
* Calling this function on a timer struct which has been allocated but not
|
* Calling this function on a timer struct which has been allocated but not
|
||||||
* zero initialized is undefined behavior.
|
* zero initialized is undefined behavior.
|
||||||
*
|
*
|
||||||
|
* The clock handle must be a pointer to an initialized rcl_clock_t struct.
|
||||||
|
* The life time of the clock must exceed the life time of the timer.
|
||||||
|
*
|
||||||
* The period is a duration (rather an absolute time in the future).
|
* The period is a duration (rather an absolute time in the future).
|
||||||
* If the period is `0` then it will always be ready.
|
* If the period is `0` then it will always be ready.
|
||||||
*
|
*
|
||||||
|
@ -101,9 +105,14 @@ rcl_get_zero_initialized_timer(void);
|
||||||
* // Optionally reconfigure, cancel, or reset the timer...
|
* // Optionally reconfigure, cancel, or reset the timer...
|
||||||
* }
|
* }
|
||||||
*
|
*
|
||||||
|
* rcl_clock_t clock;
|
||||||
|
* rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
|
* rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
|
||||||
|
* // ... error handling
|
||||||
|
*
|
||||||
* rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
* rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||||
* rcl_ret_t ret = rcl_timer_init(
|
* ret = rcl_timer_init(
|
||||||
* &timer, RCL_MS_TO_NS(100), my_timer_callback, rcl_get_default_allocator());
|
* &timer, &clock, RCL_MS_TO_NS(100), my_timer_callback, allocator);
|
||||||
* // ... error handling, use the timer with a wait set, or poll it manually, then cleanup
|
* // ... error handling, use the timer with a wait set, or poll it manually, then cleanup
|
||||||
* ret = rcl_timer_fini(&timer);
|
* ret = rcl_timer_fini(&timer);
|
||||||
* // ... error handling
|
* // ... error handling
|
||||||
|
@ -123,6 +132,7 @@ rcl_get_zero_initialized_timer(void);
|
||||||
* <i>[3] if `atomic_is_lock_free()` returns true for `atomic_bool`</i>
|
* <i>[3] if `atomic_is_lock_free()` returns true for `atomic_bool`</i>
|
||||||
*
|
*
|
||||||
* \param[inout] timer the timer handle to be initialized
|
* \param[inout] timer the timer handle to be initialized
|
||||||
|
* \param[in] clock the clock providing the current time
|
||||||
* \param[in] period the duration between calls to the callback in nanoseconds
|
* \param[in] period the duration between calls to the callback in nanoseconds
|
||||||
* \param[in] callback the user defined function to be called every period
|
* \param[in] callback the user defined function to be called every period
|
||||||
* \param[in] allocator the allocator to use for allocations
|
* \param[in] allocator the allocator to use for allocations
|
||||||
|
@ -137,6 +147,7 @@ RCL_WARN_UNUSED
|
||||||
rcl_ret_t
|
rcl_ret_t
|
||||||
rcl_timer_init(
|
rcl_timer_init(
|
||||||
rcl_timer_t * timer,
|
rcl_timer_t * timer,
|
||||||
|
rcl_clock_t * clock,
|
||||||
int64_t period,
|
int64_t period,
|
||||||
const rcl_timer_callback_t callback,
|
const rcl_timer_callback_t callback,
|
||||||
rcl_allocator_t allocator);
|
rcl_allocator_t allocator);
|
||||||
|
@ -216,6 +227,32 @@ RCL_WARN_UNUSED
|
||||||
rcl_ret_t
|
rcl_ret_t
|
||||||
rcl_timer_call(rcl_timer_t * timer);
|
rcl_timer_call(rcl_timer_t * timer);
|
||||||
|
|
||||||
|
/// Retrieve the clock of the timer.
|
||||||
|
/**
|
||||||
|
* This function retrieves the clock pointer and copies it into the given variable.
|
||||||
|
*
|
||||||
|
* The clock argument must be a pointer to an already allocated rcl_clock_t *.
|
||||||
|
*
|
||||||
|
* <hr>
|
||||||
|
* Attribute | Adherence
|
||||||
|
* ------------------ | -------------
|
||||||
|
* Allocates Memory | No
|
||||||
|
* Thread-Safe | Yes
|
||||||
|
* Uses Atomics | No
|
||||||
|
* Lock-Free | Yes
|
||||||
|
*
|
||||||
|
* \param[in] timer the handle to the timer which is being queried
|
||||||
|
* \param[out] clock the rcl_clock_t * in which the clock is stored
|
||||||
|
* \return `RCL_RET_OK` if the period was retrieved successfully, or
|
||||||
|
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||||
|
* \return `RCL_RET_TIMER_INVALID` if the timer is invalid, or
|
||||||
|
* \return `RCL_RET_ERROR` an unspecified error occur.
|
||||||
|
*/
|
||||||
|
RCL_PUBLIC
|
||||||
|
RCL_WARN_UNUSED
|
||||||
|
rcl_ret_t
|
||||||
|
rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock);
|
||||||
|
|
||||||
/// Calculates whether or not the timer should be called.
|
/// Calculates whether or not the timer should be called.
|
||||||
/**
|
/**
|
||||||
* The result is true if the time until next call is less than, or equal to, 0
|
* The result is true if the time until next call is less than, or equal to, 0
|
||||||
|
@ -315,7 +352,7 @@ rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, int64_t * time_sin
|
||||||
|
|
||||||
/// Retrieve the period of the timer.
|
/// Retrieve the period of the timer.
|
||||||
/**
|
/**
|
||||||
* This function retrieves the period and copies it into the give variable.
|
* This function retrieves the period and copies it into the given variable.
|
||||||
*
|
*
|
||||||
* The period argument must be a pointer to an already allocated int64_t.
|
* The period argument must be a pointer to an already allocated int64_t.
|
||||||
*
|
*
|
||||||
|
@ -343,7 +380,7 @@ rcl_timer_get_period(const rcl_timer_t * timer, int64_t * period);
|
||||||
/// Exchange the period of the timer and return the previous 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
|
* This function exchanges the period in the timer and copies the old one into
|
||||||
* the give variable.
|
* the given variable.
|
||||||
*
|
*
|
||||||
* Exchanging (changing) the period will not affect already waiting wait sets.
|
* Exchanging (changing) the period will not affect already waiting wait sets.
|
||||||
*
|
*
|
||||||
|
|
|
@ -110,6 +110,7 @@ rcl_ret_t
|
||||||
rcl_clock_fini(
|
rcl_clock_fini(
|
||||||
rcl_clock_t * clock)
|
rcl_clock_t * clock)
|
||||||
{
|
{
|
||||||
|
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||||
switch (clock->type) {
|
switch (clock->type) {
|
||||||
case RCL_ROS_TIME:
|
case RCL_ROS_TIME:
|
||||||
return rcl_ros_clock_fini(clock);
|
return rcl_ros_clock_fini(clock);
|
||||||
|
@ -226,12 +227,13 @@ rcl_difference_times(
|
||||||
}
|
}
|
||||||
|
|
||||||
rcl_ret_t
|
rcl_ret_t
|
||||||
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_t * time_point)
|
rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value)
|
||||||
{
|
{
|
||||||
RCL_CHECK_ARGUMENT_FOR_NULL(time_point, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||||
|
RCL_CHECK_ARGUMENT_FOR_NULL(
|
||||||
|
time_point_value, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||||
if (clock->type && clock->get_now) {
|
if (clock->type && clock->get_now) {
|
||||||
return clock->get_now(clock->data,
|
return clock->get_now(clock->data, time_point_value);
|
||||||
&(time_point->nanoseconds));
|
|
||||||
}
|
}
|
||||||
RCL_SET_ERROR_MSG(
|
RCL_SET_ERROR_MSG(
|
||||||
"clock is not initialized or does not have get_now registered.",
|
"clock is not initialized or does not have get_now registered.",
|
||||||
|
|
|
@ -28,6 +28,8 @@ extern "C"
|
||||||
|
|
||||||
typedef struct rcl_timer_impl_t
|
typedef struct rcl_timer_impl_t
|
||||||
{
|
{
|
||||||
|
// The clock providing time.
|
||||||
|
rcl_clock_t * clock;
|
||||||
// The user supplied callback.
|
// The user supplied callback.
|
||||||
atomic_uintptr_t callback;
|
atomic_uintptr_t callback;
|
||||||
// This is a duration in nanoseconds.
|
// This is a duration in nanoseconds.
|
||||||
|
@ -50,26 +52,29 @@ rcl_get_zero_initialized_timer()
|
||||||
rcl_ret_t
|
rcl_ret_t
|
||||||
rcl_timer_init(
|
rcl_timer_init(
|
||||||
rcl_timer_t * timer,
|
rcl_timer_t * timer,
|
||||||
|
rcl_clock_t * clock,
|
||||||
int64_t period,
|
int64_t period,
|
||||||
const rcl_timer_callback_t callback,
|
const rcl_timer_callback_t callback,
|
||||||
rcl_allocator_t allocator)
|
rcl_allocator_t allocator)
|
||||||
{
|
{
|
||||||
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
|
RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT);
|
||||||
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, allocator);
|
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, allocator);
|
||||||
|
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, allocator);
|
||||||
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing timer with period: %" PRIu64 "ns", period)
|
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing timer with period: %" PRIu64 "ns", period)
|
||||||
if (timer->impl) {
|
if (timer->impl) {
|
||||||
RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator);
|
RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator);
|
||||||
return RCL_RET_ALREADY_INIT;
|
return RCL_RET_ALREADY_INIT;
|
||||||
}
|
}
|
||||||
rcl_time_point_value_t now_steady;
|
rcl_time_point_value_t now;
|
||||||
rcl_ret_t now_ret = rcutils_steady_time_now(&now_steady);
|
rcl_ret_t now_ret = rcl_clock_get_now(clock, &now);
|
||||||
if (now_ret != RCL_RET_OK) {
|
if (now_ret != RCL_RET_OK) {
|
||||||
return now_ret; // rcl error state should already be set.
|
return now_ret; // rcl error state should already be set.
|
||||||
}
|
}
|
||||||
rcl_timer_impl_t impl;
|
rcl_timer_impl_t impl;
|
||||||
|
impl.clock = clock;
|
||||||
atomic_init(&impl.callback, (uintptr_t)callback);
|
atomic_init(&impl.callback, (uintptr_t)callback);
|
||||||
atomic_init(&impl.period, period);
|
atomic_init(&impl.period, period);
|
||||||
atomic_init(&impl.last_call_time, now_steady);
|
atomic_init(&impl.last_call_time, now);
|
||||||
atomic_init(&impl.canceled, false);
|
atomic_init(&impl.canceled, false);
|
||||||
impl.allocator = allocator;
|
impl.allocator = allocator;
|
||||||
timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
|
timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state);
|
||||||
|
@ -92,6 +97,18 @@ rcl_timer_fini(rcl_timer_t * timer)
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
RCL_PUBLIC
|
||||||
|
RCL_WARN_UNUSED
|
||||||
|
rcl_ret_t
|
||||||
|
rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock)
|
||||||
|
{
|
||||||
|
RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||||
|
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||||
|
RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID, rcl_get_default_allocator());
|
||||||
|
*clock = timer->impl->clock;
|
||||||
|
return RCL_RET_OK;
|
||||||
|
}
|
||||||
|
|
||||||
rcl_ret_t
|
rcl_ret_t
|
||||||
rcl_timer_call(rcl_timer_t * timer)
|
rcl_timer_call(rcl_timer_t * timer)
|
||||||
{
|
{
|
||||||
|
@ -105,18 +122,18 @@ rcl_timer_call(rcl_timer_t * timer)
|
||||||
RCL_SET_ERROR_MSG("timer is canceled", *allocator);
|
RCL_SET_ERROR_MSG("timer is canceled", *allocator);
|
||||||
return RCL_RET_TIMER_CANCELED;
|
return RCL_RET_TIMER_CANCELED;
|
||||||
}
|
}
|
||||||
rcl_time_point_value_t now_steady;
|
rcl_time_point_value_t now;
|
||||||
rcl_ret_t now_ret = rcutils_steady_time_now(&now_steady);
|
rcl_ret_t now_ret = rcl_clock_get_now(timer->impl->clock, &now);
|
||||||
if (now_ret != RCL_RET_OK) {
|
if (now_ret != RCL_RET_OK) {
|
||||||
return now_ret; // rcl error state should already be set.
|
return now_ret; // rcl error state should already be set.
|
||||||
}
|
}
|
||||||
rcl_time_point_value_t previous_ns =
|
rcl_time_point_value_t previous_ns =
|
||||||
rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now_steady);
|
rcl_atomic_exchange_uint64_t(&timer->impl->last_call_time, now);
|
||||||
rcl_timer_callback_t typed_callback =
|
rcl_timer_callback_t typed_callback =
|
||||||
(rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
|
(rcl_timer_callback_t)rcl_atomic_load_uintptr_t(&timer->impl->callback);
|
||||||
|
|
||||||
if (typed_callback != NULL) {
|
if (typed_callback != NULL) {
|
||||||
int64_t since_last_call = now_steady - previous_ns;
|
int64_t since_last_call = now - previous_ns;
|
||||||
typed_callback(timer, since_last_call);
|
typed_callback(timer, since_last_call);
|
||||||
}
|
}
|
||||||
return RCL_RET_OK;
|
return RCL_RET_OK;
|
||||||
|
|
|
@ -76,7 +76,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
|
||||||
ret = rcl_is_enabled_ros_time_override(nullptr, nullptr);
|
ret = rcl_is_enabled_ros_time_override(nullptr, nullptr);
|
||||||
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
|
EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe();
|
||||||
rcl_reset_error();
|
rcl_reset_error();
|
||||||
rcl_time_point_t query_now;
|
rcl_time_point_value_t query_now;
|
||||||
bool is_enabled;
|
bool is_enabled;
|
||||||
ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
|
ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled);
|
||||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||||
|
@ -86,7 +86,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
|
||||||
ret = rcl_clock_get_now(&ros_clock, &query_now);
|
ret = rcl_clock_get_now(&ros_clock, &query_now);
|
||||||
});
|
});
|
||||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||||
EXPECT_NE(query_now.nanoseconds, 0u);
|
EXPECT_NE(query_now, 0u);
|
||||||
// Compare to std::chrono::system_clock time (within a second).
|
// Compare to std::chrono::system_clock time (within a second).
|
||||||
ret = rcl_clock_get_now(&ros_clock, &query_now);
|
ret = rcl_clock_get_now(&ros_clock, &query_now);
|
||||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||||
|
@ -94,7 +94,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
|
||||||
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
|
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
|
||||||
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
|
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
|
||||||
int64_t now_ns_int = now_ns.count();
|
int64_t now_ns_int = now_ns.count();
|
||||||
int64_t now_diff = query_now.nanoseconds - now_ns_int;
|
int64_t now_diff = query_now - now_ns_int;
|
||||||
const int k_tolerance_ms = 1000;
|
const int k_tolerance_ms = 1000;
|
||||||
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
|
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
|
||||||
}
|
}
|
||||||
|
@ -118,7 +118,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
|
||||||
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
|
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
|
||||||
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
|
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
|
||||||
int64_t now_ns_int = now_ns.count();
|
int64_t now_ns_int = now_ns.count();
|
||||||
int64_t now_diff = query_now.nanoseconds - now_ns_int;
|
int64_t now_diff = query_now - now_ns_int;
|
||||||
const int k_tolerance_ms = 1000;
|
const int k_tolerance_ms = 1000;
|
||||||
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
|
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
|
||||||
}
|
}
|
||||||
|
@ -132,7 +132,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
|
||||||
// get sim
|
// get sim
|
||||||
ret = rcl_clock_get_now(&ros_clock, &query_now);
|
ret = rcl_clock_get_now(&ros_clock, &query_now);
|
||||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||||
EXPECT_EQ(query_now.nanoseconds, set_point);
|
EXPECT_EQ(query_now, set_point);
|
||||||
// disable
|
// disable
|
||||||
ret = rcl_disable_ros_time_override(&ros_clock);
|
ret = rcl_disable_ros_time_override(&ros_clock);
|
||||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||||
|
@ -147,7 +147,7 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
|
||||||
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
|
std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now();
|
||||||
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
|
auto now_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(now_sc.time_since_epoch());
|
||||||
int64_t now_ns_int = now_ns.count();
|
int64_t now_ns_int = now_ns.count();
|
||||||
int64_t now_diff = query_now.nanoseconds - now_ns_int;
|
int64_t now_diff = query_now - now_ns_int;
|
||||||
const int k_tolerance_ms = 1000;
|
const int k_tolerance_ms = 1000;
|
||||||
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
|
EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs";
|
||||||
}
|
}
|
||||||
|
@ -352,7 +352,7 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) {
|
||||||
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
||||||
rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
|
rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
|
||||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||||
rcl_time_point_t query_now;
|
rcl_time_point_value_t query_now;
|
||||||
rcl_ret_t ret;
|
rcl_ret_t ret;
|
||||||
rcl_time_point_value_t set_point = 1000000000ull;
|
rcl_time_point_value_t set_point = 1000000000ull;
|
||||||
|
|
||||||
|
|
|
@ -50,13 +50,19 @@ public:
|
||||||
|
|
||||||
TEST_F(TestTimerFixture, test_two_timers) {
|
TEST_F(TestTimerFixture, test_two_timers) {
|
||||||
rcl_ret_t ret;
|
rcl_ret_t ret;
|
||||||
|
|
||||||
|
rcl_clock_t clock;
|
||||||
|
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
|
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
|
||||||
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||||
rcl_timer_t timer2 = rcl_get_zero_initialized_timer();
|
rcl_timer_t timer2 = rcl_get_zero_initialized_timer();
|
||||||
|
|
||||||
ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
|
ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
|
||||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
ret = rcl_timer_init(&timer2, RCL_MS_TO_NS(20), nullptr, rcl_get_default_allocator());
|
ret = rcl_timer_init(&timer2, &clock, RCL_MS_TO_NS(20), nullptr, rcl_get_default_allocator());
|
||||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
|
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
|
||||||
|
@ -91,17 +97,26 @@ TEST_F(TestTimerFixture, test_two_timers) {
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
EXPECT_FALSE(is_ready);
|
EXPECT_FALSE(is_ready);
|
||||||
ASSERT_EQ(1, nonnull_timers);
|
ASSERT_EQ(1, nonnull_timers);
|
||||||
|
|
||||||
|
ret = rcl_clock_fini(&clock);
|
||||||
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) {
|
TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) {
|
||||||
rcl_ret_t ret;
|
rcl_ret_t ret;
|
||||||
|
|
||||||
|
rcl_clock_t clock;
|
||||||
|
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
|
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
|
||||||
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||||
rcl_timer_t timer2 = rcl_get_zero_initialized_timer();
|
rcl_timer_t timer2 = rcl_get_zero_initialized_timer();
|
||||||
|
|
||||||
ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
|
ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
|
||||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
ret = rcl_timer_init(&timer2, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
|
ret = rcl_timer_init(&timer2, &clock, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
|
||||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
|
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
|
||||||
|
@ -136,13 +151,22 @@ TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) {
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
EXPECT_FALSE(is_ready);
|
EXPECT_FALSE(is_ready);
|
||||||
ASSERT_EQ(1, nonnull_timers);
|
ASSERT_EQ(1, nonnull_timers);
|
||||||
|
|
||||||
|
ret = rcl_clock_fini(&clock);
|
||||||
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestTimerFixture, test_timer_not_ready) {
|
TEST_F(TestTimerFixture, test_timer_not_ready) {
|
||||||
rcl_ret_t ret;
|
rcl_ret_t ret;
|
||||||
|
|
||||||
|
rcl_clock_t clock;
|
||||||
|
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
|
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
|
||||||
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||||
|
|
||||||
ret = rcl_timer_init(&timer, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
|
ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(5), nullptr, rcl_get_default_allocator());
|
||||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
|
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
|
||||||
|
@ -171,13 +195,22 @@ TEST_F(TestTimerFixture, test_timer_not_ready) {
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
EXPECT_FALSE(is_ready);
|
EXPECT_FALSE(is_ready);
|
||||||
ASSERT_EQ(0, nonnull_timers);
|
ASSERT_EQ(0, nonnull_timers);
|
||||||
|
|
||||||
|
ret = rcl_clock_fini(&clock);
|
||||||
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(TestTimerFixture, test_canceled_timer) {
|
TEST_F(TestTimerFixture, test_canceled_timer) {
|
||||||
rcl_ret_t ret;
|
rcl_ret_t ret;
|
||||||
|
|
||||||
|
rcl_clock_t clock;
|
||||||
|
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
|
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
|
||||||
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||||
|
|
||||||
ret = rcl_timer_init(&timer, 500, nullptr, rcl_get_default_allocator());
|
ret = rcl_timer_init(&timer, &clock, 500, nullptr, rcl_get_default_allocator());
|
||||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
ret = rcl_timer_cancel(&timer);
|
ret = rcl_timer_cancel(&timer);
|
||||||
|
@ -209,4 +242,7 @@ TEST_F(TestTimerFixture, test_canceled_timer) {
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
EXPECT_FALSE(is_ready);
|
EXPECT_FALSE(is_ready);
|
||||||
ASSERT_EQ(0, nonnull_timers);
|
ASSERT_EQ(0, nonnull_timers);
|
||||||
|
|
||||||
|
ret = rcl_clock_fini(&clock);
|
||||||
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
}
|
}
|
||||||
|
|
|
@ -103,8 +103,13 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) {
|
||||||
ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond);
|
ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond);
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
|
rcl_clock_t clock;
|
||||||
|
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
|
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
|
||||||
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
rcl_timer_t timer = rcl_get_zero_initialized_timer();
|
||||||
ret = rcl_timer_init(&timer, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
|
ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator());
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
ret = rcl_wait_set_add_timer(&wait_set, &timer);
|
ret = rcl_wait_set_add_timer(&wait_set, &timer);
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
@ -205,8 +210,14 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) {
|
||||||
ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond);
|
ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond);
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
|
rcl_clock_t clock;
|
||||||
|
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||||
|
ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator);
|
||||||
|
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
|
||||||
rcl_timer_t canceled_timer = rcl_get_zero_initialized_timer();
|
rcl_timer_t canceled_timer = rcl_get_zero_initialized_timer();
|
||||||
ret = rcl_timer_init(&canceled_timer, RCL_MS_TO_NS(1), nullptr, rcl_get_default_allocator());
|
ret = rcl_timer_init(
|
||||||
|
&canceled_timer, &clock, RCL_MS_TO_NS(1), nullptr, rcl_get_default_allocator());
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
ret = rcl_timer_cancel(&canceled_timer);
|
ret = rcl_timer_cancel(&canceled_timer);
|
||||||
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
|
@ -231,6 +242,9 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) {
|
||||||
// Check time
|
// Check time
|
||||||
int64_t diff = std::chrono::duration_cast<std::chrono::nanoseconds>(after_sc - before_sc).count();
|
int64_t diff = std::chrono::duration_cast<std::chrono::nanoseconds>(after_sc - before_sc).count();
|
||||||
EXPECT_LE(diff, RCL_MS_TO_NS(10) + TOLERANCE);
|
EXPECT_LE(diff, RCL_MS_TO_NS(10) + TOLERANCE);
|
||||||
|
|
||||||
|
ret = rcl_clock_fini(&clock);
|
||||||
|
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Test rcl_wait_set_t with excess capacity works.
|
// Test rcl_wait_set_t with excess capacity works.
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue