Fix executing_nav status issues

sea-bass · Sep 27, 2024 · ea0e46b · ea0e46b
1 parent a573b4c
commit ea0e46b
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 8 deletions.
diff --git a/pyrobosim/pyrobosim/core/robot.py b/pyrobosim/pyrobosim/core/robot.py
@@ -326,6 +326,13 @@ def follow_path(
                 status=ExecutionStatus.PRECONDITION_FAILURE,
                 message=message,
             )
+        if self.path_executor.following_path:
+            message = "Robot is already following an existing path."
+            warnings.warn(message)
+            return ExecutionResult(
+                status=ExecutionStatus.PRECONDITION_FAILURE,
+                message=message,
+            )
 
         # Follow the path.
         self.executing_nav = True
@@ -344,6 +351,8 @@ def follow_path(
             )
 
         # Update the robot state.
+        self.last_nav_result = result
+        self.executing_nav = False
         if self.world:
             if (
                 isinstance(self.location, ObjectSpawn)
@@ -355,9 +364,6 @@ def follow_path(
             if self.world.has_gui:
                 self.world.gui.canvas.show_world_state(robot=self)
                 self.world.gui.update_button_state()
-
-        self.last_nav_result = result
-        self.executing_nav = False
         return result
 
     def navigate(
@@ -927,10 +933,16 @@ def cancel_actions(self):
             warnings.warn("There is no running action or plan to cancel.")
             return
 
-        self.canceling_execution = True
         if self.executing_nav and self.path_executor is not None:
             print(f"[{self.name}] Canceling path execution...")
             self.path_executor.cancel_execution = True
+            while self.executing_nav:
+                time.sleep(0.1)
+
+        if self.executing_action or self.executing_plan:
+            self.canceling_execution = True
+            while self.canceling_execution:
+                time.sleep(0.1)
 
     def execute_plan(self, plan, delay=0.5):
         """

diff --git a/pyrobosim/pyrobosim/gui/main.py b/pyrobosim/pyrobosim/gui/main.py
@@ -376,6 +376,7 @@ def on_cancel_action_click(self):
         robot = self.get_current_robot()
         if robot:
             robot.cancel_actions()
+            self.update_button_state()
             self.canvas.draw_signal.emit()
 
     def on_reset_world_click(self):

diff --git a/pyrobosim/pyrobosim/gui/world_canvas.py b/pyrobosim/pyrobosim/gui/world_canvas.py
@@ -426,7 +426,7 @@ def nav_animation_callback(self):
                 self.show_world_state(cur_robot)
                 world.gui.set_buttons_during_action(False)
 
-            self.draw_signal.emit()
+            self.draw_and_sleep()
 
     def update_robots_plot(self):
         """Updates the robot visualization graphics objects."""
@@ -527,7 +527,7 @@ def pick_object(self, robot, obj_name, grasp_pose=None):
         if result.is_success():
             self.update_object_plot(robot.manipulated_object)
             self.show_world_state(robot)
-            self.draw_signal.emit()
+            self.draw_and_sleep()
         return result
 
     def place_object(self, robot, pose=None):
@@ -557,7 +557,7 @@ def place_object(self, robot, pose=None):
             self.obj_patches.append(obj.viz_patch)
             self.update_object_plot(obj)
         self.show_world_state(robot)
-        self.draw_signal.emit()
+        self.draw_and_sleep()
         return result
 
     def detect_objects(self, robot, query=None):
@@ -579,7 +579,7 @@ def detect_objects(self, robot, query=None):
 
         result = robot.detect_objects(query)
         self.show_objects()
-        self.draw_signal.emit()
+        self.draw_and_sleep()
         return result
 
     def open_location(self, robot):