From ceff88b974303c51cba589f9992dd5fcc253ca57 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 27 Aug 2018 15:37:06 -0700 Subject: [PATCH] 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 --- rcl/include/rcl/time.h | 109 ++++++++++++- rcl/src/rcl/time.c | 163 +++++++++++++++++-- rcl/test/rcl/test_time.cpp | 321 +++++++++++++++++++++++++++++++++---- 3 files changed, 547 insertions(+), 46 deletions(-) diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index c6251dbdc..c5dea13d1 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -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 diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index b033aadf0..3beecb8e8 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -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; } - storage->active = true; + 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; } - storage->active = false; + 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; } diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 8a6e4052f..f94210169 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -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( 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( 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(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(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) { - pre_callback_called = true; - EXPECT_FALSE(post_callback_called); -} -void post_callback(void) -{ - EXPECT_TRUE(pre_callback_called); - post_callback_called = true; + if (before_jump) { + pre_callback_called = true; + EXPECT_FALSE(post_callback_called); + } else { + EXPECT_TRUE(pre_callback_called); + post_callback_called = true; + } + *(static_cast(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(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(); + + // 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); + + // 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(); - // Query it to do something different. no changes expected - ret = rcl_clock_get_now(ros_clock, &query_now); + // 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(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_point); + 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 + // 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(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(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(0xBEEF); + void * user_data = reinterpret_cast(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(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(0xBEEF); + void * user_data1 = reinterpret_cast(0xCAFE); + void * user_data2 = reinterpret_cast(0xFACE); + void * user_data3 = reinterpret_cast(0xBEAD); + void * user_data4 = reinterpret_cast(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); }