Node has a reference to its executor

rclpy/rclpy/executors.py

@@ -17,6 +17,7 @@
 import multiprocessing
 from threading import Condition
 from threading import Lock
+from threading import RLock
 
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.task import Task
@@ -111,7 +112,7 @@ class Executor:
     def __init__(self):
         super().__init__()
         self._nodes = set()
-        self._nodes_lock = Lock()
+        self._nodes_lock = RLock()
         # Tasks to be executed (oldest first) 3-tuple Task, Entity, Node
         self._tasks = []
         self._tasks_lock = Lock()
@@ -182,6 +183,7 @@ def add_node(self, node):
         with self._nodes_lock:
             if node not in self._nodes:
                 self._nodes.add(node)
+                node.executor = self
                 # Rebuild the wait set so it includes this new node
                 _rclpy.rclpy_trigger_guard_condition(self._guard_condition)
                 return True

rclpy/rclpy/node.py

@@ -12,6 +12,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import weakref
+
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.client import Client
 from rclpy.clock import ROSClock
@@ -88,6 +90,25 @@ def __init__(self, node_name, *, cli_args=None, namespace=None, use_global_argum
         self._time_source = TimeSource(node=self)
         self._time_source.attach_clock(self._clock)
 
+        self.__executor_weakref = None
+
+    @property
+    def executor(self):
+        """Get the executor if the node has been added to one, else return None."""
+        if self.__executor_weakref:
+            return self.__executor_weakref()
+
+    @executor.setter
+    def executor(self, new_executor):
+        """Set or change the executor the node belongs to."""
+        current_executor = self.executor
+        if current_executor is not None:
+            current_executor.remove_node(self)
+        if new_executor is None:
+            self.__executor_weakref = None
+        elif new_executor.add_node(self):
+            self.__executor_weakref = weakref.ref(new_executor)
+
     @property
     def handle(self):
         return self._handle

rclpy/test/test_node.py

@@ -13,6 +13,7 @@
 # limitations under the License.
 
 import unittest
+from unittest.mock import Mock
 
 from rcl_interfaces.srv import GetParameters
 import rclpy
@@ -128,6 +129,52 @@ def test_node_logger(self):
         node_logger.set_level(rclpy.logging.LoggingSeverity.INFO)
         node_logger.debug('test')
 
+    def test_initially_no_executor(self):
+        node = rclpy.create_node('my_node')
+        try:
+            assert node.executor is None
+        finally:
+            node.destroy_node()
+
+    def test_set_executor_adds_node_to_it(self):
+        node = rclpy.create_node('my_node')
+        executor = Mock()
+        executor.add_node.return_value = True
+        try:
+            node.executor = executor
+            assert id(executor) == id(node.executor)
+        finally:
+            node.destroy_node()
+        executor.add_node.assert_called_once_with(node)
+
+    def test_set_executor_removes_node_from_old_executor(self):
+        node = rclpy.create_node('my_node')
+        old_executor = Mock()
+        old_executor.add_node.return_value = True
+        new_executor = Mock()
+        new_executor.add_node.return_value = True
+        try:
+            node.executor = old_executor
+            assert id(old_executor) == id(node.executor)
+            node.executor = new_executor
+            assert id(new_executor) == id(node.executor)
+        finally:
+            node.destroy_node()
+        old_executor.remove_node.assert_called_once_with(node)
+        new_executor.remove_node.assert_not_called()
+
+    def test_set_executor_clear_executor(self):
+        node = rclpy.create_node('my_node')
+        executor = Mock()
+        executor.add_node.return_value = True
+        try:
+            node.executor = executor
+            assert id(executor) == id(node.executor)
+            node.executor = None
+            assert node.executor is None
+        finally:
+            node.destroy_node()
+
 
 class TestCreateNode(unittest.TestCase):