diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index 30f6f1299..ea3c1ff72 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -57,6 +57,12 @@ typedef rcutils_duration_value_t rcl_duration_value_t; * RCL_SYSTEM_TIME reports the same value as the system clock. * * RCL_STEADY_TIME reports a value from a monotonically increasing clock. + * + * RCL_RAW_STEADY_TIME reports a value from a monotonic clock that is not + * adjusted for time jumps, such as those caused by NTP synchronization. + * This clock is suitable for measuring the time intervals without being + * affected by system time changes. This is true for the systems supporting + * the CLOCK_MONOTONIC_RAW clock type. */ typedef enum rcl_clock_type_e { @@ -67,7 +73,9 @@ typedef enum rcl_clock_type_e /// Use system time RCL_SYSTEM_TIME, /// Use a steady clock time - RCL_STEADY_TIME + RCL_STEADY_TIME, + /// Use a monotonic slew-free steady clock time + RCL_RAW_STEADY_TIME } rcl_clock_type_t; /// A duration of time, measured in nanoseconds and its source. @@ -406,6 +414,69 @@ rcl_ret_t rcl_steady_clock_fini( rcl_clock_t * clock); +/// Initialize a clock as a #RCL_RAW_STEADY_TIME time source. +/** + * This will allocate all necessary internal structures, and initialize variables. + * It is specifically setting up a #RCL_RAW_STEADY_TIME time source. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object. + * + * \param[in] clock the handle to the clock which is being initialized + * \param[in] allocator The allocator to use for allocations + * \return #RCL_RET_OK if the time source was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_raw_steady_clock_init( + rcl_clock_t * clock, + rcl_allocator_t * allocator); + +/// Finalize a clock as a #RCL_RAW_STEADY_TIME time source. +/** + * Finalize the clock as a #RCL_RAW_STEADY_TIME time source. + * + * This will deallocate all necessary internal structures, and clean up any variables. + * It is specifically setting up a steady time source. It is expected to be + * paired with the init fuction. + * + * This function is not thread-safe with any other function operating on the same + * clock object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object associated with the + * `clock` object. + * + * \param[in] clock the handle to the clock which is being initialized + * \return #RCL_RET_OK if the time source was successfully finalized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_raw_steady_clock_fini( + rcl_clock_t * clock); + /// Initialize a clock as a #RCL_SYSTEM_TIME time source. /** * Initialize the clock as a #RCL_SYSTEM_TIME time source. diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index 61e74c8e3..d3a51de24 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -39,6 +39,14 @@ rcl_get_steady_time(void * data, rcl_time_point_value_t * current_time) return rcutils_steady_time_now(current_time); } +// Implementation only +static rcl_ret_t +rcl_get_raw_steady_time(void * data, rcl_time_point_value_t * current_time) +{ + (void)data; // unused + return rcutils_raw_steady_time_now(current_time); +} + // Implementation only static rcl_ret_t rcl_get_system_time(void * data, rcl_time_point_value_t * current_time) @@ -111,6 +119,8 @@ rcl_clock_init( return rcl_system_clock_init(clock, allocator); case RCL_STEADY_TIME: return rcl_steady_clock_init(clock, allocator); + case RCL_RAW_STEADY_TIME: + return rcl_raw_steady_clock_init(clock, allocator); default: return RCL_RET_INVALID_ARGUMENT; } @@ -142,6 +152,8 @@ rcl_clock_fini( return rcl_system_clock_fini(clock); case RCL_STEADY_TIME: return rcl_steady_clock_fini(clock); + case RCL_RAW_STEADY_TIME: + return rcl_raw_steady_clock_fini(clock); case RCL_CLOCK_UNINITIALIZED: // fall through default: @@ -213,6 +225,32 @@ rcl_steady_clock_fini( return RCL_RET_OK; } +rcl_ret_t +rcl_raw_steady_clock_init( + rcl_clock_t * clock, + rcl_allocator_t * allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + rcl_init_generic_clock(clock, allocator); + clock->get_now = rcl_get_raw_steady_time; + clock->type = RCL_RAW_STEADY_TIME; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_raw_steady_clock_fini( + rcl_clock_t * clock) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + if (clock->type != RCL_RAW_STEADY_TIME) { + RCL_SET_ERROR_MSG("clock not of type RCL_RAW_STEADY_TIME"); + return RCL_RET_ERROR; + } + rcl_clock_generic_fini(clock); + return RCL_RET_OK; +} + rcl_ret_t rcl_system_clock_init( rcl_clock_t * clock, diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 145c1eae9..e2693327a 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -544,18 +544,27 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) } } - int64_t min_next_call_time[RCL_STEADY_TIME + 1]; - rcl_clock_t * clocks[RCL_STEADY_TIME + 1] = {NULL, NULL, NULL, NULL}; + int64_t min_next_call_time[RCL_RAW_STEADY_TIME + 1]; + rcl_clock_t * clocks[RCL_RAW_STEADY_TIME + 1] = {NULL, NULL, NULL, NULL}; // asserts to make sure nobody changes the ordering of RCL_ROS_TIME, - // RCL_SYSTEM_TIME and RCL_STEADY_TIME - static_assert(RCL_ROS_TIME < RCL_STEADY_TIME + 1, "RCL_ROS_TIME won't fit in the array"); - static_assert(RCL_SYSTEM_TIME < RCL_STEADY_TIME + 1, "RCL_SYSTEM_TIME won't fit in the array"); - static_assert(RCL_STEADY_TIME < RCL_STEADY_TIME + 1, "RCL_STEADY_TIME won't fit in the array"); + // RCL_SYSTEM_TIME, RCL_STEADY_TIME and RCL_RAW_STEADY_TIME + static_assert(RCL_ROS_TIME < RCL_RAW_STEADY_TIME + 1, "RCL_ROS_TIME won't fit in the array"); + static_assert( + RCL_SYSTEM_TIME < RCL_RAW_STEADY_TIME + 1, + "RCL_SYSTEM_TIME won't fit in the array"); + static_assert( + RCL_STEADY_TIME < RCL_RAW_STEADY_TIME + 1, + "RCL_STEADY_TIME won't fit in the array"); + static_assert( + RCL_RAW_STEADY_TIME < RCL_RAW_STEADY_TIME + 1, + "RCL_RAW_STEADY_TIME won't fit in the array"); min_next_call_time[RCL_ROS_TIME] = INT64_MAX; min_next_call_time[RCL_SYSTEM_TIME] = INT64_MAX; min_next_call_time[RCL_STEADY_TIME] = INT64_MAX; + min_next_call_time[RCL_RAW_STEADY_TIME] = INT64_MAX; + if (!is_non_blocking) { for (size_t t_idx = 0; t_idx < wait_set->impl->timer_index; ++t_idx) { @@ -623,7 +632,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) int64_t min_timeout = has_valid_timeout ? timeout : INT64_MAX; // determine the min timeout of all clocks - for (size_t i = RCL_ROS_TIME; i <= RCL_STEADY_TIME; i++) { + for (size_t i = RCL_ROS_TIME; i <= RCL_RAW_STEADY_TIME; i++) { if (clocks[i] == NULL) { continue; } diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 503bdfd50..773549daa 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -239,6 +239,22 @@ TEST(rcl_time, default_clock_instanciation) { }); ASSERT_TRUE(rcl_clock_valid(steady_clock)); + auto * raw_steady_clock = static_cast( + allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(raw_steady_clock, allocator.state); + }); + retval = rcl_raw_steady_clock_init(raw_steady_clock, &allocator); + EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ( + RCL_RET_OK, rcl_raw_steady_clock_fini( + raw_steady_clock)) << rcl_get_error_string().str; + }); + ASSERT_TRUE(rcl_clock_valid(raw_steady_clock)); + auto * system_clock = static_cast( allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state)); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( @@ -272,6 +288,9 @@ TEST(rcl_time, specific_clock_instantiation) { EXPECT_EQ( rcl_steady_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str; rcl_reset_error(); + EXPECT_EQ( + rcl_raw_steady_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); EXPECT_EQ( rcl_system_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str; rcl_reset_error(); @@ -303,6 +322,15 @@ TEST(rcl_time, specific_clock_instantiation) { ret = rcl_clock_fini(&steady_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } + { + rcl_clock_t raw_steady_clock; + rcl_ret_t ret = rcl_clock_init(RCL_RAW_STEADY_TIME, &raw_steady_clock, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(raw_steady_clock.type, RCL_RAW_STEADY_TIME) << + "Expected time source of type RCL_RAW_STEADY_TIME"; + ret = rcl_clock_fini(&raw_steady_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } { rcl_clock_t fail_clock; rcl_clock_type_t undefined_type = (rcl_clock_type_t) 130; @@ -344,6 +372,14 @@ TEST(rcl_time, rcl_clock_time_started) { ret = rcl_clock_fini(&steady_clock); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } + { + rcl_clock_t raw_steady_clock; + rcl_ret_t ret = rcl_clock_init(RCL_RAW_STEADY_TIME, &raw_steady_clock, &allocator); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ASSERT_TRUE(rcl_clock_time_started(&raw_steady_clock)); + ret = rcl_clock_fini(&raw_steady_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } } TEST(rcl_time, rcl_time_difference) {