diff --git a/rclpy/rclpy/clock.py b/rclpy/rclpy/clock.py index e5f2d0d6d..1008e3e5c 100644 --- a/rclpy/rclpy/clock.py +++ b/rclpy/rclpy/clock.py @@ -31,13 +31,16 @@ class ClockType(IntEnum): class Clock: - def __init__(self, *, clock_type=ClockType.SYSTEM_TIME): + def __new__(cls, *, clock_type=ClockType.SYSTEM_TIME): if not isinstance(clock_type, ClockType): raise TypeError('Clock type must be a ClockType enum') if clock_type is ClockType.ROS_TIME: - raise NotImplementedError + self = super().__new__(ROSClock) + else: + self = super().__new__(cls) self._clock_handle = _rclpy.rclpy_create_clock(clock_type) self._clock_type = clock_type + return self @property def clock_type(self): @@ -53,3 +56,24 @@ def now(self): return Time( nanoseconds=_rclpy.rclpy_time_point_get_nanoseconds(time_handle), clock_type=self.clock_type) + + +class ROSClock(Clock): + + def __new__(cls): + return super().__new__(Clock, clock_type=ClockType.ROS_TIME) + + @property + def ros_time_is_active(self): + return _rclpy.rclpy_clock_get_ros_time_override_is_enabled(self._clock_handle) + + def _set_ros_time_is_active(self, enabled): + # This is not public because it is only to be called by a TimeSource managing the Clock + _rclpy.rclpy_clock_set_ros_time_override_is_enabled(self._clock_handle, enabled) + + def set_ros_time_override(self, time): + from rclpy.time import Time + if not isinstance(time, Time): + TypeError( + 'Time must be specified as rclpy.time.Time. Received type: {0}'.format(type(time))) + _rclpy.rclpy_clock_set_ros_time_override(self._clock_handle, time._time_handle) diff --git a/rclpy/rclpy/node.py b/rclpy/rclpy/node.py index fbf1c0d92..e8b8998a1 100644 --- a/rclpy/rclpy/node.py +++ b/rclpy/rclpy/node.py @@ -14,6 +14,7 @@ from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.client import Client +from rclpy.clock import ROSClock from rclpy.constants import S_TO_NS from rclpy.exceptions import NotInitializedException from rclpy.exceptions import NoTypeSupportImportedException @@ -25,6 +26,7 @@ from rclpy.qos import qos_profile_default, qos_profile_services_default from rclpy.service import Service from rclpy.subscription import Subscription +from rclpy.time_source import TimeSource from rclpy.timer import WallTimer from rclpy.utilities import ok from rclpy.validate_full_topic_name import validate_full_topic_name @@ -80,6 +82,12 @@ def __init__(self, node_name, *, cli_args=None, namespace=None, use_global_argum raise RuntimeError('rclpy_create_node failed for unknown reason') self._logger = get_logger(_rclpy.rclpy_get_node_logger_name(self.handle)) + # Clock that has support for ROS time. + # TODO(dhood): use sim time if parameter has been set on the node. + self._clock = ROSClock() + self._time_source = TimeSource(node=self) + self._time_source.attach_clock(self._clock) + @property def handle(self): return self._handle @@ -94,6 +102,9 @@ def get_name(self): def get_namespace(self): return _rclpy.rclpy_get_node_namespace(self.handle) + def get_clock(self): + return self._clock + def get_logger(self): return self._logger diff --git a/rclpy/rclpy/time_source.py b/rclpy/rclpy/time_source.py new file mode 100644 index 000000000..9eb9a4a15 --- /dev/null +++ b/rclpy/rclpy/time_source.py @@ -0,0 +1,90 @@ +# 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 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, 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_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) diff --git a/rclpy/src/rclpy/_rclpy.c b/rclpy/src/rclpy/_rclpy.c index 61c4dacfe..ceb142a52 100644 --- a/rclpy/src/rclpy/_rclpy.c +++ b/rclpy/src/rclpy/_rclpy.c @@ -3246,6 +3246,134 @@ 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; + int enabled; + if (!PyArg_ParseTuple(args, "Op", &pyclock, &enabled)) { + return NULL; + } + + rcl_clock_t * clock = (rcl_clock_t *)PyCapsule_GetPointer( + pyclock, "rcl_clock_t"); + if (!clock) { + return NULL; + } + + 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 +3656,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 */ }; diff --git a/rclpy/test/test_clock.py b/rclpy/test/test_clock.py index d234100a4..8fe23f6bd 100644 --- a/rclpy/test/test_clock.py +++ b/rclpy/test/test_clock.py @@ -17,6 +17,7 @@ from rclpy.clock import Clock from rclpy.clock import ClockType +from rclpy.clock import ROSClock from rclpy.time import Time @@ -28,12 +29,18 @@ def test_clock_construction(self): with self.assertRaises(TypeError): clock = Clock(clock_type='STEADY_TIME') - with self.assertRaises(NotImplementedError): - clock = Clock(clock_type=ClockType.ROS_TIME) clock = Clock(clock_type=ClockType.STEADY_TIME) assert clock.clock_type == ClockType.STEADY_TIME clock = Clock(clock_type=ClockType.SYSTEM_TIME) assert clock.clock_type == ClockType.SYSTEM_TIME + # A subclass ROSClock is returned if ROS_TIME is specified. + clock = Clock(clock_type=ClockType.ROS_TIME) + assert clock.clock_type == ClockType.ROS_TIME + assert isinstance(clock, ROSClock) + + # Direct instantiation of a ROSClock is also possible. + clock = ROSClock() + assert clock.clock_type == ClockType.ROS_TIME def test_clock_now(self): # System time should be roughly equal to time.time() diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index 4271c2499..0b4a55b32 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -16,6 +16,7 @@ from rcl_interfaces.srv import GetParameters import rclpy +from rclpy.clock import ClockType from rclpy.exceptions import InvalidServiceNameException from rclpy.exceptions import InvalidTopicNameException from test_msgs.msg import Primitives @@ -42,6 +43,7 @@ def test_accessors(self): self.node.handle = 'garbage' self.assertEqual(self.node.get_name(), TEST_NODE) self.assertEqual(self.node.get_namespace(), TEST_NAMESPACE) + self.assertEqual(self.node.get_clock().clock_type, ClockType.ROS_TIME) def test_create_publisher(self): self.node.create_publisher(Primitives, 'chatter') diff --git a/rclpy/test/test_time_source.py b/rclpy/test/test_time_source.py new file mode 100644 index 000000000..7bcf3a4a5 --- /dev/null +++ b/rclpy/test/test_time_source.py @@ -0,0 +1,122 @@ +# 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 time +import unittest + +import builtin_interfaces.msg +import rclpy +from rclpy.clock import Clock +from rclpy.clock import ClockType +from rclpy.clock import ROSClock +from rclpy.time import Time +from rclpy.time_source import CLOCK_TOPIC +from rclpy.time_source import TimeSource + + +class TestTimeSource(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = rclpy.create_node('TestTimeSource', namespace='/rclpy') + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def publish_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 = cycle_count + clock_pub.publish(time_msg) + cycle_count += 1 + rclpy.spin_once(self.node, timeout_sec=1) + # TODO(dhood): use rate once available + time.sleep(1) + + def test_time_source_attach_clock(self): + time_source = TimeSource(node=self.node) + + # ROSClock is a specialization of Clock with ROS time methods. + time_source.attach_clock(ROSClock()) + + # Other clock types are not supported. + with self.assertRaises(ValueError): + time_source.attach_clock(Clock(clock_type=ClockType.SYSTEM_TIME)) + + with self.assertRaises(ValueError): + time_source.attach_clock(Clock(clock_type=ClockType.STEADY_TIME)) + + def test_time_source_not_using_sim_time(self): + time_source = TimeSource(node=self.node) + clock = ROSClock() + time_source.attach_clock(clock) + + # When not using sim time, ROS time should look like system time + now = clock.now() + system_now = Clock(clock_type=ClockType.SYSTEM_TIME).now() + assert (system_now.nanoseconds - now.nanoseconds) < 1e9 + + # Presence of clock publisher should not affect the clock + self.publish_clock_messages() + self.assertFalse(clock.ros_time_is_active) + now = clock.now() + system_now = Clock(clock_type=ClockType.SYSTEM_TIME).now() + assert (system_now.nanoseconds - now.nanoseconds) < 1e9 + + # Whether or not an attached clock is using ROS time should be determined by the time + # source managing it. + self.assertFalse(time_source.ros_time_is_active) + clock2 = ROSClock() + clock2._set_ros_time_is_active(True) + time_source.attach_clock(clock2) + self.assertFalse(clock2.ros_time_is_active) + assert time_source._clock_sub is None + + def test_time_source_using_sim_time(self): + time_source = TimeSource(node=self.node) + clock = ROSClock() + time_source.attach_clock(clock) + + # Setting ROS time active on a time source should also cause attached clocks' use of ROS + # time to be set to active. + self.assertFalse(time_source.ros_time_is_active) + self.assertFalse(clock.ros_time_is_active) + time_source.ros_time_is_active = True + self.assertTrue(time_source.ros_time_is_active) + self.assertTrue(clock.ros_time_is_active) + + # A subscriber should have been created + assert time_source._clock_sub is not None + + # When using sim time, ROS time should look like the messages received on /clock + self.publish_clock_messages() + assert clock.now() > Time(seconds=0, clock_type=ClockType.ROS_TIME) + assert clock.now() <= Time(seconds=5, clock_type=ClockType.ROS_TIME) + + # Check that attached clocks get the cached message + clock2 = Clock(clock_type=ClockType.ROS_TIME) + time_source.attach_clock(clock2) + assert clock2.now() > Time(seconds=0, clock_type=ClockType.ROS_TIME) + assert clock2.now() <= Time(seconds=5, clock_type=ClockType.ROS_TIME) + + # Check detaching the node + time_source.detach_node() + node2 = rclpy.create_node('TestTimeSource2', namespace='/rclpy') + time_source.attach_node(node2) + node2.destroy_node() + assert time_source._node == node2 + assert time_source._clock_sub is not None