TimeSource and support for ROS time

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)

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
 

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)

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 */
 };
 

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()

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')