Wake executor when entities created or destroyed

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

rclpy/rclpy/executors.py

@@ -253,6 +253,15 @@ def remove_node(self, node: 'Node') -> None:
                 # Rebuild the wait set so it doesn't include this node
                 self._guard.trigger()
 
+    def wake(self) -> None:
+        """
+        Wake the executor because something changed.
+
+        This is used to tell the executor when entities are created or destroyed.
+        """
+        if self._guard:
+            self._guard.trigger()
+
     def get_nodes(self) -> List['Node']:
         """Return nodes that have been added to this executor."""
         with self._nodes_lock:

rclpy/rclpy/node.py

@@ -210,6 +210,11 @@ def executor(self, new_executor: Executor) -> None:
             new_executor.add_node(self)
             self.__executor_weakref = weakref.ref(new_executor)
 
+    def _wake_executor(self):
+        executor = self.executor
+        if executor:
+            executor.wake()
+
     @property
     def context(self) -> Context:
         """Get the context associated with the node."""
@@ -376,6 +381,7 @@ def add_waitable(self, waitable: Waitable) -> None:
         :param waitable: An instance of a waitable that the node will add to the waitset.
         """
         self.__waitables.append(waitable)
+        self._wake_executor()
 
     def remove_waitable(self, waitable: Waitable) -> None:
         """
@@ -384,6 +390,7 @@ def remove_waitable(self, waitable: Waitable) -> None:
         :param waitable: The Waitable to remove.
         """
         self.__waitables.remove(waitable)
+        self._wake_executor()
 
     def create_publisher(
         self,
@@ -417,6 +424,7 @@ def create_publisher(
 
         publisher = Publisher(publisher_handle, msg_type, topic, qos_profile, self.handle)
         self.__publishers.append(publisher)
+        self._wake_executor()
         return publisher
 
     def create_subscription(
@@ -464,6 +472,7 @@ def create_subscription(
             topic, callback, callback_group, qos_profile, raw)
         self.__subscriptions.append(subscription)
         callback_group.add_entity(subscription)
+        self._wake_executor()
         return subscription
 
     def create_client(
@@ -508,6 +517,7 @@ def create_client(
             callback_group)
         self.__clients.append(client)
         callback_group.add_entity(client)
+        self._wake_executor()
         return client
 
     def create_service(
@@ -554,6 +564,7 @@ def create_service(
             srv_type, srv_name, callback, callback_group, qos_profile)
         self.__services.append(service)
         callback_group.add_entity(service)
+        self._wake_executor()
         return service
 
     def create_timer(
@@ -581,6 +592,7 @@ def create_timer(
 
         self.__timers.append(timer)
         callback_group.add_entity(timer)
+        self._wake_executor()
         return timer
 
     def create_guard_condition(
@@ -596,6 +608,7 @@ def create_guard_condition(
 
         self.__guards.append(guard)
         callback_group.add_entity(guard)
+        self._wake_executor()
         return guard
 
     def destroy_publisher(self, publisher: Publisher) -> bool:
@@ -610,6 +623,7 @@ def destroy_publisher(self, publisher: Publisher) -> bool:
                 publisher.destroy()
             except InvalidHandle:
                 return False
+            self._wake_executor()
             return True
         return False
 
@@ -625,6 +639,7 @@ def destroy_subscription(self, subscription: Subscription) -> bool:
                 subscription.destroy()
             except InvalidHandle:
                 return False
+            self._wake_executor()
             return True
         return False
 
@@ -640,6 +655,7 @@ def destroy_client(self, client: Client) -> bool:
                 client.destroy()
             except InvalidHandle:
                 return False
+            self._wake_executor()
             return True
         return False
 
@@ -655,6 +671,7 @@ def destroy_service(self, service: Service) -> bool:
                 service.destroy()
             except InvalidHandle:
                 return False
+            self._wake_executor()
             return True
         return False
 
@@ -670,6 +687,7 @@ def destroy_timer(self, timer: WallTimer) -> bool:
                 timer.destroy()
             except InvalidHandle:
                 return False
+            self._wake_executor()
             return True
         return False
 
@@ -685,6 +703,7 @@ def destroy_guard_condition(self, guard: GuardCondition) -> bool:
                 guard.destroy()
             except InvalidHandle:
                 return False
+            self._wake_executor()
             return True
         return False
 
@@ -713,6 +732,7 @@ def destroy_node(self) -> bool:
         self.__timers.clear()
         self.__guards.clear()
         self.handle.destroy()
+        self._wake_executor()
 
     def get_publisher_names_and_types_by_node(
         self,