Skip to content

Commit

Permalink
Callbacks when time jumps (#222)
Browse files Browse the repository at this point in the history
* TimeSource calls callbacks when time jumps

callbacks on ROS time activation or deactivation
callbacks on forward time jumps
callbacks on backward time jumps
  • Loading branch information
sloretz authored Aug 28, 2018
1 parent e2479de commit dce1750
Show file tree
Hide file tree
Showing 5 changed files with 537 additions and 8 deletions.
124 changes: 124 additions & 0 deletions rclpy/rclpy/clock.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@

from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy

from .duration import Duration


class ClockType(IntEnum):
"""
Expand All @@ -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):
Expand Down Expand Up @@ -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):

Expand Down
16 changes: 8 additions & 8 deletions rclpy/rclpy/time_source.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand All @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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)

Expand All @@ -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)
180 changes: 180 additions & 0 deletions rclpy/src/rclpy/_rclpy.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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 */
};

Expand Down
Loading

0 comments on commit dce1750

Please sign in to comment.