From 3e7a82a0334e7b39543eea2586f26e720dae917d Mon Sep 17 00:00:00 2001
From: Ferry Schoenmakers <ferry.schoenmakers@nobleo.nl>
Date: Tue, 21 Feb 2023 13:37:26 +0100
Subject: [PATCH] Potential fix for preempting a concurrence container while it
 was already terminating

---
 smach/src/smach/concurrence.py | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/smach/src/smach/concurrence.py b/smach/src/smach/concurrence.py
index 3dae707..5a13880 100644
--- 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))