From 85ecfcbbf7bbf97b5a957468744817285e0c0c3d Mon Sep 17 00:00:00 2001 From: Tomoya Fujita Date: Tue, 30 Jul 2024 09:50:57 -0700 Subject: [PATCH] Add TimerInfo to timer callback. (#1292) * Add TimerInfo to timer callback. Signed-off-by: Tomoya.Fujita * call partial to bind the TimerInfo if user callback requests. Signed-off-by: Tomoya Fujita * update comment for the signature check and remove print. Signed-off-by: Tomoya Fujita * add test case for TimerInfo callback with functools.partial. Signed-off-by: Tomoya Fujita --------- Signed-off-by: Tomoya.Fujita Signed-off-by: Tomoya Fujita --- rclpy/rclpy/executors.py | 33 ++++++++++-- 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 | 104 ++++++++++++++++++++++++++++++++++++ rclpy/test/test_waitable.py | 2 +- 7 files changed, 197 insertions(+), 9 deletions(-) diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index a9d08e086..a72036d7a 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -14,6 +14,7 @@ from concurrent.futures import ThreadPoolExecutor from contextlib import ExitStack +from functools import partial import inspect import os from threading import Condition @@ -49,7 +50,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 @@ -369,11 +370,33 @@ 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) + + def check_argument_type(callback_func, target_type): + sig = inspect.signature(callback_func) + for param in sig.parameters.values(): + if param.annotation == target_type: + # return 1st one immediately + return param.name + # We could not find the target type in the signature + return None - async def _execute(): - await await_or_execute(tmr.callback) - return _execute + # User might change the Timer.callback function signature at runtime, + # so it needs to check the signature every time. + arg_name = check_argument_type(tmr.callback, target_type=TimerInfo) + prefilled_arg = {arg_name: timer_info} + if arg_name is not None: + async def _execute(): + await await_or_execute(partial(tmr.callback, **prefilled_arg)) + return _execute + else: + async def _execute(): + await await_or_execute(tmr.callback) + return _execute except InvalidHandle: # Timer is a Destroyable, which means that on __enter__ it can throw an # InvalidHandle exception if the entity has already been destroyed. Handle that here diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index 3b7fd37b9..555085637 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 @@ -1743,7 +1743,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 f4c53daa2..236dcfb45 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..8e1fdb219 100644 --- a/rclpy/test/test_timer.py +++ b/rclpy/test/test_timer.py @@ -12,14 +12,17 @@ # See the License for the specific language governing permissions and # limitations under the License. +import functools import os import platform import time 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 +221,104 @@ 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) + + +def test_timer_info_with_partial(): + node = None + executor = None + timer = None + timer_info: TimerInfo = None + timer_called = False + context = rclpy.context.Context() + rclpy.init(context=context) + try: + node = rclpy.create_node('test_timer_with_partial', 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 + nonlocal timer_called + if timer_called is False: + timer_called = True + timer = node.create_timer(1, functools.partial(timer_callback)) + assert not timer.is_canceled() + executor.spin_once(3) + timer.cancel() + assert timer.is_canceled() + assert timer_called is True + 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