destroy sub/pub/client in executor thread; use SingleThreadedExecutor without callback groups; use basic pub/sub for onboard; cleanup

Author: David Conner

flexbe_core/flexbe_core/behavior_library.py

@@ -64,7 +64,11 @@ def parse_packages(self):
             pkg = parse_package(os.path.join(pkg_path, 'share', pkg_name))
             for export in pkg.exports:
                 if export.tagname == "flexbe_behaviors":
-                    self._add_behavior_manifests(os.path.join(pkg_path, 'lib', pkg_name, 'manifest'), pkg_name)
+                    try:
+                        self._add_behavior_manifests(os.path.join(pkg_path, 'lib', pkg_name, 'manifest'), pkg_name)
+                    except KeyError as exc:
+                        print(f"Error : duplicate behavior name found in {pkg_name} \n  {exc}", flush=True)
+                        raise exc
 
     def _add_behavior_manifests(self, path, pkg=None):
         """
@@ -130,7 +134,6 @@ def find_behavior(self, be_identifier):
 
         @return Tuple (be_key, be_entry) corresponding to the name or (None, None) if not found.
         """
-
         if "/" in be_identifier:
             # Identifier in the form of package/Name
             # where only first slash delineates package

flexbe_core/flexbe_core/core/operatable_state_machine.py

@@ -287,10 +287,12 @@ def _notify_start(self):
                 state._notify_start()
 
     def _notify_stop(self):
+        self.on_stop()
         for state in self._states:
             if isinstance(state, OperatableState):
                 state.on_stop()
             if isinstance(state, OperatableStateMachine):
+                state.on_stop()
                 state._notify_stop()
             if state._is_controlled:
                 state._disable_ros_control()

flexbe_core/flexbe_core/core/preemptable_state_machine.py

@@ -67,6 +67,10 @@ def _preempt_cb(self, msg):
             Logger.localinfo(f'Preempting {self.name}!')
             PreemptableState.preempt = True
 
+    def on_stop(self):
+        # No other SM classes have on_stop, so no call to super().on_stop
+        self._sub.unsubscribe_topic(self._preempt_topic, inst_id=id(self))
+
     @staticmethod
     def add(label, state, transitions=None, remapping=None):
         transitions[PreemptableState._preempted_name] = PreemptableStateMachine._preempted_name

flexbe_core/flexbe_core/proxy/proxy_action_client.py

@@ -115,12 +115,9 @@ def setupClient(cls, topic, action_type, wait_duration=10):
                     Logger.localinfo(f'Existing action client for {topic}'
                                      f' with same action type name, but different instance -  re-create  client!')
 
+                    # Destroy the existing client in executor thread
                     client = ProxyActionClient._clients[topic]
-                    try:
-                        client.destroy()
-                    except Exception as exc:
-                        Logger.localinfo(f'Exception destroying old client for {topic}'
-                                         f'{type(exc)} - {exc}')
+                    ProxyActionClient._node.executor.create_task(ProxyActionClient.destroy_client, client, topic)
 
                     ProxyActionClient._clients[topic] = ActionClient(ProxyActionClient._node, action_type, topic)
                     ProxyActionClient._check_topic_available(topic, wait_duration)
@@ -346,3 +343,16 @@ def _check_topic_available(cls, topic, wait_duration=0.1):
     @classmethod
     def _print_wait_warning(cls, topic):
         Logger.logwarn(f"Waiting for action client/server for '{topic}'")
+
+    @classmethod
+    def destroy_client(cls, client, topic):
+        """Handle client destruction from within the executor threads."""
+        try:
+            if ProxyActionClient._node.destroy_client(client):
+                Logger.localinfo(f'Destroyed the proxy action client for {topic} ({id(client)})!')
+            else:
+                Logger.localwarn(f'Some issue destroying the proxy action client for {topic}!')
+            del client
+        except Exception as exc:  # pylint: disable=W0703
+            Logger.error("Something went wrong destroying proxy action client"
+                         f" for {topic}!\n  {type(exc)} - {str(exc)}")

