diff --git a/rcl/include/rcl/time.h b/rcl/include/rcl/time.h index 359bc139a..e36991aa3 100644 --- a/rcl/include/rcl/time.h +++ b/rcl/include/rcl/time.h @@ -272,12 +272,12 @@ rcl_ret_t rcl_difference_times( 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. * \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_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_ERROR` an unspecified error occur. @@ -285,7 +285,7 @@ rcl_difference_times( RCL_PUBLIC RCL_WARN_UNUSED 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. diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index 069c1dac6..0861b3775 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -28,6 +28,7 @@ extern "C" #include "rcl/types.h" #include "rmw/rmw.h" +struct rcl_clock_t; struct rcl_timer_impl_t; /// Structure which encapsulates a ROS Timer. @@ -60,7 +61,7 @@ rcl_get_zero_initialized_timer(void); /// 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 * 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 * 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). * 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... * } * + * 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_ret_t ret = rcl_timer_init( - * &timer, RCL_MS_TO_NS(100), my_timer_callback, rcl_get_default_allocator()); + * ret = rcl_timer_init( + * &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 * ret = rcl_timer_fini(&timer); * // ... error handling @@ -123,6 +132,7 @@ rcl_get_zero_initialized_timer(void); * [3] if `atomic_is_lock_free()` returns true for `atomic_bool` * * \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] callback the user defined function to be called every period * \param[in] allocator the allocator to use for allocations @@ -137,6 +147,7 @@ RCL_WARN_UNUSED rcl_ret_t rcl_timer_init( rcl_timer_t * timer, + rcl_clock_t * clock, int64_t period, const rcl_timer_callback_t callback, rcl_allocator_t allocator); @@ -216,6 +227,32 @@ RCL_WARN_UNUSED rcl_ret_t 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 *. + * + *
+ * 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. /** * 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. /** - * 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. * @@ -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. /** * 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. * diff --git a/rcl/src/rcl/time.c b/rcl/src/rcl/time.c index 17ef21a27..b3fbe8f76 100644 --- a/rcl/src/rcl/time.c +++ b/rcl/src/rcl/time.c @@ -110,6 +110,7 @@ rcl_ret_t rcl_clock_fini( rcl_clock_t * clock) { + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); switch (clock->type) { case RCL_ROS_TIME: return rcl_ros_clock_fini(clock); @@ -226,12 +227,13 @@ rcl_difference_times( } 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) { - return clock->get_now(clock->data, - &(time_point->nanoseconds)); + return clock->get_now(clock->data, time_point_value); } RCL_SET_ERROR_MSG( "clock is not initialized or does not have get_now registered.", diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 8ff6ac230..087d40f31 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -28,6 +28,8 @@ extern "C" typedef struct rcl_timer_impl_t { + // The clock providing time. + rcl_clock_t * clock; // The user supplied callback. atomic_uintptr_t callback; // This is a duration in nanoseconds. @@ -50,26 +52,29 @@ rcl_get_zero_initialized_timer() rcl_ret_t rcl_timer_init( rcl_timer_t * timer, + rcl_clock_t * clock, int64_t period, const rcl_timer_callback_t callback, rcl_allocator_t allocator) { 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(clock, RCL_RET_INVALID_ARGUMENT, allocator); RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Initializing timer with period: %" PRIu64 "ns", period) if (timer->impl) { RCL_SET_ERROR_MSG("timer already initailized, or memory was uninitialized", allocator); return RCL_RET_ALREADY_INIT; } - rcl_time_point_value_t now_steady; - rcl_ret_t now_ret = rcutils_steady_time_now(&now_steady); + rcl_time_point_value_t now; + rcl_ret_t now_ret = rcl_clock_get_now(clock, &now); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } rcl_timer_impl_t impl; + impl.clock = clock; atomic_init(&impl.callback, (uintptr_t)callback); 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); impl.allocator = allocator; 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; } +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_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); return RCL_RET_TIMER_CANCELED; } - rcl_time_point_value_t now_steady; - rcl_ret_t now_ret = rcutils_steady_time_now(&now_steady); + rcl_time_point_value_t now; + rcl_ret_t now_ret = rcl_clock_get_now(timer->impl->clock, &now); if (now_ret != RCL_RET_OK) { return now_ret; // rcl error state should already be set. } 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)rcl_atomic_load_uintptr_t(&timer->impl->callback); 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); } return RCL_RET_OK; diff --git a/rcl/test/rcl/test_time.cpp b/rcl/test/rcl/test_time.cpp index 57ac4ad61..98196aa84 100644 --- a/rcl/test/rcl/test_time.cpp +++ b/rcl/test/rcl/test_time.cpp @@ -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); EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string_safe(); rcl_reset_error(); - rcl_time_point_t query_now; + rcl_time_point_value_t query_now; bool is_enabled; ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled); 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); }); 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). ret = rcl_clock_get_now(&ros_clock, &query_now); 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(); auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); 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; 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(); auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); 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; 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 ret = rcl_clock_get_now(&ros_clock, &query_now); 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 ret = rcl_disable_ros_time_override(&ros_clock); 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(); auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); 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; 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(allocator.allocate(sizeof(rcl_clock_t), allocator.state)); rcl_ret_t retval = rcl_ros_clock_init(ros_clock, &allocator); 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_time_point_value_t set_point = 1000000000ull; diff --git a/rcl/test/rcl/test_timer.cpp b/rcl/test/rcl/test_timer.cpp index 2b8971e4d..66061a8bf 100644 --- a/rcl/test/rcl/test_timer.cpp +++ b/rcl/test/rcl/test_timer.cpp @@ -50,13 +50,19 @@ class TestTimerFixture : public ::testing::Test TEST_F(TestTimerFixture, test_two_timers) { 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 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(); - 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(); 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_FALSE(is_ready); 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) { 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 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(); - 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(); 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_FALSE(is_ready); 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) { 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(); - 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(); 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_FALSE(is_ready); 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) { 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(); - 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(); 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_FALSE(is_ready); ASSERT_EQ(0, nonnull_timers); + + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); } diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index b09431878..081277b73 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -103,8 +103,13 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) { ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); 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(); - 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(); ret = rcl_wait_set_add_timer(&wait_set, &timer); 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); 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(); - 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(); ret = rcl_timer_cancel(&canceled_timer); 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 int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); 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.