-
Notifications
You must be signed in to change notification settings - Fork 221
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
TimeSource and support for ROS time #210
Changes from 12 commits
3942ac4
5451fe1
3df0a7e
d64dba0
0670f5b
df7bca9
be9aaca
5789eb5
a835e8b
69a98ca
c1b59b5
cb6245c
6a9d42b
9992b1f
cf38ae7
6ac5bf9
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,96 @@ | ||
# Copyright 2018 Open Source Robotics Foundation, Inc. | ||
# | ||
# Licensed under the Apache License, Version 2.0 (the "License"); | ||
# you may not use this file except in compliance with the License. | ||
# You may obtain a copy of the License at | ||
# | ||
# http://www.apache.org/licenses/LICENSE-2.0 | ||
# | ||
# Unless required by applicable law or agreed to in writing, software | ||
# distributed under the License is distributed on an "AS IS" BASIS, | ||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
# See the License for the specific language governing permissions and | ||
# limitations under the License. | ||
|
||
import builtin_interfaces.msg | ||
from rclpy.clock import Clock | ||
from rclpy.clock import ClockType | ||
from rclpy.clock import ROSClock | ||
from rclpy.time import Time | ||
|
||
CLOCK_TOPIC = '/clock' | ||
|
||
|
||
class TimeSource: | ||
|
||
def __init__(self, *, node=None): | ||
self._clock_sub = None | ||
self._node = None | ||
self._associated_clocks = [] | ||
self._last_time_set = None | ||
self._ros_time_is_active = False | ||
if node is not None: | ||
self.attach_node(node) | ||
|
||
@property | ||
def ros_time_is_active(self): | ||
return self._ros_time_is_active | ||
|
||
@ros_time_is_active.setter | ||
def ros_time_is_active(self, enabled): | ||
self._ros_time_is_active = enabled | ||
for clock in self._associated_clocks: | ||
clock._set_ros_time_is_active(enabled) | ||
if enabled: | ||
self._subscribe_to_clock_topic() | ||
|
||
def _subscribe_to_clock_topic(self): | ||
if self._clock_sub is None and self._node is not None: | ||
self._clock_sub = self._node.create_subscription( | ||
builtin_interfaces.msg.Time, | ||
CLOCK_TOPIC, | ||
self.clock_callback | ||
) | ||
|
||
def attach_node(self, node): | ||
from rclpy.node import Node | ||
if not isinstance(node, Node): | ||
raise TypeError('Node must be of type rclpy.node.Node') | ||
# Remove an existing node. | ||
if self._node is not None: | ||
self.detach_node() | ||
self._node = node | ||
if self.ros_time_is_active: | ||
self._subscribe_to_clock_topic() | ||
|
||
def detach_node(self): | ||
# Remove the subscription to the clock topic. | ||
if self._clock_sub is not None: | ||
if self._node is None: | ||
raise RuntimeError('Unable to destroy previously created clock subscription') | ||
self._node.destroy_subscription(self._clock_sub) | ||
self._clock_sub = None | ||
self._node = None | ||
|
||
def attach_clock(self, clock): | ||
if not isinstance(clock, Clock): | ||
raise TypeError('Clock must be of type rclpy.clock.Clock') | ||
if not clock.clock_type == ClockType.ROS_TIME: | ||
raise ValueError('Only clocks with type ROS_TIME can be attached.') | ||
# "Cast" to ROSClock subclass so ROS time methods can be used. | ||
clock.__class__ = ROSClock | ||
if self._last_time_set is not None: | ||
self._set_clock(self._last_time_set, clock) | ||
clock._set_ros_time_is_active(self.ros_time_is_active) | ||
self._associated_clocks.append(clock) | ||
|
||
def clock_callback(self, msg): | ||
# Cache the last message in case a new clock is attached. | ||
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) |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -3246,6 +3246,136 @@ rclpy_clock_get_now(PyObject * Py_UNUSED(self), PyObject * args) | |
return PyCapsule_New(time_point, "rcl_time_point_t", _rclpy_destroy_time_point); | ||
} | ||
|
||
/// Returns if a clock using ROS time has the ROS time override enabled. | ||
/** | ||
* On failure, an exception is raised and NULL is returned if: | ||
* | ||
* Raises ValueError if pyclock is not a clock capsule | ||
* Raises RuntimeError if the clock's ROS time override state cannot be retrieved | ||
* | ||
* \param[in] pyclock Capsule pointing to the clock | ||
* \return NULL on failure | ||
* True if enabled | ||
* False if not enabled | ||
*/ | ||
static PyObject * | ||
rclpy_clock_get_ros_time_override_is_enabled(PyObject * Py_UNUSED(self), PyObject * args) | ||
{ | ||
PyObject * pyclock; | ||
if (!PyArg_ParseTuple(args, "O", &pyclock)) { | ||
return NULL; | ||
} | ||
|
||
rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( | ||
pyclock, "rcl_clock_t"); | ||
if (!clock) { | ||
return NULL; | ||
} | ||
|
||
bool is_enabled; | ||
rcl_ret_t ret = rcl_is_enabled_ros_time_override(clock, &is_enabled); | ||
if (ret != RCL_RET_OK) { | ||
PyErr_Format(PyExc_RuntimeError, | ||
"Failed to get if ROS time override is enabled for clock: %s", rcl_get_error_string_safe()); | ||
rcl_reset_error(); | ||
return NULL; | ||
} | ||
|
||
if (is_enabled) { | ||
Py_RETURN_TRUE; | ||
} else { | ||
Py_RETURN_FALSE; | ||
} | ||
} | ||
|
||
/// Set if a clock using ROS time has the ROS time override enabled. | ||
/** | ||
* On failure, an exception is raised and NULL is returned if: | ||
* | ||
* Raises ValueError if pyclock is not a clock capsule | ||
* Raises RuntimeError if the clock's ROS time override cannot be set | ||
* | ||
* \param[in] pyclock Capsule pointing to the clock to set | ||
* \param[in] enabled Override state to set | ||
* \return NULL on failure | ||
* None on success | ||
*/ | ||
static PyObject * | ||
rclpy_clock_set_ros_time_override_is_enabled(PyObject * Py_UNUSED(self), PyObject * args) | ||
{ | ||
PyObject * pyclock; | ||
PyObject * pyenabled; | ||
if (!PyArg_ParseTuple(args, "OO", &pyclock, &pyenabled)) { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Could use There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. thanks! 6a9d42b |
||
return NULL; | ||
} | ||
|
||
rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( | ||
pyclock, "rcl_clock_t"); | ||
if (!clock) { | ||
return NULL; | ||
} | ||
|
||
bool enabled = PyObject_IsTrue(pyenabled); | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Unlikely to see this happen, but I think this needs to check |
||
|
||
rcl_ret_t ret; | ||
if (enabled) { | ||
ret = rcl_enable_ros_time_override(clock); | ||
} else { | ||
ret = rcl_disable_ros_time_override(clock); | ||
} | ||
if (ret != RCL_RET_OK) { | ||
PyErr_Format(PyExc_RuntimeError, | ||
"Failed to set ROS time override for clock: %s", rcl_get_error_string_safe()); | ||
rcl_reset_error(); | ||
return NULL; | ||
} | ||
Py_RETURN_NONE; | ||
} | ||
|
||
/// Set the ROS time override for a clock using ROS time. | ||
/** | ||
* On failure, an exception is raised and NULL is returned if: | ||
* | ||
* Raises ValueError if pyclock is not a clock capsule, or | ||
* pytime_point is not a time point capsule | ||
* Raises RuntimeError if the clock's ROS time override cannot be set | ||
* | ||
* \param[in] pyclock Capsule pointing to the clock to set | ||
* \param[in] pytime_point Capsule pointing to the time point | ||
* \return NULL on failure | ||
* None on success | ||
*/ | ||
static PyObject * | ||
rclpy_clock_set_ros_time_override(PyObject * Py_UNUSED(self), PyObject * args) | ||
{ | ||
PyObject * pyclock; | ||
PyObject * pytime_point; | ||
if (!PyArg_ParseTuple(args, "OO", &pyclock, &pytime_point)) { | ||
return NULL; | ||
} | ||
|
||
rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( | ||
pyclock, "rcl_clock_t"); | ||
if (!clock) { | ||
return NULL; | ||
} | ||
|
||
rcl_time_point_t * time_point = (rcl_time_point_t *)PyCapsule_GetPointer( | ||
pytime_point, "rcl_time_point_t"); | ||
if (!time_point) { | ||
return NULL; | ||
} | ||
|
||
rcl_ret_t ret = rcl_set_ros_time_override(clock, time_point->nanoseconds); | ||
if (ret != RCL_RET_OK) { | ||
PyErr_Format(PyExc_RuntimeError, | ||
"Failed to set ROS time override for clock: %s", rcl_get_error_string_safe()); | ||
rcl_reset_error(); | ||
return NULL; | ||
} | ||
Py_RETURN_NONE; | ||
} | ||
|
||
/// Define the public methods of this module | ||
static PyMethodDef rclpy_methods[] = { | ||
{ | ||
|
@@ -3528,6 +3658,21 @@ static PyMethodDef rclpy_methods[] = { | |
"Get the current value of a clock." | ||
}, | ||
|
||
{ | ||
"rclpy_clock_get_ros_time_override_is_enabled", rclpy_clock_get_ros_time_override_is_enabled, | ||
METH_VARARGS, "Get if a clock using ROS time has the ROS time override enabled." | ||
}, | ||
|
||
{ | ||
"rclpy_clock_set_ros_time_override_is_enabled", rclpy_clock_set_ros_time_override_is_enabled, | ||
METH_VARARGS, "Set if a clock using ROS time has the ROS time override enabled." | ||
}, | ||
|
||
{ | ||
"rclpy_clock_set_ros_time_override", rclpy_clock_set_ros_time_override, METH_VARARGS, | ||
"Set the current time of a clock using ROS time." | ||
}, | ||
|
||
{NULL, NULL, 0, NULL} /* sentinel */ | ||
}; | ||
|
||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Not asking to change anything, just sharing because I think it's interesting. This led me discover that
__new__
could be used to return aROSClock
instance when passed a ROS clock type.Details
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think this approach might be better! Because there's no extra step for users to get access to ROS clock methods if they have created a ROS clock with
Clock(clock_type=ClockType.ROS_TIME)
, they will always get a ROSClock back.I don't immediately see any disadvantages of this (it's ok in this instance that the Clock class 'knows about' the ROSClock class), so I plan to incorporate it, thank you for the suggestion
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done in 9992b1f + cf38ae7, I think it's better to use now: happy you pointed this out!