streamline pub/sub for mirror; cleanup on behavior shutdown

Author: David Conner

flexbe_mirror/flexbe_mirror/behavior_mirror_sm.py

@@ -42,7 +42,6 @@ def main(args=None):
 
     mirror = FlexbeMirror()
 
-    # Use at least 2 threads to stay on top of pub/sub requirements
     executor = rclpy.executors.SingleThreadedExecutor()
     executor.add_node(mirror)
 

flexbe_mirror/flexbe_mirror/flexbe_mirror.py

@@ -64,12 +64,6 @@ def __init__(self):
         PreemptableStateMachine.initialize_ros(self)
         LockableStateMachine.initialize_ros(self)
 
-        # set up proxys for sm <--> GUI communication
-        # publish topics
-        self._pub = ProxyPublisher({'flexbe/behavior_update': String,
-                                    'flexbe/request_mirror_structure': Int32})
-        self._heartbeat_pub = self.create_publisher(Int32, 'flexbe/mirror/heartbeat', 2)
-
         self._timing_event = threading.Event()  # Used for wait timer
         self._running = False
         self._stopping = False
@@ -80,20 +74,30 @@ def __init__(self):
         self._sync_lock = threading.Lock()
         self._state_checksums = {}
 
+        # set up pub/sub sm <--> GUI communication of mirror
+        # publish topics
+        self._heartbeat_pub = self.create_publisher(Int32, 'flexbe/mirror/heartbeat', 2)
+        self._request_struct_pub = self.create_publisher(Int32, 'flexbe/request_mirror_structure', 2)
+
+        # listen for mirror control messages using standard subscriptions
+        self._status_sub = self.create_subscription(BEStatus, 'flexbe/status', self._status_callback, 10)
+        self._struct_sub = self.create_subscription(ContainerStructure, 'flexbe/mirror/structure',
+                                                    self._mirror_structure_callback, 10)
+        self._sync_sub = self.create_subscription(BehaviorSync, 'flexbe/mirror/sync', self._sync_callback, 10)
+        self._preempt_sub = self.create_subscription(Empty, 'flexbe/mirror/preempt', self._preempt_callback, 10)
+        self._onboard_heartbeat_sub = self.create_subscription(BehaviorSync, 'flexbe/heartbeat', self._heartbeat_callback, 10)
+        self._sync_heartbeat_mismatch_counter = 0
+
         self._outcome_topic = 'flexbe/mirror/outcome'
+        self._update_topic = 'flexbe/behavior_update'
+        # Use proxy publisher/subscriber for access in states
+        # but just initialize here once for all
+        self._beh_update_pub = ProxyPublisher({self._update_topic: String})
 
-        # listen for mirror message
         self._sub = ProxySubscriberCached()
         self._sub.subscribe(self._outcome_topic, UInt8, inst_id=id(self))
         self._sub.enable_buffer(self._outcome_topic)
 
-        self._sub.subscribe('flexbe/status', BEStatus, self._status_callback, inst_id=id(self))
-        self._sub.subscribe('flexbe/mirror/structure', ContainerStructure, self._mirror_structure_callback, inst_id=id(self))
-        self._sub.subscribe('flexbe/mirror/sync', BehaviorSync, self._sync_callback, inst_id=id(self))
-        self._sub.subscribe('flexbe/mirror/preempt', Empty, self._preempt_callback, inst_id=id(self))
-        self._sub.subscribe('flexbe/heartbeat', BehaviorSync, self._heartbeat_callback, inst_id=id(self))
-        self._sync_heartbeat_mismatch_counter = 0
-
         # no clean way to wait for publisher to be ready...
         Logger.loginfo('--> Mirror - setting up publishers and subscribers ...')
         self._timing_event.wait(1.0)  # Give publishers time to initialize
@@ -184,7 +188,7 @@ def _activate_mirror(self, struct_msg):
             else:
                 Logger.localwarn(f'Error processing mirror structure for behavior checksum id = {struct_msg.behavior_id}')
                 Logger.logwarn('Requesting a new mirror structure from onboard ...')
-                self._pub.publish('flexbe/request_mirror_structure', Int32(data=struct_msg.behavior_id))
+                self._request_struct_pub.publish(Int32(data=struct_msg.behavior_id))
                 self._active_id = struct_msg.behavior_id
                 return
 
@@ -249,7 +253,7 @@ def _start_mirror(self, msg):
             if self._sm is None:
                 Logger.localwarn(f'Missing correct mirror structure for starting behavior checksum id ={msg.behavior_id}')
                 Logger.logwarn('Requesting mirror structure from onboard ...')
