Clock has multiple time jump callbacks (#284)
* Clock has multiple time jump callbacks Replaces pre_update and post_update Adds callbacks when ROS time is activated or deactivated Add rcl_clock_change_t, rcl_time_jump_t, rcl_jump_threshold_t Add rcl_clock_add_jump_callback() Add rcl_clock_remove_jump_callback() * Fini generic clock after confirming correct clock type * test_time no ASAN detections
This commit is contained in:
Shane Loretz
GitHub
parent
ba9f24b852
commit
ceff88b974
3 changed files with 555 additions and 54 deletions
|
|
@ -54,12 +54,73 @@ typedef enum rcl_clock_type_t
|
|||
RCL_STEADY_TIME
|
||||
} rcl_clock_type_t;
|
||||
|
||||
/// A duration of time, measured in nanoseconds and its source.
|
||||
typedef struct rcl_duration_t
|
||||
{
|
||||
rcl_duration_value_t nanoseconds;
|
||||
} rcl_duration_t;
|
||||
|
||||
/// Enumeration to describe the type of time jump.
|
||||
typedef enum rcl_clock_change_t
|
||||
{
|
||||
/// The source before and after the jump is ROS_TIME.
|
||||
RCL_ROS_TIME_NO_CHANGE = 1,
|
||||
/// The source switched to ROS_TIME from SYSTEM_TIME.
|
||||
RCL_ROS_TIME_ACTIVATED = 2,
|
||||
/// The source switched to SYSTEM_TIME from ROS_TIME.
|
||||
RCL_ROS_TIME_DEACTIVATED = 3,
|
||||
/// The source before and after the jump is SYSTEM_TIME.
|
||||
RCL_SYSTEM_TIME_NO_CHANGE = 4
|
||||
} rcl_clock_change_t;
|
||||
|
||||
/// Struct to describe a jump in time.
|
||||
typedef struct rcl_time_jump_t
|
||||
{
|
||||
/// Indicate whether or not the source of time changed.
|
||||
rcl_clock_change_t clock_change;
|
||||
/// The new time minus the last time before the jump.
|
||||
rcl_duration_t delta;
|
||||
} rcl_time_jump_t;
|
||||
|
||||
/// Signature of a time jump callback.
|
||||
/// \param[in] time_jump A description of the jump in time.
|
||||
/// \param[in] before_jump Every jump callback is called twice: once before the clock changes and
|
||||
/// once after. This is true the first call and false the second.
|
||||
/// \param[in] user_data A pointer given at callback registration which is passed to the callback.
|
||||
typedef void (* rcl_jump_callback_t)(
|
||||
const struct rcl_time_jump_t * time_jump,
|
||||
bool before_jump,
|
||||
void * user_data);
|
||||
|
||||
/// Describe the prerequisites for calling a time jump callback.
|
||||
typedef struct rcl_jump_threshold_t
|
||||
{
|
||||
/// True to call callback when the clock type changes.
|
||||
bool on_clock_change;
|
||||
/// A positive duration indicating the minimum jump forwards to be considered exceeded, or zero
|
||||
/// to disable.
|
||||
rcl_duration_t min_forward;
|
||||
/// A negative duration indicating the minimum jump backwards to be considered exceeded, or zero
|
||||
/// to disable.
|
||||
rcl_duration_t min_backward;
|
||||
} rcl_jump_threshold_t;
|
||||
|
||||
/// Struct to describe an added callback.
|
||||
typedef struct rcl_jump_callback_info_t
|
||||
{
|
||||
rcl_jump_callback_t callback;
|
||||
rcl_jump_threshold_t threshold;
|
||||
void * user_data;
|
||||
} rcl_jump_callback_info_t;
|
||||
|
||||
/// Encapsulation of a time source.
|
||||
typedef struct rcl_clock_t
|
||||
{
|
||||
enum rcl_clock_type_t type;
|
||||
void (* pre_update)(void);
|
||||
void (* post_update)(void);
|
||||
/// An array of added jump callbacks.
|
||||
rcl_jump_callback_info_t * jump_callbacks;
|
||||
/// Number of callbacks in jump_callbacks.
|
||||
size_t num_jump_callbacks;
|
||||
rcl_ret_t (* get_now)(void * data, rcl_time_point_value_t * now);
|
||||
// void (*set_now) (rcl_time_point_value_t);
|
||||
void * data;
|
||||
|
|
@ -73,12 +134,6 @@ typedef struct rcl_time_point_t
|
|||
rcl_clock_type_t clock_type;
|
||||
} rcl_time_point_t;
|
||||
|
||||
/// A duration of time, measured in nanoseconds and its source.
|
||||
typedef struct rcl_duration_t
|
||||
{
|
||||
rcl_duration_value_t nanoseconds;
|
||||
} rcl_duration_t;
|
||||
|
||||
// typedef struct rcl_rate_t
|
||||
// {
|
||||
// rcl_time_point_value_t trigger_time;
|
||||
|
|
@ -358,6 +413,44 @@ rcl_ret_t
|
|||
rcl_set_ros_time_override(
|
||||
rcl_clock_t * clock, rcl_time_point_value_t time_value);
|
||||
|
||||
/// Add a callback to be called when a time jump exceeds a threshold.
|
||||
/**
|
||||
* The callback is called twice when the threshold is exceeded: once before the clock is
|
||||
* updated, and once after.
|
||||
* The user_data pointer is passed to the callback as the last argument.
|
||||
* A callback and user_data pair must be unique among the callbacks added to a clock.
|
||||
*
|
||||
* \param[in] clock A clock to add a jump callback to.
|
||||
* \param[in] threshold Criteria indicating when to call the callback.
|
||||
* \param[in] callback A callback to call.
|
||||
* \param[in] user_data A pointer to be passed to the callback.
|
||||
* \return `RCL_RET_OK` if the callback was added successfully, or
|
||||
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||
* \return `RCL_RET_ERROR` an unspecified error occurs.
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_clock_add_jump_callback(
|
||||
rcl_clock_t * clock, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback,
|
||||
void * user_data);
|
||||
|
||||
/// Remove a previously added time jump callback.
|
||||
/**
|
||||
* \param[in] clock The clock to remove a jump callback from.
|
||||
* \param[in] threshold Criteria indicating when to call callback.
|
||||
* \param[in] callback The callback to call.
|
||||
* \param[in] user_data A pointer to be passed to the callback.
|
||||
* \return `RCL_RET_OK` if the callback was added successfully, or
|
||||
* \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
|
||||
* \return `RCL_RET_ERROR` the callback was not found or an unspecified error occurs.
|
||||
*/
|
||||
RCL_PUBLIC
|
||||
RCL_WARN_UNUSED
|
||||
rcl_ret_t
|
||||
rcl_clock_remove_jump_callback(
|
||||
rcl_clock_t * clock, rcl_jump_callback_t callback, void * user_data);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -52,8 +52,8 @@ void
|
|||
rcl_init_generic_clock(rcl_clock_t * clock)
|
||||
{
|
||||
clock->type = RCL_CLOCK_UNINITIALIZED;
|
||||
clock->pre_update = NULL;
|
||||
clock->post_update = NULL;
|
||||
clock->jump_callbacks = NULL;
|
||||
clock->num_jump_callbacks = 0u;
|
||||
clock->get_now = NULL;
|
||||
clock->data = NULL;
|
||||
}
|
||||
|
|
@ -106,6 +106,18 @@ rcl_clock_init(
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
_rcl_clock_generic_fini(
|
||||
rcl_clock_t * clock)
|
||||
{
|
||||
// Internal function; assume caller has already checked that clock is valid.
|
||||
if (clock->num_jump_callbacks > 0) {
|
||||
clock->num_jump_callbacks = 0;
|
||||
clock->allocator.deallocate(clock->jump_callbacks, clock->allocator.state);
|
||||
clock->jump_callbacks = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_clock_fini(
|
||||
rcl_clock_t * clock)
|
||||
|
|
@ -155,6 +167,7 @@ rcl_ros_clock_fini(
|
|||
RCL_SET_ERROR_MSG("clock not of type RCL_ROS_TIME", rcl_get_default_allocator());
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
_rcl_clock_generic_fini(clock);
|
||||
if (!clock->data) {
|
||||
RCL_SET_ERROR_MSG("clock data invalid", rcl_get_default_allocator());
|
||||
return RCL_RET_ERROR;
|
||||
|
|
@ -186,6 +199,7 @@ rcl_steady_clock_fini(
|
|||
RCL_SET_ERROR_MSG("clock not of type RCL_STEADY_TIME", rcl_get_default_allocator());
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
_rcl_clock_generic_fini(clock);
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
|
|
@ -212,6 +226,7 @@ rcl_system_clock_fini(
|
|||
RCL_SET_ERROR_MSG("clock not of type RCL_SYSTEM_TIME", rcl_get_default_allocator());
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
_rcl_clock_generic_fini(clock);
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
|
|
@ -249,6 +264,27 @@ rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value
|
|||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
void
|
||||
_rcl_clock_call_callbacks(
|
||||
rcl_clock_t * clock, const rcl_time_jump_t * time_jump, bool before_jump)
|
||||
{
|
||||
// Internal function; assume parameters are valid.
|
||||
bool is_clock_change = time_jump->clock_change == RCL_ROS_TIME_ACTIVATED ||
|
||||
time_jump->clock_change == RCL_ROS_TIME_DEACTIVATED;
|
||||
for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) {
|
||||
rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]);
|
||||
if (
|
||||
(is_clock_change && info->threshold.on_clock_change) ||
|
||||
(time_jump->delta.nanoseconds < 0 &&
|
||||
time_jump->delta.nanoseconds <= info->threshold.min_backward.nanoseconds) ||
|
||||
(time_jump->delta.nanoseconds > 0 &&
|
||||
time_jump->delta.nanoseconds >= info->threshold.min_forward.nanoseconds))
|
||||
{
|
||||
info->callback(time_jump, before_jump, info->user_data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_enable_ros_time_override(rcl_clock_t * clock)
|
||||
{
|
||||
|
|
@ -264,7 +300,14 @@ rcl_enable_ros_time_override(rcl_clock_t * clock)
|
|||
rcl_get_default_allocator())
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
if (!storage->active) {
|
||||
rcl_time_jump_t time_jump;
|
||||
time_jump.delta.nanoseconds = 0;
|
||||
time_jump.clock_change = RCL_ROS_TIME_ACTIVATED;
|
||||
_rcl_clock_call_callbacks(clock, &time_jump, true);
|
||||
storage->active = true;
|
||||
_rcl_clock_call_callbacks(clock, &time_jump, false);
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
|
|
@ -284,7 +327,14 @@ rcl_disable_ros_time_override(rcl_clock_t * clock)
|
|||
rcl_get_default_allocator())
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
if (storage->active) {
|
||||
rcl_time_jump_t time_jump;
|
||||
time_jump.delta.nanoseconds = 0;
|
||||
time_jump.clock_change = RCL_ROS_TIME_DEACTIVATED;
|
||||
_rcl_clock_call_callbacks(clock, &time_jump, true);
|
||||
storage->active = false;
|
||||
_rcl_clock_call_callbacks(clock, &time_jump, false);
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
|
|
@ -323,14 +373,107 @@ rcl_set_ros_time_override(
|
|||
"Clock is not of type RCL_ROS_TIME, cannot set time override.", rcl_get_default_allocator())
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
rcl_ros_clock_storage_t * storage = \
|
||||
(rcl_ros_clock_storage_t *)clock->data;
|
||||
if (storage->active && clock->pre_update) {
|
||||
clock->pre_update();
|
||||
rcl_time_jump_t time_jump;
|
||||
rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data;
|
||||
if (storage->active) {
|
||||
time_jump.clock_change = RCL_ROS_TIME_NO_CHANGE;
|
||||
rcl_time_point_value_t current_time;
|
||||
rcl_ret_t ret = rcl_get_ros_time(storage, ¤t_time);
|
||||
if (RCL_RET_OK != ret) {
|
||||
return ret;
|
||||
}
|
||||
time_jump.delta.nanoseconds = time_value - current_time;
|
||||
_rcl_clock_call_callbacks(clock, &time_jump, true);
|
||||
}
|
||||
rcl_atomic_store(&(storage->current_time), time_value);
|
||||
if (storage->active && clock->post_update) {
|
||||
clock->post_update();
|
||||
if (storage->active) {
|
||||
_rcl_clock_call_callbacks(clock, &time_jump, false);
|
||||
}
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_clock_add_jump_callback(
|
||||
rcl_clock_t * clock, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback,
|
||||
void * user_data)
|
||||
{
|
||||
// Make sure parameters are valid
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||
RCL_CHECK_ALLOCATOR_WITH_MSG(&(clock->allocator), "invalid allocator",
|
||||
return RCL_RET_INVALID_ARGUMENT);
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT, clock->allocator);
|
||||
if (threshold.min_forward.nanoseconds < 0) {
|
||||
RCL_SET_ERROR_MSG("forward jump threshold must be positive or zero", clock->allocator);
|
||||
return RCL_RET_INVALID_ARGUMENT;
|
||||
}
|
||||
if (threshold.min_backward.nanoseconds > 0) {
|
||||
RCL_SET_ERROR_MSG("backward jump threshold must be negative or zero", clock->allocator);
|
||||
return RCL_RET_INVALID_ARGUMENT;
|
||||
}
|
||||
|
||||
// Callback/user_data pair must be unique
|
||||
for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) {
|
||||
const rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]);
|
||||
if (info->callback == callback && info->user_data == user_data) {
|
||||
RCL_SET_ERROR_MSG("callback/user_data are already added to this clock", clock->allocator);
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
// Add the new callback, increasing the size of the callback list
|
||||
rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate(
|
||||
clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * (clock->num_jump_callbacks + 1),
|
||||
clock->allocator.state);
|
||||
if (NULL == callbacks) {
|
||||
RCL_SET_ERROR_MSG("Failed to realloc jump callbacks", clock->allocator);
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
clock->jump_callbacks = callbacks;
|
||||
clock->jump_callbacks[clock->num_jump_callbacks].callback = callback;
|
||||
clock->jump_callbacks[clock->num_jump_callbacks].threshold = threshold;
|
||||
clock->jump_callbacks[clock->num_jump_callbacks].user_data = user_data;
|
||||
++(clock->num_jump_callbacks);
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
||||
rcl_ret_t
|
||||
rcl_clock_remove_jump_callback(
|
||||
rcl_clock_t * clock, rcl_jump_callback_t callback, void * user_data)
|
||||
{
|
||||
// Make sure parameters are valid
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
|
||||
RCL_CHECK_ALLOCATOR_WITH_MSG(&(clock->allocator), "invalid allocator",
|
||||
return RCL_RET_INVALID_ARGUMENT);
|
||||
RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT, clock->allocator);
|
||||
|
||||
// Delete callback if found, moving all callbacks after back one
|
||||
bool found_callback = false;
|
||||
for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) {
|
||||
const rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]);
|
||||
if (found_callback) {
|
||||
clock->jump_callbacks[cb_idx - 1] = *info;
|
||||
} else if (info->callback == callback && info->user_data == user_data) {
|
||||
found_callback = true;
|
||||
}
|
||||
}
|
||||
if (!found_callback) {
|
||||
RCL_SET_ERROR_MSG("jump callback was not found", clock->allocator);
|
||||
return RCL_RET_ERROR;
|
||||
}
|
||||
|
||||
// Shrink size of the callback array
|
||||
if (clock->num_jump_callbacks == 1) {
|
||||
clock->allocator.deallocate(clock->jump_callbacks, clock->allocator.state);
|
||||
} else {
|
||||
rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate(
|
||||
clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * (clock->num_jump_callbacks - 1),
|
||||
clock->allocator.state);
|
||||
if (NULL == callbacks) {
|
||||
RCL_SET_ERROR_MSG("Failed to shrink jump callbacks", clock->allocator);
|
||||
return RCL_RET_BAD_ALLOC;
|
||||
}
|
||||
clock->jump_callbacks = callbacks;
|
||||
}
|
||||
--(clock->num_jump_callbacks);
|
||||
return RCL_RET_OK;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -60,7 +60,10 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_ove
|
|||
rcl_clock_t ros_clock;
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
|
||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
rcl_ret_t ret;
|
||||
// Check for invalid argument error condition (allowed to alloc).
|
||||
|
|
@ -169,11 +172,17 @@ TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_init_for_clock_a
|
|||
// Check for normal operation (not allowed to alloc).
|
||||
rcl_clock_t source;
|
||||
ret = rcl_ros_clock_init(&source, &allocator);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&source)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
rcl_clock_t ros_clock;
|
||||
rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
|
||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
}
|
||||
|
||||
TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_ros_clock_initially_zero) {
|
||||
|
|
@ -197,26 +206,45 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), clock_validation) {
|
|||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_ret_t ret;
|
||||
ret = rcl_ros_clock_init(&uninitialized, &allocator);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&uninitialized)) << rcl_get_error_string_safe();
|
||||
});
|
||||
}
|
||||
|
||||
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_clock_instanciation) {
|
||||
rcl_clock_t ros_clock;
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator);
|
||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
ASSERT_TRUE(rcl_clock_valid(&ros_clock));
|
||||
|
||||
rcl_clock_t * steady_clock = reinterpret_cast<rcl_clock_t *>(
|
||||
allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(steady_clock, allocator.state);
|
||||
});
|
||||
|
||||
retval = rcl_steady_clock_init(steady_clock, &allocator);
|
||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_steady_clock_fini(steady_clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
ASSERT_TRUE(rcl_clock_valid(steady_clock));
|
||||
|
||||
rcl_clock_t * system_clock = reinterpret_cast<rcl_clock_t *>(
|
||||
allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(system_clock, allocator.state);
|
||||
});
|
||||
retval = rcl_system_clock_init(system_clock, &allocator);
|
||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_system_clock_fini(system_clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
ASSERT_TRUE(rcl_clock_valid(system_clock));
|
||||
}
|
||||
|
||||
|
|
@ -263,8 +291,14 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) {
|
|||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_clock_t * ros_clock =
|
||||
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(ros_clock, allocator.state);
|
||||
});
|
||||
rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
|
||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(ros_clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
EXPECT_TRUE(ros_clock != nullptr);
|
||||
EXPECT_TRUE(ros_clock->data != nullptr);
|
||||
EXPECT_EQ(ros_clock->type, RCL_ROS_TIME);
|
||||
|
|
@ -292,8 +326,14 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference_signed) {
|
|||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_clock_t * ros_clock =
|
||||
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(ros_clock, allocator.state);
|
||||
});
|
||||
rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
|
||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(ros_clock)) << rcl_get_error_string_safe();
|
||||
});
|
||||
|
||||
rcl_time_point_t a, b;
|
||||
a.nanoseconds = RCL_S_TO_NS(0LL) + 0LL;
|
||||
|
|
@ -339,19 +379,22 @@ TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference_signed) {
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
static bool pre_callback_called = false;
|
||||
static bool post_callback_called = false;
|
||||
|
||||
void pre_callback(void)
|
||||
void clock_callback(
|
||||
const struct rcl_time_jump_t * time_jump,
|
||||
bool before_jump,
|
||||
void * user_data)
|
||||
{
|
||||
if (before_jump) {
|
||||
pre_callback_called = true;
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
}
|
||||
void post_callback(void)
|
||||
{
|
||||
} else {
|
||||
EXPECT_TRUE(pre_callback_called);
|
||||
post_callback_called = true;
|
||||
}
|
||||
*(static_cast<rcl_time_jump_t *>(user_data)) = *time_jump;
|
||||
}
|
||||
|
||||
void reset_callback_triggers(void)
|
||||
|
|
@ -360,48 +403,270 @@ void reset_callback_triggers(void)
|
|||
post_callback_called = false;
|
||||
}
|
||||
|
||||
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_update_callbacks) {
|
||||
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_clock_change_callbacks) {
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_clock_t * ros_clock =
|
||||
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(ros_clock, allocator.state);
|
||||
});
|
||||
rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
|
||||
EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock));
|
||||
});
|
||||
rcl_time_point_value_t query_now;
|
||||
rcl_ret_t ret;
|
||||
rcl_time_point_value_t set_point = 1000000000ull;
|
||||
|
||||
// set callbacks
|
||||
ros_clock->pre_update = pre_callback;
|
||||
ros_clock->post_update = post_callback;
|
||||
rcl_time_jump_t time_jump;
|
||||
rcl_jump_threshold_t threshold;
|
||||
threshold.on_clock_change = true;
|
||||
threshold.min_forward.nanoseconds = 0;
|
||||
threshold.min_backward.nanoseconds = 0;
|
||||
ASSERT_EQ(RCL_RET_OK,
|
||||
rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) <<
|
||||
rcl_get_error_string_safe();
|
||||
reset_callback_triggers();
|
||||
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// Query it to do something different. no changes expected
|
||||
// Query time, no changes expected.
|
||||
ret = rcl_clock_get_now(ros_clock, &query_now);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// Set the time before it's enabled. Should be no callbacks
|
||||
ret = rcl_set_ros_time_override(ros_clock, set_point);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// enable
|
||||
// Clock change callback called when ROS time is enabled
|
||||
ret = rcl_enable_ros_time_override(ros_clock);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_TRUE(pre_callback_called);
|
||||
EXPECT_TRUE(post_callback_called);
|
||||
EXPECT_EQ(RCL_ROS_TIME_ACTIVATED, time_jump.clock_change);
|
||||
reset_callback_triggers();
|
||||
|
||||
// Clock change callback not called because ROS time is already enabled.
|
||||
ret = rcl_enable_ros_time_override(ros_clock);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
reset_callback_triggers();
|
||||
|
||||
// Clock change callback called when ROS time is disabled
|
||||
ret = rcl_disable_ros_time_override(ros_clock);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_TRUE(pre_callback_called);
|
||||
EXPECT_TRUE(post_callback_called);
|
||||
EXPECT_EQ(RCL_ROS_TIME_DEACTIVATED, time_jump.clock_change);
|
||||
reset_callback_triggers();
|
||||
|
||||
// Clock change callback not called because ROS time is already disabled.
|
||||
ret = rcl_disable_ros_time_override(ros_clock);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
reset_callback_triggers();
|
||||
}
|
||||
|
||||
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_forward_jump_callbacks) {
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_clock_t * ros_clock =
|
||||
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(ros_clock, allocator.state);
|
||||
});
|
||||
rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
|
||||
ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock));
|
||||
});
|
||||
|
||||
rcl_ret_t ret;
|
||||
rcl_time_point_value_t set_point1 = 1000L * 1000L * 1000L;
|
||||
rcl_time_point_value_t set_point2 = 2L * 1000L * 1000L * 1000L;
|
||||
|
||||
rcl_time_jump_t time_jump;
|
||||
rcl_jump_threshold_t threshold;
|
||||
threshold.on_clock_change = false;
|
||||
threshold.min_forward.nanoseconds = 1;
|
||||
threshold.min_backward.nanoseconds = 0;
|
||||
ASSERT_EQ(RCL_RET_OK,
|
||||
rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) <<
|
||||
rcl_get_error_string_safe();
|
||||
reset_callback_triggers();
|
||||
|
||||
// Set the time before it's enabled. Should be no callbacks
|
||||
ret = rcl_set_ros_time_override(ros_clock, set_point1);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// enable no callbacks
|
||||
ret = rcl_enable_ros_time_override(ros_clock);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// Set the time now that it's enabled, now get callbacks
|
||||
ret = rcl_set_ros_time_override(ros_clock, set_point);
|
||||
ret = rcl_set_ros_time_override(ros_clock, set_point2);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
|
||||
EXPECT_TRUE(pre_callback_called);
|
||||
EXPECT_TRUE(post_callback_called);
|
||||
EXPECT_EQ(set_point2 - set_point1, time_jump.delta.nanoseconds);
|
||||
EXPECT_EQ(RCL_ROS_TIME_NO_CHANGE, time_jump.clock_change);
|
||||
reset_callback_triggers();
|
||||
|
||||
// Setting same value as previous time, not a jump
|
||||
ret = rcl_set_ros_time_override(ros_clock, set_point2);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// disable no callbacks
|
||||
ret = rcl_disable_ros_time_override(ros_clock);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
}
|
||||
|
||||
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_backward_jump_callbacks) {
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_clock_t * ros_clock =
|
||||
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(ros_clock, allocator.state);
|
||||
});
|
||||
rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator);
|
||||
ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(ros_clock));
|
||||
});
|
||||
rcl_ret_t ret;
|
||||
rcl_time_point_value_t set_point1 = 1000000000;
|
||||
rcl_time_point_value_t set_point2 = 2000000000;
|
||||
|
||||
rcl_time_jump_t time_jump;
|
||||
rcl_jump_threshold_t threshold;
|
||||
threshold.on_clock_change = false;
|
||||
threshold.min_forward.nanoseconds = 0;
|
||||
threshold.min_backward.nanoseconds = -1;
|
||||
ASSERT_EQ(RCL_RET_OK,
|
||||
rcl_clock_add_jump_callback(ros_clock, threshold, clock_callback, &time_jump)) <<
|
||||
rcl_get_error_string_safe();
|
||||
reset_callback_triggers();
|
||||
|
||||
// Set the time before it's enabled. Should be no callbacks
|
||||
ret = rcl_set_ros_time_override(ros_clock, set_point2);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// enable no callbacks
|
||||
ret = rcl_enable_ros_time_override(ros_clock);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// Set the time now that it's enabled, now get callbacks
|
||||
ret = rcl_set_ros_time_override(ros_clock, set_point1);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_TRUE(pre_callback_called);
|
||||
EXPECT_TRUE(post_callback_called);
|
||||
EXPECT_EQ(set_point1 - set_point2, time_jump.delta.nanoseconds);
|
||||
EXPECT_EQ(RCL_ROS_TIME_NO_CHANGE, time_jump.clock_change);
|
||||
reset_callback_triggers();
|
||||
|
||||
// Setting same value as previous time, not a jump
|
||||
ret = rcl_set_ros_time_override(ros_clock, set_point1);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
|
||||
// disable no callbacks
|
||||
ret = rcl_disable_ros_time_override(ros_clock);
|
||||
EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string_safe();
|
||||
EXPECT_FALSE(pre_callback_called);
|
||||
EXPECT_FALSE(post_callback_called);
|
||||
}
|
||||
|
||||
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_add_jump_callback) {
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_clock_t * clock =
|
||||
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(clock, allocator.state);
|
||||
});
|
||||
rcl_ret_t retval = rcl_ros_clock_init(clock, &allocator);
|
||||
ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(clock));
|
||||
});
|
||||
|
||||
rcl_jump_threshold_t threshold;
|
||||
threshold.on_clock_change = false;
|
||||
threshold.min_forward.nanoseconds = 0;
|
||||
threshold.min_backward.nanoseconds = 0;
|
||||
rcl_jump_callback_t cb = reinterpret_cast<rcl_jump_callback_t>(0xBEEF);
|
||||
void * user_data = reinterpret_cast<void *>(0xCAFE);
|
||||
|
||||
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_clock_add_jump_callback(clock, threshold, NULL, NULL));
|
||||
rcl_reset_error();
|
||||
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, NULL)) <<
|
||||
rcl_get_error_string_safe();
|
||||
EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(clock, threshold, cb, NULL));
|
||||
rcl_reset_error();
|
||||
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data)) <<
|
||||
rcl_get_error_string_safe();
|
||||
EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(clock, threshold, cb, user_data));
|
||||
rcl_reset_error();
|
||||
|
||||
EXPECT_EQ(2u, clock->num_jump_callbacks);
|
||||
}
|
||||
|
||||
TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_remove_jump_callback) {
|
||||
rcl_allocator_t allocator = rcl_get_default_allocator();
|
||||
rcl_clock_t * clock =
|
||||
reinterpret_cast<rcl_clock_t *>(allocator.allocate(sizeof(rcl_clock_t), allocator.state));
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
allocator.deallocate(clock, allocator.state);
|
||||
});
|
||||
rcl_ret_t retval = rcl_ros_clock_init(clock, &allocator);
|
||||
ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string_safe();
|
||||
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(clock));
|
||||
});
|
||||
|
||||
rcl_jump_threshold_t threshold;
|
||||
threshold.on_clock_change = false;
|
||||
threshold.min_forward.nanoseconds = 0;
|
||||
threshold.min_backward.nanoseconds = 0;
|
||||
rcl_jump_callback_t cb = reinterpret_cast<rcl_jump_callback_t>(0xBEEF);
|
||||
void * user_data1 = reinterpret_cast<void *>(0xCAFE);
|
||||
void * user_data2 = reinterpret_cast<void *>(0xFACE);
|
||||
void * user_data3 = reinterpret_cast<void *>(0xBEAD);
|
||||
void * user_data4 = reinterpret_cast<void *>(0xDEED);
|
||||
|
||||
EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_clock_remove_jump_callback(clock, NULL, NULL));
|
||||
rcl_reset_error();
|
||||
EXPECT_EQ(RCL_RET_ERROR, rcl_clock_remove_jump_callback(clock, cb, NULL));
|
||||
rcl_reset_error();
|
||||
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data1)) <<
|
||||
rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data2)) <<
|
||||
rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data3)) <<
|
||||
rcl_get_error_string_safe();
|
||||
ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(clock, threshold, cb, user_data4)) <<
|
||||
rcl_get_error_string_safe();
|
||||
EXPECT_EQ(4u, clock->num_jump_callbacks);
|
||||
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data3));
|
||||
EXPECT_EQ(3u, clock->num_jump_callbacks);
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data4));
|
||||
EXPECT_EQ(2u, clock->num_jump_callbacks);
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data1));
|
||||
EXPECT_EQ(1u, clock->num_jump_callbacks);
|
||||
EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(clock, cb, user_data2));
|
||||
EXPECT_EQ(0u, clock->num_jump_callbacks);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue