diff --git a/rcl/include/rcl/timer.h b/rcl/include/rcl/timer.h index e0a31f2b0..c0cda141d 100644 --- a/rcl/include/rcl/timer.h +++ b/rcl/include/rcl/timer.h @@ -50,6 +50,13 @@ typedef struct rcl_timer_on_reset_callback_data_s size_t reset_counter; } rcl_timer_on_reset_callback_data_t; +/// Structure which encapsulates timer information when called. +typedef struct rcl_timer_call_info_s +{ + rcl_time_point_value_t expected_call_time; + rcl_time_point_value_t actual_call_time; +} rcl_timer_call_info_t; + /// User callback signature for timers. /** * The first argument the callback gets is a pointer to the timer. @@ -243,6 +250,34 @@ RCL_WARN_UNUSED rcl_ret_t rcl_timer_call(rcl_timer_t * timer); +/// Same as rcl_timer_call() except that it also retrieves the actual and expected call time. +/** + * Same as rcl_timer_call() except that it also retrieves the actual and expected call time. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes [1] + * Uses Atomics | Yes + * Lock-Free | Yes [2] + * [1] user callback might not be thread-safe + * + * [2] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[inout] timer the handle to the timer to call + * \param[out] call_info the struct in which the actual and expected call times are stored + * \return #RCL_RET_OK if the timer was called successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_TIMER_CANCELED if the timer has been canceled, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_call_with_info(rcl_timer_t * timer, rcl_timer_call_info_t * call_info); + /// Retrieve the clock of the timer. /** * This function retrieves the clock pointer and copies it into the given variable. diff --git a/rcl/src/rcl/timer.c b/rcl/src/rcl/timer.c index 78ca042d2..68a665ea3 100644 --- a/rcl/src/rcl/timer.c +++ b/rcl/src/rcl/timer.c @@ -250,10 +250,18 @@ rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock) rcl_ret_t rcl_timer_call(rcl_timer_t * timer) +{ + rcl_timer_call_info_t info; + return rcl_timer_call_with_info(timer, &info); +} + +rcl_ret_t +rcl_timer_call_with_info(rcl_timer_t * timer, rcl_timer_call_info_t * call_info) { RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Calling timer"); RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(call_info, RCL_RET_INVALID_ARGUMENT); if (rcutils_atomic_load_bool(&timer->impl->canceled)) { RCL_SET_ERROR_MSG("timer is canceled"); return RCL_RET_TIMER_CANCELED; @@ -273,6 +281,8 @@ rcl_timer_call(rcl_timer_t * timer) (rcl_timer_callback_t)rcutils_atomic_load_uintptr_t(&timer->impl->callback); int64_t next_call_time = rcutils_atomic_load_int64_t(&timer->impl->next_call_time); + call_info->expected_call_time = next_call_time; + call_info->actual_call_time = now; int64_t period = rcutils_atomic_load_int64_t(&timer->impl->period); // always move the next call time by exactly period forward // don't use now as the base to avoid extending each cycle by the time diff --git a/rcl/test/rcl/test_timer.cpp b/rcl/test/rcl/test_timer.cpp index ec36259c4..8003cebba 100644 --- a/rcl/test/rcl/test_timer.cpp +++ b/rcl/test/rcl/test_timer.cpp @@ -937,6 +937,91 @@ TEST_F(TestPreInitTimer, test_timer_get_period) { rcl_reset_error(); } +TEST_F(TestPreInitTimer, test_timer_info) { + int64_t next_call_start = 0; + int64_t old_period = 0; + times_called = 0; + rcl_timer_call_info_t call_info; + call_info.actual_call_time = 0; + call_info.expected_call_time = 0; + int64_t period = RCL_MS_TO_NS(10); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_exchange_period(&timer, period, &old_period)); + ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer)); + ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 1); + + int64_t next_expected_call_time = call_info.expected_call_time + period; + + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start)); + std::this_thread::sleep_for(std::chrono::nanoseconds(next_call_start)); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str; + ASSERT_EQ(next_expected_call_time, call_info.expected_call_time); + ASSERT_GE(call_info.actual_call_time, call_info.expected_call_time); + EXPECT_EQ(times_called, 2); + + next_expected_call_time = call_info.expected_call_time + period; + + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start)); + std::this_thread::sleep_for(std::chrono::nanoseconds(next_call_start)); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str; + ASSERT_EQ(next_expected_call_time, call_info.expected_call_time); + ASSERT_GE(call_info.actual_call_time, call_info.expected_call_time); + EXPECT_EQ(times_called, 3); + + next_expected_call_time = call_info.expected_call_time + period; + + EXPECT_EQ(RCL_RET_OK, rcl_timer_cancel(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_TIMER_CANCELED, rcl_timer_call(&timer)); + rcl_reset_error(); + EXPECT_EQ(times_called, 3); +} + +TEST_F(TestPreInitTimer, test_timer_info_detect_overrun) { + int64_t next_call_start = 0; + int64_t old_period = 0; + times_called = 0; + rcl_timer_call_info_t call_info; + call_info.actual_call_time = 0; + call_info.expected_call_time = 0; + int64_t period = RCL_MS_TO_NS(10); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_exchange_period(&timer, period, &old_period)); + ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer)); + ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 1); + + int64_t next_expected_call_time = call_info.expected_call_time + period; + + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start)); + std::this_thread::sleep_for(std::chrono::nanoseconds(next_call_start + period)); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str; + ASSERT_EQ(next_expected_call_time, call_info.expected_call_time); + ASSERT_GE(call_info.actual_call_time, call_info.expected_call_time); + // check, if we can detect a timer overrun + ASSERT_GE(call_info.actual_call_time - call_info.expected_call_time, period); + EXPECT_EQ(times_called, 2); + + // check, if the expected_call_time for next call is as expected, and skips a period + next_expected_call_time = call_info.expected_call_time + period + period; + + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start)); + std::this_thread::sleep_for(std::chrono::nanoseconds(next_call_start)); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_call_with_info(&timer, &call_info)) << rcl_get_error_string().str; + ASSERT_EQ(next_expected_call_time, call_info.expected_call_time); + ASSERT_GE(call_info.actual_call_time, call_info.expected_call_time); + EXPECT_EQ(times_called, 3); + + EXPECT_EQ(RCL_RET_OK, rcl_timer_cancel(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_TIMER_CANCELED, rcl_timer_call(&timer)); + rcl_reset_error(); + EXPECT_EQ(times_called, 3); +} + TEST_F(TestPreInitTimer, test_time_since_last_call) { rcl_time_point_value_t time_sice_next_call_start = 0u; rcl_time_point_value_t time_sice_next_call_end = 0u;