Add TimerInfo to timer callback.

Signed-off-by: Tomoya.Fujita <tomoya.fujita825@gmail.com>

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)

rclpy/rclpy/node.py

@@ -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
@@ -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,

rclpy/rclpy/timer.py

@@ -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__(

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.")

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

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 = (
@@ -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)

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