Skip to content

Commit

Permalink
Add rcl_timer_call_with_info function that retrieves the expected and…
Browse files Browse the repository at this point in the history
… the actual timer trigger times

Signed-off-by: Alexis Tsogias <a.tsogias@cellumation.com>
  • Loading branch information
atsogias authored and Janosch Machowinski committed Feb 20, 2024
1 parent 8baed57 commit 205989d
Show file tree
Hide file tree
Showing 2 changed files with 45 additions and 0 deletions.
35 changes: 35 additions & 0 deletions rcl/include/rcl/timer.h
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,13 @@ typedef struct rcl_timer_on_reset_callback_data_s
size_t reset_counter;
} rcl_timer_on_reset_callback_data_t;

/// Structure which encapsulates call times about a ROS Timer.
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.
Expand Down Expand Up @@ -261,6 +268,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.
*
* <hr>
* Attribute | Adherence
* ------------------ | -------------
* Allocates Memory | No
* Thread-Safe | Yes [1]
* Uses Atomics | Yes
* Lock-Free | Yes [2]
* <i>[1] user callback might not be thread-safe</i>
*
* <i>[2] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t`</i>
*
* \param[inout] timer the handle to the timer to call
* \param[out] call_info the struct in which the actual and ecpected 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.
Expand Down
10 changes: 10 additions & 0 deletions rcl/src/rcl/timer.c
Original file line number Diff line number Diff line change
Expand Up @@ -265,10 +265,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;
Expand All @@ -288,6 +296,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
Expand Down

0 comments on commit 205989d

Please sign in to comment.