From baad477b683a77b102d5726415bad7b57d7305a6 Mon Sep 17 00:00:00 2001 From: "Tomoya.Fujita" Date: Sun, 2 Jun 2024 15:18:53 -0700 Subject: [PATCH] Add TimerInfo to timer callback. Signed-off-by: Tomoya.Fujita --- rclpy/rclpy/executors.py | 16 ++++++++-- rclpy/rclpy/node.py | 4 +-- rclpy/rclpy/timer.py | 35 ++++++++++++++++++++- rclpy/src/rclpy/timer.cpp | 19 ++++++++++++ rclpy/src/rclpy/timer.hpp | 9 ++++++ rclpy/test/test_timer.py | 62 +++++++++++++++++++++++++++++++++++++ rclpy/test/test_waitable.py | 2 +- 7 files changed, 141 insertions(+), 6 deletions(-) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 78544c91c..0dbe641fc 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -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 @@ -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) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index d2d08460d..2def019ae 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -79,7 +79,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 @@ -1741,7 +1741,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, diff --git a/rclpy/rclpy/timer.py b/rclpy/rclpy/timer.py index 6f2480055..08404c043 100644 --- a/rclpy/rclpy/timer.py +++ b/rclpy/rclpy/timer.py @@ -20,13 +20,46 @@ from typing import Type 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__( diff --git a/rclpy/src/rclpy/timer.cpp b/rclpy/src/rclpy/timer.cpp index c3e744f09..8fd6b7025 100644 --- a/rclpy/src/rclpy/timer.cpp +++ b/rclpy/src/rclpy/timer.cpp @@ -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; @@ -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.") diff --git a/rclpy/src/rclpy/timer.hpp b/rclpy/src/rclpy/timer.hpp index 0f3393128..5cbab817f 100644 --- a/rclpy/src/rclpy/timer.hpp +++ b/rclpy/src/rclpy/timer.hpp @@ -74,6 +74,15 @@ class Timer : public Destroyable, public std::enable_shared_from_this */ 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 diff --git a/rclpy/test/test_timer.py b/rclpy/test/test_timer.py index f947a7738..a174fe8fa 100644 --- a/rclpy/test/test_timer.py +++ b/rclpy/test/test_timer.py @@ -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 = ( @@ -218,3 +220,63 @@ def test_timer_context_manager(): assert timer.is_canceled() finally: 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) diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 3a8f28583..5debdda40 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -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