-                self._pub.publish('flexbe/request_mirror_structure', Int32(data=msg.behavior_id))
+                self._request_struct_pub.publish(Int32(data=msg.behavior_id))
                 self._active_id = msg.behavior_id
                 return
 
@@ -271,19 +275,21 @@ def _stop_mirror(self, msg):
             if self._sm is not None and self._running:
                 if msg is not None and msg.code == BEStatus.FINISHED:
                     Logger.loginfo('Onboard behavior finished successfully.')
-                    self._pub.publish('flexbe/behavior_update', String())
+                    self._beh_update_pub.publish(self._update_topic, String())
                 elif msg is not None and msg.code == BEStatus.SWITCHING:
                     self._starting_path = None
                     Logger.loginfo('Onboard performing behavior switch.')
                 elif msg is not None and msg.code == BEStatus.READY:
                     Logger.loginfo('Onboard engine just started, stopping currently running mirror.')
-                    self._pub.publish('flexbe/behavior_update', String())
+                    self._beh_update_pub.publish(self._update_topic, String())
                 elif msg is not None:
                     Logger.logwarn('Onboard behavior failed!')
-                    self._pub.publish('flexbe/behavior_update', String())
+                    self._beh_update_pub.publish(self._update_topic, String())
 
                 self._wait_stop_running()
 
+                self._sm.destroy()
+
             else:
                 Logger.localinfo('No onboard behavior is active.')
 
@@ -398,7 +404,7 @@ def _restart_mirror(self, msg):
                 else:
                     Logger.localwarn(f'Missing correct mirror structure for restarting behavior checksum id ={msg.behavior_id}')
                     Logger.logwarn('Requesting mirror structure from onboard ...')
-                    self._pub.publish('flexbe/request_mirror_structure', Int32(data=msg.behavior_id))
+                    self._request_struct_pub.publish(Int32(data=msg.behavior_id))
                     self._active_id = msg.behavior_id
                     return
 
@@ -426,6 +432,7 @@ def _execute_mirror(self):
         try:
             result = self._sm.spin()
             Logger.loginfo(f"Mirror for active id = {self._active_id} finished with result '{result}'")
+            self._sm.destroy()
         except Exception as exc:
             try:
                 Logger.logerr('\n(_execute_mirror Traceback): Caught exception on preempt:\n%s' % str(exc))

flexbe_mirror/flexbe_mirror/mirror_state.py

@@ -29,7 +29,7 @@
 
 """Simplified state for use with FlexBE UI State machine mirror."""
 
-from std_msgs.msg import String, UInt8
+from std_msgs.msg import String
 
 from flexbe_core import EventState
 from flexbe_core.proxy import ProxyPublisher, ProxySubscriberCached
@@ -52,8 +52,9 @@ def __init__(self, target_name, target_path, given_outcomes, outcome_autonomy):
 
         self._outcome_topic = 'flexbe/mirror/outcome'
 
+        # Allow access to standard proxies
         self._pub = ProxyPublisher()
-        self._sub = ProxySubscriberCached({self._outcome_topic: UInt8}, inst_id=id(self))
+        self._sub = ProxySubscriberCached()
 
     def execute(self, userdata):
         if self._sub.has_buffered(self._outcome_topic):
@@ -65,3 +66,6 @@ def execute(self, userdata):
     def on_enter(self, userdata):
         self._pub.publish('flexbe/behavior_update',
                           String(data="/" + "/".join(self._target_path.split("/")[1:])))
+
+    def on_stop(self):
+        """Call when mirror SM stops."""

flexbe_mirror/flexbe_mirror/mirror_state_machine.py

@@ -37,6 +37,7 @@
 from flexbe_core import Logger
 from flexbe_core.core import PreemptableStateMachine
 from flexbe_msgs.msg import BehaviorSync
+from flexbe_mirror.mirror_state import MirrorState
 
 
 class MirrorStateMachine(PreemptableStateMachine):
@@ -74,6 +75,18 @@ def spin(self, userdata=None):
 
         return outcome
 
+    def destroy(self):
+        Logger.localinfo(f'Destroy mirror state machine {self.name} ...')
+        self._notify_stop()
+
+    def _notify_stop(self):
+        self.on_stop()
+        for state in self._states:
+            if isinstance(state, MirrorState):
+                state.on_stop()
+            if isinstance(state, MirrorStateMachine):
+                state._notify_stop()
+
     def get_latest_status(self):
         """
         Return the latest execution information as a BehaviorSync message.