Add TimerInfo to timer callback. (#1292)

* Add TimerInfo to timer callback. Signed-off-by: Tomoya.Fujita <[email protected]> * call partial to bind the TimerInfo if user callback requests. Signed-off-by: Tomoya Fujita <[email protected]> * update comment for the signature check and remove print. Signed-off-by: Tomoya Fujita <[email protected]> * add test case for TimerInfo callback with functools.partial. Signed-off-by: Tomoya Fujita <[email protected]> --------- Signed-off-by: Tomoya.Fujita <[email protected]> Signed-off-by: Tomoya Fujita <[email protected]>
ros2 · Jul 30, 2024 · 85ecfcb · 85ecfcb
1 parent 14a423a
commit 85ecfcb
Show file tree

Hide file tree

Showing 7 changed files with 197 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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

diff --git 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
@@ -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