Skip to content

Commit

Permalink
Add TimerInfo to timer callback.
Browse files Browse the repository at this point in the history
Signed-off-by: Tomoya.Fujita <tomoya.fujita825@gmail.com>
  • Loading branch information
fujitatomoya committed Jun 2, 2024
1 parent 547245a commit 375742d
Show file tree
Hide file tree
Showing 7 changed files with 141 additions and 6 deletions.
16 changes: 14 additions & 2 deletions rclpy/rclpy/executors.py
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@
from rclpy.subscription import Subscription
from rclpy.task import Future
from rclpy.task import Task
from rclpy.timer import Timer
from rclpy.timer import Timer, TimerInfo
from rclpy.utilities import get_default_context
from rclpy.utilities import timeout_sec_to_nsec
from rclpy.waitable import NumberOfEntities
Expand Down Expand Up @@ -366,7 +366,19 @@ def spin_once_until_future_complete(
def _take_timer(self, tmr):
try:
with tmr.handle:
tmr.handle.call_timer()
info = tmr.handle.call_timer_with_info()
timer_info = TimerInfo(
expected_call_time=info['expected_call_time'],
actual_call_time=info['actual_call_time'],
clock_type=tmr.clock.clock_type)
try:
inspect.signature(tmr.callback).bind(object())

async def _execute():
await await_or_execute(tmr.callback, timer_info)
return _execute
except TypeError:
pass

async def _execute():
await await_or_execute(tmr.callback)
Expand Down
4 changes: 2 additions & 2 deletions rclpy/rclpy/node.py
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@
from rclpy.subscription import Subscription
from rclpy.time_source import TimeSource
from rclpy.timer import Rate
from rclpy.timer import Timer
from rclpy.timer import Timer, TimerInfo
from rclpy.topic_endpoint_info import TopicEndpointInfo
from rclpy.type_description_service import TypeDescriptionService
from rclpy.type_support import check_is_valid_msg_type
Expand Down Expand Up @@ -1740,7 +1740,7 @@ def create_service(
def create_timer(
self,
timer_period_sec: float,
callback: Callable,
callback: Callable[[TimerInfo], None],
callback_group: Optional[CallbackGroup] = None,
clock: Optional[Clock] = None,
autostart: bool = True,
Expand Down
35 changes: 34 additions & 1 deletion rclpy/rclpy/timer.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,46 @@
from typing import Optional

from rclpy.callback_groups import CallbackGroup
from rclpy.clock import Clock
from rclpy.clock import Clock, ClockType
from rclpy.context import Context
from rclpy.exceptions import InvalidHandle, ROSInterruptException
from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
from rclpy.time import Time
from rclpy.utilities import get_default_context


class TimerInfo:
"""
Represents a timer call information.
A ``TimerInfo`` object encapsulate the timer information when called.
"""

def __init__(
self, *,
expected_call_time: int = 0,
actual_call_time: int = 0,
clock_type: ClockType = ClockType.SYSTEM_TIME):
if not isinstance(clock_type, (ClockType, _rclpy.ClockType)):
raise TypeError('Clock type must be a ClockType enum')
if expected_call_time < 0 or actual_call_time < 0:
raise ValueError('call time values must not be negative')
self._expected_call_time: Time = Time(
nanoseconds=expected_call_time, clock_type=clock_type)
self._actual_call_time: Time = Time(
nanoseconds=actual_call_time, clock_type=clock_type)

@property
def expected_call_time(self) -> Time:
""":return: the expected_call_time."""
return self._expected_call_time

@property
def actual_call_time(self) -> Time:
""":return: the actual_call_time."""
return self._actual_call_time


class Timer:

def __init__(
Expand Down
19 changes: 19 additions & 0 deletions rclpy/src/rclpy/timer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,22 @@ void Timer::call_timer()
}
}

py::object
Timer::call_timer_with_info()
{
py::dict timer_info;
rcl_timer_call_info_t call_info;
rcl_ret_t ret = rcl_timer_call_with_info(rcl_timer_.get(), &call_info);
if (ret != RCL_RET_OK) {
throw RCLError("failed to call timer");
}

timer_info["expected_call_time"] = call_info.expected_call_time;
timer_info["actual_call_time"] = call_info.actual_call_time;

return timer_info;
}

void Timer::change_timer_period(int64_t period_nsec)
{
int64_t old_period;
Expand Down Expand Up @@ -172,6 +188,9 @@ define_timer(py::object module)
.def(
"call_timer", &Timer::call_timer,
"Call a timer and starts counting again.")
.def(
"call_timer_with_info", &Timer::call_timer_with_info,
"Call a timer and starts counting again, retrieves actual and expected call time.")
.def(
"change_timer_period", &Timer::change_timer_period,
"Set the period of a timer.")
Expand Down
9 changes: 9 additions & 0 deletions rclpy/src/rclpy/timer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,15 @@ class Timer : public Destroyable, public std::enable_shared_from_this<Timer>
*/
void call_timer();

/// Same as call_timer() except that it also retrieves the actual and expected call time.
/**
* Raises RCLError if there is an rcl error
*
* \return the actual and expected call time.
*/
py::object
call_timer_with_info();

/// Update the timer period
/**
* The change in period will take effect after the next timer call
Expand Down
62 changes: 62 additions & 0 deletions rclpy/test/test_timer.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,10 @@

import pytest
import rclpy
from rclpy.clock import ClockType
from rclpy.constants import S_TO_NS
from rclpy.executors import SingleThreadedExecutor
from rclpy.timer import TimerInfo


TEST_PERIODS = (
Expand Down Expand Up @@ -202,3 +204,63 @@ def test_timer_without_autostart():
if node is not None:
node.destroy_node()
rclpy.shutdown()


def test_timer_info_construction():
timer_info = TimerInfo()
assert timer_info.expected_call_time.nanoseconds == 0
assert timer_info.actual_call_time.nanoseconds == 0
assert timer_info.expected_call_time.clock_type == ClockType.SYSTEM_TIME
assert timer_info.actual_call_time.clock_type == ClockType.SYSTEM_TIME

timer_info = TimerInfo(
expected_call_time=123456789,
actual_call_time=987654321,
clock_type=ClockType.STEADY_TIME
)
assert timer_info.expected_call_time.nanoseconds == 123456789
assert timer_info.actual_call_time.nanoseconds == 987654321
assert timer_info.expected_call_time.clock_type == ClockType.STEADY_TIME
assert timer_info.actual_call_time.clock_type == ClockType.STEADY_TIME

timer_info_copy = timer_info
assert timer_info_copy.expected_call_time.nanoseconds == 123456789
assert timer_info_copy.actual_call_time.nanoseconds == 987654321
assert timer_info_copy.expected_call_time.clock_type == ClockType.STEADY_TIME
assert timer_info_copy.actual_call_time.clock_type == ClockType.STEADY_TIME


def test_timer_with_info():
node = None
executor = None
timer = None
timer_info: TimerInfo = None
context = rclpy.context.Context()
rclpy.init(context=context)
try:
node = rclpy.create_node('test_timer_with_info', context=context)
executor = SingleThreadedExecutor(context=context)
executor.add_node(node)
executor.spin_once(timeout_sec=0)

def timer_callback(info: TimerInfo):
nonlocal timer_info
timer_info = info
timer = node.create_timer(1, timer_callback)
assert not timer.is_canceled()
executor.spin_once(3)
timer.cancel()
assert timer.is_canceled()
assert timer_info is not None
assert timer_info.actual_call_time.clock_type == timer.clock.clock_type
assert timer_info.expected_call_time.clock_type == timer.clock.clock_type
assert timer_info.actual_call_time.nanoseconds > 0
assert timer_info.expected_call_time.nanoseconds > 0
finally:
if timer is not None:
node.destroy_timer(timer)
if executor is not None:
executor.shutdown()
if node is not None:
node.destroy_node()
rclpy.shutdown(context=context)
2 changes: 1 addition & 1 deletion rclpy/test/test_waitable.py
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ def take_data(self):
"""Take stuff from lower level so the wait set doesn't immediately wake again."""
if self.timer_is_ready:
self.timer_is_ready = False
self.timer.call_timer()
self.timer.call_timer_with_info()
return 'timer'
return None

Expand Down

0 comments on commit 375742d

Please sign in to comment.