Fix action cancellation

sea-bass · Dec 19, 2024 · 60c73cb · 60c73cb
1 parent 1d35694
commit 60c73cb
Showing 1 changed file with 10 additions and 4 deletions.
diff --git a/pyrobosim_ros/pyrobosim_ros/ros_interface.py b/pyrobosim_ros/pyrobosim_ros/ros_interface.py
@@ -421,7 +421,10 @@ def action_callback(self, goal_handle):
         )
 
         # Package up the result
-        goal_handle.succeed()
+        if goal_handle.is_cancel_requested:
+            goal_handle.canceled()
+        else:
+            goal_handle.succeed()
         return ExecuteTaskAction.Result(
             execution_result=execution_result_to_ros(execution_result)
         )
@@ -436,7 +439,7 @@ def action_cancel_callback(self, goal_handle):
         robot = self.world.get_robot_by_name(goal_handle.request.action.robot)
         if robot is not None:
             self.get_logger().info(f"Canceling action for robot {robot.name}.")
-            robot.cancel_actions()
+            Thread(target=robot.cancel_actions).start()
         return CancelResponse.ACCEPT
 
     def plan_callback(self, goal_handle):
@@ -482,7 +485,10 @@ def plan_callback(self, goal_handle):
         )
 
         # Package up the result
-        goal_handle.succeed()
+        if goal_handle.is_cancel_requested:
+            goal_handle.canceled()
+        else:
+            goal_handle.succeed()
         return ExecuteTaskPlan.Result(
             execution_result=execution_result_to_ros(execution_result),
             num_completed=num_completed,
@@ -499,7 +505,7 @@ def plan_cancel_callback(self, goal_handle):
         robot = self.world.get_robot_by_name(goal_handle.request.plan.robot)
         if robot is not None:
             self.get_logger().info(f"Canceling plan for robot {robot.name}.")
-            robot.cancel_actions()
+            Thread(target=robot.cancel_actions).start()
         return CancelResponse.ACCEPT
 
     def robot_path_plan_callback(self, goal_handle, robot=None):