Potential fix for preempting a concurrence container while it was alr…

…eady terminating
ros · Feb 21, 2023 · 3e7a82a · 3e7a82a
1 parent d4226ba
commit 3e7a82a
Showing 1 changed file with 9 additions and 0 deletions.
diff --git a/smach/src/smach/concurrence.py b/smach/src/smach/concurrence.py
@@ -272,6 +272,15 @@ def execute(self, parent_ud = smach.UserData()):
             if children_preempts_serviced:
                 smach.loginfo("Concurrence serviced preempt.")
                 self.service_preempt()
+                # Cleanup and return preempted
+                outcome = 'preempted'
+                self._threads = {}
+                self._child_outcomes = {}
+                # Call termination callbacks
+                self.call_termination_cbs(list(self._states.keys()), outcome)
+                # Copy output keys
+                self._copy_output_keys(self.userdata, parent_ud)
+                return outcome
 
         # Spew some debyg info
         smach.loginfo("Concurrent Outcomes: "+str(self._child_outcomes))