diff --git a/rclpy/rclpy/time_source.py b/rclpy/rclpy/time_source.py index d697c17f7..3d09d4651 100644 --- a/rclpy/rclpy/time_source.py +++ b/rclpy/rclpy/time_source.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +import weakref + from rcl_interfaces.msg import SetParametersResult from rclpy.clock import ClockType from rclpy.clock import ROSClock @@ -27,7 +29,7 @@ class TimeSource: def __init__(self, *, node=None): self._clock_sub = None - self._node = None + self._node_weak_ref = None self._associated_clocks = [] # Zero time is a special value that means time is uninitialzied self._last_time_set = Time(clock_type=ClockType.ROS_TIME) @@ -49,27 +51,31 @@ def ros_time_is_active(self, 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 + if self._clock_sub is not None: + node = self._get_node() + if node is not None: + 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: - self._clock_sub = self._node.create_subscription( - rosgraph_msgs.msg.Clock, - CLOCK_TOPIC, - self.clock_callback, - 10 - ) + if self._clock_sub is None: + node = self._get_node() + if node is not None: + self._clock_sub = node.create_subscription( + rosgraph_msgs.msg.Clock, + CLOCK_TOPIC, + self.clock_callback, + 10 + ) 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: + if self._node_weak_ref is not None: self.detach_node() - self._node = node + self._node_weak_ref = weakref.ref(node) if not node.has_parameter(USE_SIM_TIME_NAME): node.declare_parameter(USE_SIM_TIME_NAME, False) @@ -92,11 +98,12 @@ def attach_node(self, node): def detach_node(self): # Remove the subscription to the clock topic. if self._clock_sub is not None: - if self._node is None: + node = self._get_node() + if node is None: raise RuntimeError('Unable to destroy previously created clock subscription') - self._node.destroy_subscription(self._clock_sub) + node.destroy_subscription(self._clock_sub) self._clock_sub = None - self._node = None + self._node_weak_ref = None def attach_clock(self, clock): if not isinstance(clock, ROSClock): @@ -119,9 +126,16 @@ def _on_parameter_event(self, parameter_list): if parameter.type_ == Parameter.Type.BOOL: self.ros_time_is_active = parameter.value else: - self._node.get_logger().error( - '{} parameter set to something besides a bool' - .format(USE_SIM_TIME_NAME)) + node = self._get_node() + if node: + node.get_logger().error( + '{} parameter set to something besides a bool' + .format(USE_SIM_TIME_NAME)) break return SetParametersResult(successful=True) + + def _get_node(self): + if self._node_weak_ref is not None: + return self._node_weak_ref() + return None diff --git a/rclpy/test/test_time_source.py b/rclpy/test/test_time_source.py index a9999e332..5ba2815b8 100644 --- a/rclpy/test/test_time_source.py +++ b/rclpy/test/test_time_source.py @@ -159,7 +159,7 @@ def test_time_source_using_sim_time(self): node2 = rclpy.create_node('TestTimeSource2', namespace='/rclpy', context=self.context) time_source.attach_node(node2) node2.destroy_node() - assert time_source._node == node2 + assert time_source._get_node() == node2 assert time_source._clock_sub is None def test_forwards_jump(self):