diff --git a/rclpy/rclpy/clock.py b/rclpy/rclpy/clock.py index 1008e3e5c..78cb51c84 100644 --- a/rclpy/rclpy/clock.py +++ b/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): diff --git a/rclpy/rclpy/time_source.py b/rclpy/rclpy/time_source.py index 9eb9a4a15..7f48c0611 100644 --- a/rclpy/rclpy/time_source.py +++ b/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) diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index ccf2aa92a..5b143fb35 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/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 */ }; diff --git a/rclpy/test/test_clock.py b/rclpy/test/test_clock.py index 8fe23f6bd..f96914c09 100644 --- a/rclpy/test/test_clock.py +++ b/rclpy/test/test_clock.py @@ -14,10 +14,13 @@ import time import unittest +from unittest.mock import Mock from rclpy.clock import Clock from rclpy.clock import ClockType +from rclpy.clock import JumpThreshold from rclpy.clock import ROSClock +from rclpy.duration import Duration from rclpy.time import Time @@ -64,3 +67,113 @@ def test_clock_now(self): now2 = clock.now() assert now2 > now now = now2 + + def test_ros_time_is_active(self): + clock = ROSClock() + clock._set_ros_time_is_active(True) + assert clock.ros_time_is_active + clock._set_ros_time_is_active(False) + assert not clock.ros_time_is_active + + def test_triggered_time_jump_callbacks(self): + one_second = Duration(seconds=1) + half_second = Duration(seconds=0.5) + negative_half_second = Duration(seconds=-0.5) + negative_one_second = Duration(seconds=-1) + + threshold1 = JumpThreshold( + min_forward=one_second, min_backward=negative_half_second, on_clock_change=False) + threshold2 = JumpThreshold( + min_forward=half_second, min_backward=negative_one_second, on_clock_change=False) + + pre_callback1 = Mock() + post_callback1 = Mock() + pre_callback2 = Mock() + post_callback2 = Mock() + + clock = ROSClock() + handler1 = clock.create_jump_callback( + threshold1, pre_callback=pre_callback1, post_callback=post_callback1) + handler2 = clock.create_jump_callback( + threshold2, pre_callback=pre_callback2, post_callback=post_callback2) + + clock.set_ros_time_override(Time(seconds=1)) + clock._set_ros_time_is_active(True) + pre_callback1.assert_not_called() + post_callback1.assert_not_called() + pre_callback2.assert_not_called() + post_callback2.assert_not_called() + + # forward jump + clock.set_ros_time_override(Time(seconds=1.75)) + pre_callback1.assert_not_called() + post_callback1.assert_not_called() + pre_callback2.assert_called() + post_callback2.assert_called() + + pre_callback1.reset_mock() + post_callback1.reset_mock() + pre_callback2.reset_mock() + post_callback2.reset_mock() + + # backwards jump + clock.set_ros_time_override(Time(seconds=1)) + pre_callback1.assert_called() + post_callback1.assert_called() + pre_callback2.assert_not_called() + post_callback2.assert_not_called() + + handler1.unregister() + handler2.unregister() + + def test_triggered_clock_change_callbacks(self): + one_second = Duration(seconds=1) + negative_one_second = Duration(seconds=-1) + + threshold1 = JumpThreshold( + min_forward=one_second, min_backward=negative_one_second, on_clock_change=False) + threshold2 = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True) + threshold3 = JumpThreshold( + min_forward=one_second, min_backward=negative_one_second, on_clock_change=True) + + pre_callback1 = Mock() + post_callback1 = Mock() + pre_callback2 = Mock() + post_callback2 = Mock() + pre_callback3 = Mock() + post_callback3 = Mock() + + clock = ROSClock() + handler1 = clock.create_jump_callback( + threshold1, pre_callback=pre_callback1, post_callback=post_callback1) + handler2 = clock.create_jump_callback( + threshold2, pre_callback=pre_callback2, post_callback=post_callback2) + handler3 = clock.create_jump_callback( + threshold3, pre_callback=pre_callback3, post_callback=post_callback3) + + clock._set_ros_time_is_active(True) + pre_callback1.assert_not_called() + post_callback1.assert_not_called() + pre_callback2.assert_called() + post_callback2.assert_called() + pre_callback3.assert_called() + post_callback3.assert_called() + + pre_callback1.reset_mock() + post_callback1.reset_mock() + pre_callback2.reset_mock() + post_callback2.reset_mock() + pre_callback3.reset_mock() + post_callback3.reset_mock() + + clock._set_ros_time_is_active(True) + pre_callback1.assert_not_called() + post_callback1.assert_not_called() + pre_callback2.assert_not_called() + post_callback2.assert_not_called() + pre_callback3.assert_not_called() + post_callback3.assert_not_called() + + handler1.unregister() + handler2.unregister() + handler3.unregister() diff --git a/rclpy/test/test_time_source.py b/rclpy/test/test_time_source.py index ae92033f3..b2928a2bf 100644 --- a/rclpy/test/test_time_source.py +++ b/rclpy/test/test_time_source.py @@ -14,12 +14,16 @@ import time import unittest +from unittest.mock import Mock import builtin_interfaces.msg import rclpy from rclpy.clock import Clock +from rclpy.clock import ClockChange from rclpy.clock import ClockType +from rclpy.clock import JumpThreshold from rclpy.clock import ROSClock +from rclpy.duration import Duration from rclpy.time import Time from rclpy.time_source import CLOCK_TOPIC from rclpy.time_source import TimeSource @@ -47,6 +51,17 @@ def publish_clock_messages(self): # TODO(dhood): use rate once available time.sleep(1) + def publish_reversed_clock_messages(self): + clock_pub = self.node.create_publisher(builtin_interfaces.msg.Time, CLOCK_TOPIC) + cycle_count = 0 + time_msg = builtin_interfaces.msg.Time() + while rclpy.ok() and cycle_count < 5: + time_msg.sec = 6 - cycle_count + clock_pub.publish(time_msg) + cycle_count += 1 + rclpy.spin_once(self.node, timeout_sec=1) + time.sleep(1) + def test_time_source_attach_clock(self): time_source = TimeSource(node=self.node) @@ -123,3 +138,100 @@ def test_time_source_using_sim_time(self): node2.destroy_node() assert time_source._node == node2 assert time_source._clock_sub is not None + + def test_forwards_jump(self): + time_source = TimeSource(node=self.node) + clock = ROSClock() + time_source.attach_clock(clock) + time_source.ros_time_is_active = True + + pre_cb = Mock() + post_cb = Mock() + threshold = JumpThreshold( + min_forward=Duration(seconds=0.5), min_backward=None, on_clock_change=False) + handler = clock.create_jump_callback( + threshold, pre_callback=pre_cb, post_callback=post_cb) + + self.publish_clock_messages() + + pre_cb.assert_called() + post_cb.assert_called() + assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_NO_CHANGE + handler.unregister() + + def test_backwards_jump(self): + time_source = TimeSource(node=self.node) + clock = ROSClock() + time_source.attach_clock(clock) + time_source.ros_time_is_active = True + + pre_cb = Mock() + post_cb = Mock() + threshold = JumpThreshold( + min_forward=None, min_backward=Duration(seconds=-0.5), on_clock_change=False) + handler = clock.create_jump_callback( + threshold, pre_callback=pre_cb, post_callback=post_cb) + + self.publish_reversed_clock_messages() + + pre_cb.assert_called() + post_cb.assert_called() + assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_NO_CHANGE + handler.unregister() + + def test_clock_change(self): + time_source = TimeSource(node=self.node) + clock = ROSClock() + time_source.attach_clock(clock) + time_source.ros_time_is_active = True + + pre_cb = Mock() + post_cb = Mock() + threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True) + handler = clock.create_jump_callback( + threshold, pre_callback=pre_cb, post_callback=post_cb) + + time_source.ros_time_is_active = False + pre_cb.assert_called() + post_cb.assert_called() + assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_DEACTIVATED + + pre_cb.reset_mock() + post_cb.reset_mock() + + time_source.ros_time_is_active = True + pre_cb.assert_called() + post_cb.assert_called() + assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_ACTIVATED + handler.unregister() + + def test_no_pre_callback(self): + time_source = TimeSource(node=self.node) + clock = ROSClock() + time_source.attach_clock(clock) + time_source.ros_time_is_active = True + + post_cb = Mock() + threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True) + handler = clock.create_jump_callback( + threshold, pre_callback=None, post_callback=post_cb) + + time_source.ros_time_is_active = False + post_cb.assert_called_once() + assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_DEACTIVATED + handler.unregister() + + def test_no_post_callback(self): + time_source = TimeSource(node=self.node) + clock = ROSClock() + time_source.attach_clock(clock) + time_source.ros_time_is_active = True + + pre_cb = Mock() + threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True) + handler = clock.create_jump_callback( + threshold, pre_callback=pre_cb, post_callback=None) + + time_source.ros_time_is_active = False + pre_cb.assert_called_once() + handler.unregister()