Add param callback to time_source (#297)

* Add param callback to time_source

Signed-off-by: vinnamkim <[email protected]>

* Fix review comments

Signed-off-by: vinnamkim <[email protected]>

rclpy/rclpy/node.py

@@ -130,7 +130,6 @@ def __init__(
         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)

rclpy/rclpy/time_source.py

@@ -13,8 +13,10 @@
 # limitations under the License.
 
 import builtin_interfaces.msg
+from rcl_interfaces.msg import SetParametersResult
 from rclpy.clock import ClockType
 from rclpy.clock import ROSClock
+from rclpy.parameter import Parameter
 from rclpy.time import Time
 
 CLOCK_TOPIC = '/clock'
@@ -45,6 +47,10 @@ def ros_time_is_active(self, enabled):
             clock._set_ros_time_is_active(enabled)
         if enabled:
             self._subscribe_to_clock_topic()
+        else:
+            if self._clock_sub is not None and self._node is not None:
+                self._node.destroy_subscription(self._clock_sub)
+                self._clock_sub = None
 
     def _subscribe_to_clock_topic(self):
         if self._clock_sub is None and self._node is not None:
@@ -62,8 +68,20 @@ def attach_node(self, node):
         if self._node is not None:
             self.detach_node()
         self._node = node
-        if self.ros_time_is_active:
-            self._subscribe_to_clock_topic()
+
+        use_sim_time_param = node.get_parameter('use_sim_time')
+        if use_sim_time_param.type_ != Parameter.Type.NOT_SET:
+            if use_sim_time_param.type_ == Parameter.Type.BOOL:
+                self.ros_time_is_active = use_sim_time_param.value
+            else:
+                node.get_logger().error(
+                    "Invalid type for parameter 'use_sim_time' {!r} should be bool"
+                    .format(use_sim_time_param.type_))
+        else:
+            node.get_logger().debug(
+                "'use_sim_time' parameter not set, using wall time by default")
+
+        node.set_parameters_callback(self._on_parameter_event)
 
     def detach_node(self):
         # Remove the subscription to the clock topic.
@@ -88,3 +106,15 @@ def clock_callback(self, msg):
         self._last_time_set = time_from_msg
         for clock in self._associated_clocks:
             clock.set_ros_time_override(time_from_msg)
+
+    def _on_parameter_event(self, parameter_list):
+        for parameter in parameter_list:
+            if parameter.name == 'use_sim_time':
+                if parameter.type_ == Parameter.Type.BOOL:
+                    self.ros_time_is_active = parameter.value
+                else:
+                    self._node.get_logger().error(
+                        'use_sim_time parameter set to something besides a bool')
+                break
+
+        return SetParametersResult(successful=True)

rclpy/test/test_time_source.py

@@ -24,6 +24,7 @@
 from rclpy.clock import JumpThreshold
 from rclpy.clock import ROSClock
 from rclpy.duration import Duration
+from rclpy.parameter import Parameter
 from rclpy.time import Time
 from rclpy.time_source import CLOCK_TOPIC
 from rclpy.time_source import TimeSource
@@ -67,6 +68,21 @@ def publish_reversed_clock_messages(self):
             rclpy.spin_once(self.node, timeout_sec=1, executor=executor)
             time.sleep(1)
 
+    def set_use_sim_time_parameter(self, value):
+        self.node.set_parameters(
+            [Parameter('use_sim_time', Parameter.Type.BOOL, value)])
+        executor = rclpy.executors.SingleThreadedExecutor(context=self.context)
+        cycle_count = 0
+        while rclpy.ok(context=self.context) and cycle_count < 5:
+            use_sim_time_param = self.node.get_parameter('use_sim_time')
+            cycle_count += 1
+            if use_sim_time_param.type_ == Parameter.Type.BOOL:
+                break
+
+            rclpy.spin_once(self.node, timeout_sec=1, executor=executor)
+            time.sleep(1)
+        return use_sim_time_param.value == value
+
     def test_time_source_attach_clock(self):
         time_source = TimeSource(node=self.node)
 
@@ -115,7 +131,7 @@ def test_time_source_using_sim_time(self):
         # 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
+        assert self.set_use_sim_time_parameter(True)
         self.assertTrue(time_source.ros_time_is_active)
         self.assertTrue(clock.ros_time_is_active)
 
@@ -142,13 +158,13 @@ def test_time_source_using_sim_time(self):
         time_source.attach_node(node2)
         node2.destroy_node()
         assert time_source._node == node2
-        assert time_source._clock_sub is not None
+        assert time_source._clock_sub is None
 
     def test_forwards_jump(self):
         time_source = TimeSource(node=self.node)
         clock = ROSClock()
         time_source.attach_clock(clock)
-        time_source.ros_time_is_active = True
+        assert self.set_use_sim_time_parameter(True)
 
         pre_cb = Mock()
         post_cb = Mock()
@@ -168,7 +184,7 @@ def test_backwards_jump(self):
         time_source = TimeSource(node=self.node)
         clock = ROSClock()
         time_source.attach_clock(clock)
-        time_source.ros_time_is_active = True
+        assert self.set_use_sim_time_parameter(True)
 
         pre_cb = Mock()
         post_cb = Mock()
@@ -188,23 +204,23 @@ def test_clock_change(self):
         time_source = TimeSource(node=self.node)
         clock = ROSClock()
         time_source.attach_clock(clock)
-        time_source.ros_time_is_active = True
+        assert self.set_use_sim_time_parameter(True)
 
         pre_cb = Mock()
         post_cb = Mock()
         threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True)
         handler = clock.create_jump_callback(
             threshold, pre_callback=pre_cb, post_callback=post_cb)
 
-        time_source.ros_time_is_active = False
+        assert self.set_use_sim_time_parameter(False)
         pre_cb.assert_called()
         post_cb.assert_called()
         assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_DEACTIVATED
 
         pre_cb.reset_mock()
         post_cb.reset_mock()
 
-        time_source.ros_time_is_active = True
+        assert self.set_use_sim_time_parameter(True)
         pre_cb.assert_called()
         post_cb.assert_called()
         assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_ACTIVATED
@@ -214,14 +230,14 @@ def test_no_pre_callback(self):
         time_source = TimeSource(node=self.node)
         clock = ROSClock()
         time_source.attach_clock(clock)
-        time_source.ros_time_is_active = True
+        assert self.set_use_sim_time_parameter(True)
 
         post_cb = Mock()
         threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True)
         handler = clock.create_jump_callback(
             threshold, pre_callback=None, post_callback=post_cb)
 
-        time_source.ros_time_is_active = False
+        assert self.set_use_sim_time_parameter(False)
         post_cb.assert_called_once()
         assert post_cb.call_args[0][0].clock_change == ClockChange.ROS_TIME_DEACTIVATED
         handler.unregister()
@@ -230,13 +246,17 @@ def test_no_post_callback(self):
         time_source = TimeSource(node=self.node)
         clock = ROSClock()
         time_source.attach_clock(clock)
-        time_source.ros_time_is_active = True
+        assert self.set_use_sim_time_parameter(True)
 
         pre_cb = Mock()
         threshold = JumpThreshold(min_forward=None, min_backward=None, on_clock_change=True)
         handler = clock.create_jump_callback(
             threshold, pre_callback=pre_cb, post_callback=None)
 
-        time_source.ros_time_is_active = False
+        assert self.set_use_sim_time_parameter(False)
         pre_cb.assert_called_once()
         handler.unregister()
+
+
+if __name__ == '__main__':
+    unittest.main()