flexbe_core/flexbe_core/proxy/proxy_publisher.py

@@ -114,7 +114,8 @@ def createPublisher(cls, topic, msg_type, qos=None, **kwargs):
                     # Same message type name, so likely due to reloading Python module on behavior change
                     Logger.localinfo(f'Existing publisher for {topic} with same message type name,'
                                      ' but different instance - re-create publisher!')
-                    # crashes Humble - ProxyPublisher._node.destroy_publisher(ProxyPublisher._topics[topic])
+                    ProxyPublisher._node.executor.create_task(ProxyPublisher.destroy_publisher,
+                                                              ProxyPublisher._topics[topic], topic)
                     qos = qos or QOS_DEFAULT
                     ProxyPublisher._topics[topic] = ProxyPublisher._node.create_publisher(msg_type, topic, qos)
                 else:
@@ -243,3 +244,16 @@ def _wait_for_subscribers(cls, pub, timeout=5.0):
             rate.wait(polling_rate)  # Use system time for polling, not ROS possibly sim_time
         del rate
         return False
+
+    @classmethod
+    def destroy_publisher(cls, pub, topic):
+        """Handle publisher destruction from within the executor threads."""
+        try:
+            if ProxyPublisher._node.destroy_publisher(pub):
+                Logger.localinfo(f'Destroyed the proxy publisher for {topic} ({id(pub)})!')
+            else:
+                Logger.localwarn(f'Some issue destroying the proxy publisher for {topic}!')
+            del pub
+        except Exception as exc:  # pylint: disable=W0703
+            Logger.error("Something went wrong destroying proxy publisher"
+                         f" for {topic}!\n  {type(exc)} - {str(exc)}")

flexbe_core/flexbe_core/proxy/proxy_service_caller.py

@@ -115,11 +115,8 @@ def setupService(cls, topic, srv_type, wait_duration=10):
                 if srv_type.__name__ == ProxyServiceCaller._services[topic].srv_type.__name__:
                     Logger.localinfo(f'Existing service for {topic} with same message type name,'
                                      f' but different instance - re-create service!')
-                    try:
-                        ProxyServiceCaller._node.destroy_client(ProxyServiceCaller._services[topic])
-                    except Exception as exc:  # pylint: disable=W0703
-                        Logger.error("Something went wrong destroying service client"
-                                     f" for {topic}!\n  {type(exc)} - {exc}")
+                    ProxyServiceCaller._node.executor.create_task(ProxyServiceCaller.destroy_service,
+                                                                  ProxyServiceCaller._services[topic], topic)
 
                     ProxyServiceCaller._services[topic] = ProxyServiceCaller._node.create_client(srv_type, topic)
                     if isinstance(wait_duration, float):
@@ -294,3 +291,16 @@ def _check_service_available(cls, topic, wait_duration=1):
     @classmethod
     def _print_wait_warning(cls, topic):
         Logger.warning("Waiting for service %s..." % (topic))
+
+    @classmethod
+    def destroy_service(cls, srv, topic):
+        """Handle service client destruction from within the executor threads."""
+        try:
+            if ProxyServiceCaller._node.destroy_client(srv):
+                Logger.localinfo(f'Destroyed the proxy service caller for {topic} ({id(srv)})!')
+            else:
+                Logger.localwarn(f'Some issue destroying the proxy service caller for {topic}!')
+            del srv
+        except Exception as exc:  # pylint: disable=W0703
+            Logger.error("Something went wrong destroying service caller"
+                         f" for {topic}!\n  {type(exc)} - {str(exc)}")

flexbe_core/flexbe_core/proxy/proxy_subscriber_cached.py

@@ -338,23 +338,38 @@ def unsubscribe_topic(cls, topic, inst_id=-1):
         if topic in ProxySubscriberCached._topics:
 
             try:
