Create ROSClock specialization

Author: dhood

rclpy/rclpy/clock.py

@@ -41,33 +41,36 @@ def __init__(self, *, clock_type=ClockType.SYSTEM_TIME):
     def clock_type(self):
         return self._clock_type
 
+    def __repr__(self):
+        return 'Clock(clock_type={0})'.format(self.clock_type.name)
+
+    def now(self):
+        from rclpy.time import Time
+        time_handle = _rclpy.rclpy_clock_get_now(self._clock_handle)
+        # TODO(dhood): Return a python object from the C extension
+        return Time(
+            nanoseconds=_rclpy.rclpy_time_point_get_nanoseconds(time_handle),
+            clock_type=self.clock_type)
+
+
+class ROSClock(Clock):
+
+    def __init__(self):
+        super(ROSClock, self).__init__(clock_type=ClockType.ROS_TIME)
+
     @property
     def ros_time_is_active(self):
-        # TODO(dhood): Move to ROS_TIME-specific subclass?
         if self.clock_type != ClockType.ROS_TIME:
             raise RuntimeError('Only valid for clocks using ROS_TIME')
         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
-        # TODO(dhood): Move to ROS_TIME-specific subclass?
         if self.clock_type != ClockType.ROS_TIME:
             raise RuntimeError('Only valid for clocks using ROS_TIME')
         _rclpy.rclpy_clock_set_ros_time_override_is_enabled(self._clock_handle, enabled)
 
-    def __repr__(self):
-        return 'Clock(clock_type={0})'.format(self.clock_type.name)
-
-    def now(self):
-        from rclpy.time import Time
-        time_handle = _rclpy.rclpy_clock_get_now(self._clock_handle)
-        # TODO(dhood): Return a python object from the C extension
-        return Time(
-            nanoseconds=_rclpy.rclpy_time_point_get_nanoseconds(time_handle),
-            clock_type=self.clock_type)
-
     def set_ros_time_override(self, time):
-        # TODO(dhood): Move to ROS_TIME-specific subclass?
         from rclpy.time import Time
         if self.clock_type != ClockType.ROS_TIME:
             raise RuntimeError('Only valid for clocks using ROS_TIME')

rclpy/rclpy/node.py

@@ -14,8 +14,7 @@
 
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.client import Client
-from rclpy.clock import Clock
-from rclpy.clock import ClockType
+from rclpy.clock import ROSClock
 from rclpy.constants import S_TO_NS
 from rclpy.exceptions import NotInitializedException
 from rclpy.exceptions import NoTypeSupportImportedException
@@ -83,8 +82,9 @@ 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 (uses sim time if parameter has been set on the node)
-        self._clock = Clock(clock_type=ClockType.ROS_TIME)
+        # 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)
 

rclpy/rclpy/time_source.py

@@ -15,6 +15,7 @@
 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'
@@ -76,6 +77,8 @@ def attach_clock(self, 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)

rclpy/test/test_time_source.py

@@ -19,6 +19,7 @@
 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
@@ -46,9 +47,26 @@ def publish_clock_messages(self):
             # 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())
+
+        # A clock of type ROS_TIME can be attached. It will be converted to a ROSClock for storage.
+        time_source.attach_clock(Clock(clock_type=ClockType.ROS_TIME))
+
+        assert all((isinstance(clock, ROSClock) for clock in time_source._associated_clocks))
+
+        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 = Clock(clock_type=ClockType.ROS_TIME)
+        clock = ROSClock()
         time_source.attach_clock(clock)
 
         # When not using sim time, ROS time should look like system time
@@ -66,15 +84,15 @@ def test_time_source_not_using_sim_time(self):
         # 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 = Clock(clock_type=ClockType.ROS_TIME)
+        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 = Clock(clock_type=ClockType.ROS_TIME)
+        clock = ROSClock()
         time_source.attach_clock(clock)
 
         # Setting ROS time active on a time source should also cause attached clocks' use of ROS