Make destroy_node thread safe

rclpy/CMakeLists.txt

@@ -190,6 +190,7 @@ if(BUILD_TESTING)
     test/test_executor.py
     test/test_expand_topic_name.py
     test/test_guard_condition.py
+    test/test_handle.py
     test/test_init_shutdown.py
     test/test_logging.py
     test/test_messages.py

rclpy/rclpy/action/client.py

@@ -147,16 +147,17 @@ def __init__(
         check_for_type_support(action_type)
         self._node = node
         self._action_type = action_type
-        self._client_handle = _rclpy_action.rclpy_action_create_client(
-            node.handle,
-            action_type,
-            action_name,
-            goal_service_qos_profile.get_c_qos_profile(),
-            result_service_qos_profile.get_c_qos_profile(),
-            cancel_service_qos_profile.get_c_qos_profile(),
-            feedback_sub_qos_profile.get_c_qos_profile(),
-            status_sub_qos_profile.get_c_qos_profile()
-        )
+        with node.handle as node_capsule:
+            self._client_handle = _rclpy_action.rclpy_action_create_client(
+                node_capsule,
+                action_type,
+                action_name,
+                goal_service_qos_profile.get_c_qos_profile(),
+                result_service_qos_profile.get_c_qos_profile(),
+                cancel_service_qos_profile.get_c_qos_profile(),
+                feedback_sub_qos_profile.get_c_qos_profile(),
+                status_sub_qos_profile.get_c_qos_profile()
+            )
 
         self._is_ready = False
 
@@ -536,8 +537,9 @@ def server_is_ready(self):
 
         :return: True if an action server is ready, False otherwise.
         """
-        return _rclpy_action.rclpy_action_server_is_available(
-                self._node.handle,
+        with self._node.handle as node_capsule:
+            return _rclpy_action.rclpy_action_server_is_available(
+                node_capsule,
                 self._client_handle)
 
     def wait_for_server(self, timeout_sec=None):
@@ -562,10 +564,11 @@ def wait_for_server(self, timeout_sec=None):
 
     def destroy(self):
         """Destroy the underlying action client handle."""
-        if self._client_handle is None or self._node.handle is None:
+        if self._client_handle is None:
             return
-        _rclpy_action.rclpy_action_destroy_entity(self._client_handle, self._node.handle)
-        self._node.remove_waitable(self)
+        with self._node.handle as node_capsule:
+            _rclpy_action.rclpy_action_destroy_entity(self._client_handle, node_capsule)
+            self._node.remove_waitable(self)
         self._client_handle = None
 
     def __del__(self):

rclpy/rclpy/action/graph.py

@@ -32,8 +32,9 @@ def get_action_client_names_and_types_by_node(
     :param node_namespace: Namespace of the remote node.
     :return: List of tuples containing two strings: the action name and action type.
     """
-    return _rclpy_action.rclpy_action_get_client_names_and_types_by_node(
-        node.handle, remote_node_name, remote_node_namespace)
+    with node.handle as node_capsule:
+        return _rclpy_action.rclpy_action_get_client_names_and_types_by_node(
+            node_capsule, remote_node_name, remote_node_namespace)
 
 
 def get_action_server_names_and_types_by_node(
@@ -49,8 +50,9 @@ def get_action_server_names_and_types_by_node(
     :param node_namespace: Namespace of the remote node.
     :return: List of tuples containing two strings: the action name and action type.
     """
-    return _rclpy_action.rclpy_action_get_server_names_and_types_by_node(
-        node.handle, remote_node_name, remote_node_namespace)
+    with node.handle as node_capsule:
+        return _rclpy_action.rclpy_action_get_server_names_and_types_by_node(
+            node_capsule, remote_node_name, remote_node_namespace)
 
 
 def get_action_names_and_types(node: Node) -> List[Tuple[str, str]]:
@@ -61,4 +63,5 @@ def get_action_names_and_types(node: Node) -> List[Tuple[str, str]]:
     :return: List of action names and types in the ROS graph as tuples containing two
        strings: the action name and action type.
     """
-    return _rclpy_action.rclpy_action_get_names_and_types(node.handle)
+    with node.handle as node_capsule:
+        return _rclpy_action.rclpy_action_get_names_and_types(node_capsule)

rclpy/rclpy/action/server.py

@@ -247,18 +247,19 @@ def __init__(
         check_for_type_support(action_type)
         self._node = node
         self._action_type = action_type
-        self._handle = _rclpy_action.rclpy_action_create_server(
-            node.handle,
-            node.get_clock().handle,
-            action_type,
-            action_name,
-            goal_service_qos_profile.get_c_qos_profile(),
-            result_service_qos_profile.get_c_qos_profile(),
-            cancel_service_qos_profile.get_c_qos_profile(),
-            feedback_pub_qos_profile.get_c_qos_profile(),
-            status_pub_qos_profile.get_c_qos_profile(),
-            result_timeout,
-        )
+        with node.handle as node_capsule:
+            self._handle = _rclpy_action.rclpy_action_create_server(
+                node_capsule,
+                node.get_clock().handle,
+                action_type,
+                action_name,
+                goal_service_qos_profile.get_c_qos_profile(),
+                result_service_qos_profile.get_c_qos_profile(),
+                cancel_service_qos_profile.get_c_qos_profile(),
+                feedback_pub_qos_profile.get_c_qos_profile(),
+                status_pub_qos_profile.get_c_qos_profile(),
+                result_timeout,
+            )
 
         # key: UUID in bytes, value: GoalHandle
         self._goal_handles = {}
@@ -600,13 +601,14 @@ def register_execute_callback(self, execute_callback):
 
     def destroy(self):
         """Destroy the underlying action server handle."""
-        if self._handle is None or self._node.handle is None:
+        if self._handle is None:
             return
 
         for goal_handle in self._goal_handles.values():
             goal_handle.destroy()
 
-        _rclpy_action.rclpy_action_destroy_entity(self._handle, self._node.handle)
+        with self._node.handle as node_capsule:
+            _rclpy_action.rclpy_action_destroy_entity(self._handle, node_capsule)
         self._node.remove_waitable(self)
         self._handle = None
 

rclpy/rclpy/client.py

@@ -146,7 +146,8 @@ def service_is_ready(self) -> bool:
 
         :return: ``True`` if a server is ready, ``False`` otherwise.
         """
-        return _rclpy.rclpy_service_server_is_available(self.node_handle, self.client_handle)
+        with self.node_handle as node_capsule:
+            return _rclpy.rclpy_service_server_is_available(node_capsule, self.client_handle)
 
     def wait_for_service(self, timeout_sec: float = None) -> bool:
         """

rclpy/rclpy/handle.py

@@ -12,7 +12,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from threading import Lock
+from threading import RLock
+import weakref
 
 from rclpy.impl.implementation_singleton import rclpy_pycapsule_implementation as _rclpy_capsule
 
@@ -44,7 +45,10 @@ def __init__(self, pycapsule):
         self.__use_count = 0
         self.__request_invalidation = False
         self.__valid = True
-        self.__lock = Lock()
+        self.__lock = RLock()
+        self.__required_handles = []
+        self.__dependent_handles = weakref.WeakSet()
+        self.__destroy_callbacks = []
         # Called to give an opportunity to raise an exception if the object is not a pycapsule.
         self.__capsule_name = _rclpy_capsule.rclpy_pycapsule_name(pycapsule)
         self.__capsule_pointer = _rclpy_capsule.rclpy_pycapsule_pointer(pycapsule)
@@ -75,14 +79,40 @@ def pointer(self):
         """
         return self.__capsule_pointer
 
-    def destroy(self):
-        """Destroy pycapsule as soon as possible without waiting for garbage collection."""
+    def destroy(self, then=None):
+        """
+        Destroy pycapsule as soon as possible without waiting for garbage collection.
+
+        :param then: callback to call after handle has been destroyed.
+        """
         with self.__lock:
             if not self.__valid:
                 raise InvalidHandle('Asked to destroy handle, but it was already destroyed')
+            if then:
+                self.__destroy_callbacks.append(then)
             self.__request_invalidation = True
             if 0 == self.__use_count:
-                self.__destroy()  # calls pycapsule destructor
+                self.__destroy()
+
+    def requires(self, req_handle):
+        """
+        Indicate that this handle requires another handle to live longer than itself.
+
+        Calling :meth:`destroy` on the passed in handle will cause this handle to be
+        destroyed first.
+        This handle will hold a reference to the passed in handle so the required handle is
+        garbage collected after this handle in case :meth:`destroy` is not called.
+        """
+        assert isinstance(req_handle, Handle)
+        with self.__lock, req_handle.__lock:
+            if not self.__valid:
+                raise InvalidHandle('Cannot require a new handle if already destroyed')
+            if req_handle.__valid:
+                self.__required_handles.append(req_handle)
+                req_handle.__dependent_handles.add(self)
+            else:
+                # required handle destroyed before we could link to it, destroy self
+                self.destroy()
 
     def _get_capsule(self):
         """
@@ -108,7 +138,7 @@ def _return_capsule(self):
             assert self.__use_count > 0
             self.__use_count -= 1
             if 0 == self.__use_count and self.__request_invalidation:
-                self.__destroy()  # calls pycapsule destructor
+                self.__destroy()
 
     def __enter__(self):
         return self._get_capsule()
@@ -117,13 +147,48 @@ def __exit__(self, type_, value, traceback):
         self._return_capsule()
 
     def __destroy(self):
-        # Assume lock is held
-        assert not self.__lock.acquire(blocking=False)
-        # Assume capsule has not been destroyed
-        assert self.__valid
         # Assume no one is using the capsule anymore
         assert self.__use_count == 0
         # Assume someone has asked it to be destroyed
         assert self.__request_invalidation
-        _rclpy_capsule.rclpy_pycapsule_destroy(self.__capsule)
+        # mark as invalid so no one else tries to use it
         self.__valid = False
+        self.__destroy_dependents(then=self.__destroy_self)
+
+    def __destroy_dependents(self, then):
+        # assumes self.__lock is held
+        deps_lock = RLock()
+        # Turn weak references to regular references
+        dependent_handles = [dep for dep in self.__dependent_handles]
+
+        if not dependent_handles:
+            # no dependents to wait on
+            then()
+            return
+
+        def remove_dependent(handle):
+            nonlocal dependent_handles
+            nonlocal deps_lock
+            nonlocal then
+            with deps_lock:
+                dependent_handles.remove(handle)
+                if 0 == len(dependent_handles):
+                    # all dependents destroyed, do what comes next
+                    then()
+
+        for dep in self.__dependent_handles:
+            try:
+                dep.destroy(then=remove_dependent)
+            except InvalidHandle:
+                # Dependent was already destroyed
+                remove_dependent(dep)
+
+    def __destroy_self(self):
+        with self.__lock:
+            # Calls pycapsule destructor
+            _rclpy_capsule.rclpy_pycapsule_destroy(self.__capsule)
+            # Call post-destroy callbacks
+            while self.__destroy_callbacks:
+                self.__destroy_callbacks.pop()(self)
+            # get rid of references to other handles to break reference cycles
+            del self.__required_handles