-                if inst_id in ProxySubscriberCached._topics[topic]['subscribers']:
-                    ProxySubscriberCached._topics[topic]['subscribers'].remove(inst_id)
+                topic_dict = ProxySubscriberCached._topics[topic]
+                if inst_id in topic_dict['subscribers']:
+                    topic_dict['subscribers'].remove(inst_id)
                     Logger.localinfo(f"Unsubscribed {topic} from proxy! "
-                                     f"({len(ProxySubscriberCached._topics[topic]['subscribers'])} remaining)")
+                                     f"({len(topic_dict['subscribers'])} remaining)")
 
-                if inst_id in ProxySubscriberCached._topics[topic]['callbacks']:
-                    ProxySubscriberCached._topics[topic]['callbacks'].pop(inst_id)
+                if inst_id in topic_dict['callbacks']:
+                    topic_dict['callbacks'].pop(inst_id)
                     Logger.localinfo(f"Removed callback from proxy subscription for {topic} "
-                                     f"from proxy! ({len(ProxySubscriberCached._topics[topic]['callbacks'])} remaining)")
-
-                if len(ProxySubscriberCached._topics[topic]['subscribers']) == 0:
-                    Logger.localinfo(f"Proxy subscriber has no remaining customers for {topic}\n"
-                                     f"    Destroying subscription at node causes crash in Humble "
-                                     f"for both Cyclone and Fast DDS,\n"
-                                     f"    and recreating subscription later causes multiple callbacks per "
-                                     f"published message,\n    so just leave existing subscription in place for now!")
-                    # This has potential to leave topics from old behaviors active
-                    ProxySubscriberCached.disable_buffer(topic)  # Stop buffering until someone asks for it again
+                                     f"from proxy! ({len(topic_dict['callbacks'])} remaining)")
+
+                remaining_subscribers = len(topic_dict['subscribers'])
+                remaining_callbacks = len(topic_dict['callbacks'])
+                if remaining_subscribers == 0:
+                    assert remaining_callbacks == 0, "Must have at least one subscriber tracked for every callback!"
+                    sub = topic_dict['subscription']
+                    ProxySubscriberCached._topics.pop(topic)
+                    ProxySubscriberCached._node.executor.create_task(ProxySubscriberCached.destroy_subscription,
+                                                                     sub, topic)
+
             except Exception as exc:  # pylint: disable=W0703
                 Logger.error(f'Something went wrong unsubscribing {topic} of proxy subscriber!\n%s', str(exc))
+
+    @classmethod
+    def destroy_subscription(cls, sub, topic):
+        """Handle subscription destruction from within the executor threads."""
+        try:
+            if ProxySubscriberCached._node.destroy_subscription(sub):
+                Logger.localinfo(f'Destroyed the proxy subscription for {topic} ({id(sub)})!')
+            else:
+                Logger.localwarn(f'Some issue destroying the proxy subscription for {topic}!')
+            del sub
+        except Exception as exc:  # pylint: disable=W0703
+            Logger.error("Something went wrong destroying subscription"
+                         f" for {topic}!\n  {type(exc)} - {str(exc)}")

flexbe_input/flexbe_input/input_gui.py

@@ -36,6 +36,7 @@ class InputGUI(QMainWindow):
 
     Instances of this class should be created in input_action_server.py.
     """
+
     def __init__(self, prompt):
         QMainWindow.__init__(self)
 

flexbe_mirror/flexbe_mirror/behavior_mirror_sm.py

@@ -43,7 +43,7 @@ def main(args=None):
     mirror = FlexbeMirror()
 
     # Use at least 2 threads to stay on top of pub/sub requirements
-    executor = rclpy.executors.MultiThreadedExecutor(num_threads=2)
+    executor = rclpy.executors.SingleThreadedExecutor()
     executor.add_node(mirror)
 
     print("Begin behavior mirror processing ...", flush=True)