Improve cancellation logic

pyrobosim/pyrobosim/core/robot.py

@@ -946,15 +946,20 @@ def cancel_actions(self):
             self.logger.warning("There is no running action or plan to cancel.")
             return
 
-        if self.executing_nav and self.path_executor is not None:
-            self.logger.info("Canceling path execution...")
-            self.path_executor.cancel_execution = True
-            while self.executing_nav:
+        if self.executing_action:
+            if self.executing_nav and self.path_executor is not None:
+                self.logger.info("Canceling path execution...")
+                self.path_executor.cancel_execution = True
+                while self.executing_nav:
+                    time.sleep(0.1)
+            # Wait for execute_action to return
+            while self.executing_action:
                 time.sleep(0.1)
 
-        if self.executing_action or self.executing_plan:
+        if self.executing_plan:
             self.canceling_execution = True
-            while self.canceling_execution:
+            # Wait for execute_plan to return
+            while self.executing_plan:
                 time.sleep(0.1)
 
     def execute_plan(self, plan, delay=0.5):