Callbacks when time jumps

rclpy/rclpy/clock.py

@@ -16,6 +16,8 @@
 
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 
+from .duration import Duration
+
 
 class ClockType(IntEnum):
     """
@@ -29,6 +31,90 @@ class ClockType(IntEnum):
     STEADY_TIME = 3
 
 
+class ClockChange(IntEnum):
+
+    # ROS time is active and will continue to be active
+    ROS_TIME_NO_CHANGE = 1
+    # ROS time is being activated
+    ROS_TIME_ACTIVATED = 2
+    # ROS TIME is being deactivated, the clock will report system time after the jump
+    ROS_TIME_DEACTIVATED = 3
+    # ROS time is inactive and the clock will keep reporting system time
+    SYSTEM_TIME_NO_CHANGE = 4
+
+
+class JumpThreshold:
+
+    def __init__(self, *, min_forward, min_backward, on_clock_change=True):
+        """
+        Initialize an instance of JumpThreshold.
+
+        :param min_forward: Minimum jump forwards to be considered exceeded, or None.
+        :param min_backward: Negative duration indicating minimum jump backwards to be considered
+                             exceeded, or None.
+        :param on_clock_change: True to make a callback happen when ROS_TIME is activated
+                                or deactivated.
+        """
+        self.min_forward = min_forward
+        self.min_backward = min_backward
+        self.on_clock_change = on_clock_change
+
+
+class TimeJump:
+
+    def __init__(self, clock_change, delta):
+        if not isinstance(clock_change, ClockChange):
+            raise TypeError('clock_change must be an instance of rclpy.clock.ClockChange')
+        self._clock_change = clock_change
+        self._delta = delta
+
+    @property
+    def clock_change(self):
+        return self._clock_change
+
+    @property
+    def delta(self):
+        return self._delta
+
+
+class JumpHandle:
+
+    def __init__(self, *, clock, threshold, pre_callback, post_callback):
+        """
+        Register a clock jump callback.
+
+        :param clock: Clock that time jump callback is registered to
+        :param pre_callback: Callback to be called before new time is set.
+        :param post_callback: Callback to be called after new time is set, accepting a dictionary
+            with keys "clock_change" and "delta".
+        """
+        if pre_callback is None and post_callback is None:
+            raise ValueError('One of pre_callback or post_callback must be callable')
+        if pre_callback is not None and not callable(pre_callback):
+            raise ValueError('pre_callback must be callable if given')
+        if post_callback is not None and not callable(post_callback):
+            raise ValueError('post_callback must be callable if given')
+        self._clock = clock
+        self._pre_callback = pre_callback
+        self._post_callback = post_callback
+
+        min_forward = 0
+        if threshold.min_forward is not None:
+            min_forward = threshold.min_forward.nanoseconds
+        min_backward = 0
+        if threshold.min_backward is not None:
+            min_backward = threshold.min_backward.nanoseconds
+
+        _rclpy.rclpy_add_clock_callback(
+            clock, self, threshold.on_clock_change, min_forward, min_backward)
+
+    def unregister(self):
+        """Remove a jump callback from the clock."""
+        if self._clock is not None:
+            _rclpy.rclpy_remove_clock_callback(self._clock, self)
+            self._clock = None
+
+
 class Clock:
 
     def __new__(cls, *, clock_type=ClockType.SYSTEM_TIME):
@@ -57,6 +143,44 @@ def now(self):
             nanoseconds=_rclpy.rclpy_time_point_get_nanoseconds(time_handle),
             clock_type=self.clock_type)
 
+    def create_jump_callback(self, threshold, *, pre_callback=None, post_callback=None):
+        """
+        Create callback handler for clock time jumps.
+
+        The callbacks must remain valid as long as the returned JumpHandler is valid.
+        A callback should execute as quick as possible and must not block when called.
+        If a callback raises then no time jump callbacks added after it will be called.
+
+        :param threshold: Criteria for activating time jump.
+        :param pre_callback: Callback to be called before new time is set.
+        :param post_callback: Callback to be called after new time is set accepting
+        :rtype: :class:`rclpy.clock.JumpHandler`
+        """
+        if post_callback is not None and callable(post_callback):
+            original_callback = post_callback
+
+            def callback_shim(jump_dict):
+                nonlocal original_callback
+                clock_change = None
+                if 'RCL_ROS_TIME_NO_CHANGE' == jump_dict['clock_change']:
+                    clock_change = ClockChange.ROS_TIME_NO_CHANGE
+                elif 'RCL_ROS_TIME_ACTIVATED' == jump_dict['clock_change']:
+                    clock_change = ClockChange.ROS_TIME_ACTIVATED
+                elif 'RCL_ROS_TIME_DEACTIVATED' == jump_dict['clock_change']:
+                    clock_change = ClockChange.ROS_TIME_DEACTIVATED
+                elif 'RCL_SYSTEM_TIME_NO_CHANGE' == jump_dict['clock_change']:
+                    clock_change = ClockChange.SYSTEM_TIME_NO_CHANGE
+                else:
+                    raise ValueError('Unknown clock jump type ' + repr(clock_change))
+                duration = Duration(nanoseconds=jump_dict['delta'])
+                original_callback(TimeJump(clock_change, duration))
+
+            post_callback = callback_shim
+
+        return JumpHandle(
+            clock=self._clock_handle, threshold=threshold, pre_callback=pre_callback,
+            post_callback=post_callback)
+
 
 class ROSClock(Clock):
 

rclpy/rclpy/time_source.py

@@ -13,6 +13,7 @@
 # limitations under the License.
 
 import builtin_interfaces.msg
+from rclpy.clock import ClockType
 from rclpy.clock import ROSClock
 from rclpy.time import Time
 
@@ -25,7 +26,8 @@ def __init__(self, *, node=None):
         self._clock_sub = None
         self._node = None
         self._associated_clocks = []
-        self._last_time_set = None
+        # Zero time is a special value that means time is uninitialzied
+        self._last_time_set = Time(clock_type=ClockType.ROS_TIME)
         self._ros_time_is_active = False
         if node is not None:
             self.attach_node(node)
@@ -36,6 +38,8 @@ def ros_time_is_active(self):
 
     @ros_time_is_active.setter
     def ros_time_is_active(self, enabled):
+        if self._ros_time_is_active == enabled:
+            return
         self._ros_time_is_active = enabled
         for clock in self._associated_clocks:
             clock._set_ros_time_is_active(enabled)
@@ -73,8 +77,8 @@ def detach_node(self):
     def attach_clock(self, clock):
         if not isinstance(clock, ROSClock):
             raise ValueError('Only clocks with type ROS_TIME can be attached.')
-        if self._last_time_set is not None:
-            self._set_clock(self._last_time_set, clock)
+
+        clock.set_ros_time_override(self._last_time_set)
         clock._set_ros_time_is_active(self.ros_time_is_active)
         self._associated_clocks.append(clock)
 
@@ -83,8 +87,4 @@ def clock_callback(self, msg):
         time_from_msg = Time.from_msg(msg)
         self._last_time_set = time_from_msg
         for clock in self._associated_clocks:
-            self._set_clock(time_from_msg, clock)
-
-    def _set_clock(self, time, clock):
-        # TODO(dhood): clock jump notifications
-        clock.set_ros_time_override(time)
+            clock.set_ros_time_override(time_from_msg)

rclpy/src/rclpy/_rclpy.c

@@ -3304,6 +3304,10 @@ rclpy_clock_set_ros_time_override_is_enabled(PyObject * Py_UNUSED(self), PyObjec
     rcl_reset_error();
     return NULL;
   }
+  if (PyErr_Occurred()) {
+    // Time jump callbacks raised
+    return NULL;
+  }
   Py_RETURN_NONE;
 }
 
@@ -3348,6 +3352,172 @@ rclpy_clock_set_ros_time_override(PyObject * Py_UNUSED(self), PyObject * args)
     rcl_reset_error();
     return NULL;
   }
+
+  if (PyErr_Occurred()) {
+    // Time jump callbacks raised
+    return NULL;
+  }
+  Py_RETURN_NONE;
+}
+
+/// Called when a time jump occurs.
+void
+_rclpy_on_time_jump(
+  const struct rcl_time_jump_t * time_jump,
+  bool before_jump,
+  void * user_data)
+{
+  if (PyErr_Occurred()) {
+    // An earlier time jump callback already raised an exception
+    return;
+  }
+  PyObject * pyjump_handle = user_data;
+  if (before_jump) {
+    // Call pre jump callback with no arguments
+    PyObject * pycallback = PyObject_GetAttrString(pyjump_handle, "_pre_callback");
+    if (NULL == pycallback || Py_None == pycallback) {
+      // raised or callback is None
+      return;
+    }
+    // May set exception
+    PyObject_CallObject(pycallback, NULL);
+    Py_DECREF(pycallback);
+  } else {
+    // Call post jump callback with JumpInfo as an argument
+    PyObject * pycallback = PyObject_GetAttrString(pyjump_handle, "_post_callback");
+    if (NULL == pycallback || Py_None == pycallback) {
+      // raised or callback is None
+      return;
+    }
+    // Build python dictionary with time jump info
+    const char * clock_change;
+    switch (time_jump->clock_change) {
+      case RCL_ROS_TIME_NO_CHANGE:
+        clock_change = "RCL_ROS_TIME_NO_CHANGE";
+        break;
+      case RCL_ROS_TIME_ACTIVATED:
+        clock_change = "RCL_ROS_TIME_ACTIVATED";
+        break;
+      case RCL_ROS_TIME_DEACTIVATED:
+        clock_change = "RCL_ROS_TIME_DEACTIVATED";
+        break;
+      case RCL_SYSTEM_TIME_NO_CHANGE:
+        clock_change = "RCL_SYSTEM_TIME_NO_CHANGE";
+        break;
+      default:
+        PyErr_Format(PyExc_RuntimeError, "Unknown time jump type %d", time_jump->clock_change);
+        Py_DECREF(pycallback);
+        return;
+    }
+    PY_LONG_LONG delta = time_jump->delta.nanoseconds;
+    PyObject * pyjump_info = Py_BuildValue(
+      "{zzzL}", "clock_change", clock_change, "delta", delta);
+    if (NULL == pyjump_info) {
+      Py_DECREF(pycallback);
+      return;
+    }
+    PyObject * pyargs = PyTuple_Pack(1, pyjump_info);
+    if (NULL == pyargs) {
+      Py_DECREF(pyjump_info);
+      Py_DECREF(pycallback);
+      return;
+    }
+    // May set exception
+    PyObject_CallObject(pycallback, pyargs);
+    Py_DECREF(pyjump_info);
+    Py_DECREF(pyargs);
+    Py_DECREF(pycallback);
+  }
+}
+
+/// Add a time jump callback to a clock.
+/**
+ * On failure, an exception is raised and NULL is returned if:
+ *
+ * Raises ValueError if pyclock is not a clock capsule, or
+ * any argument is invalid
+ * Raises RuntimeError if the callback cannot be added
+ *
+ * \param[in] pyclock Capsule pointing to the clock to set
+ * \param[in] pyjump_handle Instance of rclpy.clock.JumpHandle
+ * \param[in] on_clock_change True if callback should be called when ROS time is toggled
+ * \param[in] min_forward minimum nanoseconds to trigger forward jump callback
+ * \param[in] min_backward minimum negative nanoseconds to trigger backward jump callback
+ * \return NULL on failure
+ *         None on success
+ */
+static PyObject *
+rclpy_add_clock_callback(PyObject * Py_UNUSED(self), PyObject * args)
+{
+  PyObject * pyclock;
+  PyObject * pyjump_handle;
+  int on_clock_change;
+  PY_LONG_LONG min_forward;
+  PY_LONG_LONG min_backward;
+  if (!PyArg_ParseTuple(args, "OOpLL", &pyclock, &pyjump_handle, &on_clock_change, &min_forward,
+    &min_backward))
+  {
+    return NULL;
+  }
+
+  rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer(
+    pyclock, "rcl_clock_t");
+  if (!clock) {
+    return NULL;
+  }
+
+  rcl_jump_threshold_t threshold;
+  threshold.on_clock_change = on_clock_change;
+  threshold.min_forward.nanoseconds = min_forward;
+  threshold.min_backward.nanoseconds = min_backward;
+
+  rcl_ret_t ret = rcl_clock_add_jump_callback(
+    clock, threshold, _rclpy_on_time_jump, pyjump_handle);
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to add time jump callback: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+    return NULL;
+  }
+  Py_RETURN_NONE;
+}
+
+/// Remove a time jump callback from a clock.
+/**
+ * On failure, an exception is raised and NULL is returned if:
+ *
+ * Raises ValueError if pyclock is not a clock capsule, or
+ * any argument is invalid
+ * Raises RuntimeError if the callback cannot be added
+ *
+ * \param[in] pyclock Capsule pointing to the clock to set
+ * \param[in] pyjump_handle Instance of rclpy.clock.JumpHandle
+ * \return NULL on failure
+ *         None on success
+ */
+static PyObject *
+rclpy_remove_clock_callback(PyObject * Py_UNUSED(self), PyObject * args)
+{
+  PyObject * pyclock;
+  PyObject * pyjump_handle;
+  if (!PyArg_ParseTuple(args, "OO", &pyclock, &pyjump_handle)) {
+    return NULL;
+  }
+
+  rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer(
+    pyclock, "rcl_clock_t");
+  if (!clock) {
+    return NULL;
+  }
+
+  rcl_ret_t ret = rcl_clock_remove_jump_callback(
+    clock, _rclpy_on_time_jump, pyjump_handle);
+  if (ret != RCL_RET_OK) {
+    PyErr_Format(PyExc_RuntimeError,
+      "Failed to remove time jump callback: %s", rcl_get_error_string_safe());
+    rcl_reset_error();
+    return NULL;
+  }
   Py_RETURN_NONE;
 }
 
@@ -3648,6 +3818,16 @@ static PyMethodDef rclpy_methods[] = {
     "Set the current time of a clock using ROS time."
   },
 
+  {
+    "rclpy_add_clock_callback", rclpy_add_clock_callback, METH_VARARGS,
+    "Add a time jump callback to a clock."
+  },
+
+  {
+    "rclpy_remove_clock_callback", rclpy_remove_clock_callback, METH_VARARGS,
+    "Remove a time jump callback from a clock."
+  },
+
   {NULL, NULL, 0, NULL}  /* sentinel */
 };