From 9157eed1a3668a6b0f0e54116790cc2d38300285 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Date: Fri, 27 Sep 2024 18:43:40 -0400
Subject: [PATCH] Fix path following cancelation logic (#275)

---
 pyrobosim/pyrobosim/gui/main.py              | 23 +++++++++++---------
 pyrobosim/pyrobosim/gui/world_canvas.py      | 11 +++++-----
 pyrobosim/pyrobosim/navigation/execution.py  | 11 +++++++---
 pyrobosim_ros/pyrobosim_ros/ros_interface.py |  3 ++-
 4 files changed, 28 insertions(+), 20 deletions(-)

diff --git a/pyrobosim/pyrobosim/gui/main.py b/pyrobosim/pyrobosim/gui/main.py
index 26976538..0e710c33 100644
--- a/pyrobosim/pyrobosim/gui/main.py
+++ b/pyrobosim/pyrobosim/gui/main.py
@@ -257,16 +257,19 @@ def set_buttons_during_action(self, state):
         :param state: Desired button state (True to enable, False to disable)
         :type state: bool
         """
-        self.nav_button.setEnabled(state)
-        self.pick_button.setEnabled(state)
-        self.place_button.setEnabled(state)
-        self.detect_button.setEnabled(state)
-        self.open_button.setEnabled(state)
-        self.close_button.setEnabled(state)
-        self.rand_pose_button.setEnabled(state)
-        self.cancel_action_button.setEnabled(not state)
-        self.reset_world_button.setEnabled(state and self.world.source_file is not None)
-        self.reset_path_planner_button.setEnabled(state)
+        if self.get_current_robot():
+            self.nav_button.setEnabled(state)
+            self.pick_button.setEnabled(state)
+            self.place_button.setEnabled(state)
+            self.detect_button.setEnabled(state)
+            self.open_button.setEnabled(state)
+            self.close_button.setEnabled(state)
+            self.rand_pose_button.setEnabled(state)
+            self.cancel_action_button.setEnabled(not state)
+            self.reset_world_button.setEnabled(
+                state and self.world.source_file is not None
+            )
+            self.reset_path_planner_button.setEnabled(state)
 
     ####################
     # Button Callbacks #
diff --git a/pyrobosim/pyrobosim/gui/world_canvas.py b/pyrobosim/pyrobosim/gui/world_canvas.py
index 11d1fb02..e1565aef 100644
--- a/pyrobosim/pyrobosim/gui/world_canvas.py
+++ b/pyrobosim/pyrobosim/gui/world_canvas.py
@@ -74,7 +74,7 @@ class WorldCanvas(FigureCanvasQTAgg):
     robot_dir_line_factor = 3.0
     """ Multiplier of robot radius for plotting robot orientation lines. """
 
-    draw_lock = threading.Lock()
+    draw_lock = threading.RLock()
     """ Lock for drawing on the canvas in a thread-safe manner. """
 
     draw_signal = Signal()
@@ -363,11 +363,10 @@ def show(self):
 
     def draw_and_sleep(self):
         """Redraws the figure and waits a small amount of time."""
-        if self.draw_lock.locked():
-            return
-        self.fig.canvas.draw()
-        self.fig.canvas.flush_events()
-        time.sleep(0.005)
+        with self.draw_lock:
+            self.fig.canvas.draw()
+            self.fig.canvas.flush_events()
+            time.sleep(0.005)
 
     def show_planner_and_path(self, robot=None, show_graphs=True, path=None):
         """
diff --git a/pyrobosim/pyrobosim/navigation/execution.py b/pyrobosim/pyrobosim/navigation/execution.py
index a069093a..bbb064ec 100644
--- a/pyrobosim/pyrobosim/navigation/execution.py
+++ b/pyrobosim/pyrobosim/navigation/execution.py
@@ -52,6 +52,12 @@ def __init__(
         self.validation_step_dist = validation_step_dist
 
         # Execution state
+        self.reset_state()
+
+    def reset_state(self):
+        """
+        Resets all the states for tracking the status of path execution.
+        """
         self.current_traj_time = 0.0
         self.following_path = False  # Flag to track path following
         self.abort_execution = False  # Flag to abort internally
@@ -86,8 +92,7 @@ def execute(self, path, realtime_factor=1.0, battery_usage=0.0):
                 message=message,
             )
 
-        self.current_traj_time = 0.0
-        self.abort_execution = False
+        self.reset_state()
         self.following_path = True
 
         # Convert the path to an interpolated trajectory.
@@ -148,7 +153,7 @@ def execute(self, path, realtime_factor=1.0, battery_usage=0.0):
             time.sleep(max(0, sleep_time - (time.time() - start_time)))
 
         # Finalize path execution.
-        self.following_path = False
+        self.reset_state()
         time.sleep(0.1)  # To ensure background threads get the end of the path.
         self.robot.last_nav_result = ExecutionResult(status=status, message=message)
         return self.robot.last_nav_result
diff --git a/pyrobosim_ros/pyrobosim_ros/ros_interface.py b/pyrobosim_ros/pyrobosim_ros/ros_interface.py
index 604995a9..e72c997d 100644
--- a/pyrobosim_ros/pyrobosim_ros/ros_interface.py
+++ b/pyrobosim_ros/pyrobosim_ros/ros_interface.py
@@ -546,7 +546,8 @@ def robot_path_follow_callback(self, goal_handle, robot=None):
             if goal_handle.is_cancel_requested:
                 robot.cancel_actions()
                 goal_handle.canceled()
-            time.sleep(0.5)
+                break
+            time.sleep(0.1)
 
         if self.world.has_gui:
             self.world.gui.set_buttons_during_action(True)