From f66e0a93058d56a77334307b51b17f76b51501c5 Mon Sep 17 00:00:00 2001
From: David Conner <robotics@cnu.edu>
Date: Mon, 24 Jun 2024 16:40:46 -0400
Subject: [PATCH] fix typo in flexbe_mirror; ignore second behavior start
 request while first is processing (in case of zombie launcher)

---
 flexbe_mirror/flexbe_mirror/flexbe_mirror.py    |  2 +-
 flexbe_onboard/flexbe_onboard/flexbe_onboard.py | 17 ++++++++++++++---
 2 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py
index 36b7d5e..b25db38 100644
--- a/flexbe_mirror/flexbe_mirror/flexbe_mirror.py
+++ b/flexbe_mirror/flexbe_mirror/flexbe_mirror.py
@@ -372,7 +372,7 @@ def _start_mirror(self, msg, start_time):
             if len(self._struct_buffer) > 0:
                 while self._sm is None and len(self._struct_buffer) > 0:
                     # Search buffer looking for desired structure
-                    struct = self._struct_buffer.popLeft()
+                    struct = self._struct_buffer.popleft()
                     if struct.behavior_id == msg.behavior_id:
                         self._mirror_state_machine(struct)
                         Logger.localinfo(f"Mirror built for checksum '{msg.behavior_id}'")
diff --git a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py
index b884f28..de3b768 100644
--- a/flexbe_onboard/flexbe_onboard/flexbe_onboard.py
+++ b/flexbe_onboard/flexbe_onboard/flexbe_onboard.py
@@ -113,6 +113,7 @@ def __init__(self):
         self._trigger_ready = True
         self._running = False
         self._run_lock = threading.Lock()
+        self._starting = False
         self._switching = False
         self._switch_lock = threading.Lock()
         self._behavior_id = -1
@@ -152,6 +153,12 @@ def _parse_version(v):
         return result
 
     def _behavior_callback(self, beh_sel_msg):
+        if self._starting:
+            # Prevent multiple request messages from triggering too soon
+            Logger.logwarn(f"Received behavior start request for {beh_sel_msg.behavior_key} "
+                           f"({beh_sel_msg.behavior_id}) while prior request was starting.\n    Ignore second request!")
+            return
+        self._starting = True  # Prevent two start requests from ocurring back to back
         self._trigger_ready = False  # We have received the behavior selection request
         self._ready_counter = 0
         thread = threading.Thread(target=self._behavior_execution, args=[beh_sel_msg])
@@ -222,6 +229,7 @@ def _behavior_execution(self, beh_sel_msg):
                 # self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), code=BEStatus.READY))
                 Logger.localinfo('\033[92m--- Behavior Engine ready to try again! ---\033[0m')
                 self._ready_counter = 8  # Trigger heartbeat to republish READY within 2 seconds
+            self._starting = False  # Now allow a new start requests
             return
 
         # perform the behavior switch if required
@@ -244,6 +252,7 @@ def _behavior_execution(self, beh_sel_msg):
                                   ' because switching is not possible.')
                     self._proxy_pub.publish(Topics._CMD_FEEDBACK_TOPIC,
                                             CommandFeedback(command='switch', args=['not_switchable']))
+                    self._starting = False  # Now allow a new start requests
                     return
 
                 self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(),
@@ -261,11 +270,11 @@ def _behavior_execution(self, beh_sel_msg):
 
                 # extract the active state if any
                 if active_states is not None:
-                    Logger.localinfo(f"Behavior Engine - '{self.be.name}': {self.be.beh_id} "
+                    Logger.localinfo(f"Behavior Engine - '{be.name}': {be.beh_id} "
                                      f'switching behaviors from active state {[acst.name for acst in active_states]} ...')
                     try:
                         if len(active_states) > 1:
-                            Logger.localinfo(f"\n\nBehavior Engine - '{self.be.name}': {self.be.beh_id} "
+                            Logger.localinfo(f"\n\nBehavior Engine - '{be.name}': {be.beh_id} "
                                              f'- switching only top level state!\n')
                             Logger.logwarn('Switch multiple active states?')
 
@@ -280,6 +289,7 @@ def _behavior_execution(self, beh_sel_msg):
                         self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(),
                                                           behavior_id=self.be.beh_id,
                                                           code=BEStatus.RUNNING))
+                        self._starting = False  # Now allow a new start requests
                         return
                     # stop the rest
                     Logger.localinfo(f"Behavior Engine - '{self.be.name}': {self.be.beh_id} - "
@@ -296,6 +306,7 @@ def _behavior_execution(self, beh_sel_msg):
                              f'key={beh_sel_msg.behavior_key}={be.beh_id} ({beh_sel_msg.behavior_id}) ...')
             assert self.be is None, 'Run lock with old behavior active?'
             self._running = True
+            self._starting = False  # Now allow a new start requests
             self.be = be
 
             result = None
@@ -346,7 +357,7 @@ def _behavior_execution(self, beh_sel_msg):
                 self._status_pub.publish(BEStatus(stamp=self.get_clock().now().to_msg(), code=BEStatus.READY))
                 Logger.localinfo('\033[92m--- Behavior Engine finished - ready for more! ---\033[0m')
 
-            # Logger.localinfo(f"Behavior execution finished for id={self.be.beh_id}, exit thread!")
+            Logger.localinfo(f"Behavior execution finished for id={self.be.beh_id}, exit thread!")
             self._running = False
             self._switching = False
             self.be = None