Fix MoveGroup action cancel callback (#2118)

Moves the execution callback into its own thread to avoid blocking and actually calls the preempt function in with the cancel callback.
moveit · May 9, 2023 · c85b6a3 · c85b6a3
1 parent a929090
commit c85b6a3
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 7 deletions.
diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp
@@ -64,17 +64,21 @@ void MoveGroupMoveAction::initialize()
         RCLCPP_INFO(LOGGER, "Received request");
         return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
       },
-      [](const std::shared_ptr<MGActionGoal>& /*unused*/) {
+      [this](const std::shared_ptr<MGActionGoal>& /*unused*/) {
         RCLCPP_INFO(LOGGER, "Received request to cancel goal");
+        preemptMoveCallback();
         return rclcpp_action::CancelResponse::ACCEPT;
       },
-      [this](const std::shared_ptr<MGActionGoal>& goal) { return executeMoveCallback(goal); });
+      [this](std::shared_ptr<MGActionGoal> goal) {
+        std::thread{ std::bind(&MoveGroupMoveAction::executeMoveCallback, this, std::placeholders::_1), goal }.detach();
+      });
 }
 
 void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr<MGActionGoal>& goal)
 {
+  goal_ = goal;
   RCLCPP_INFO(LOGGER, "executing..");
-  setMoveState(PLANNING, goal);
+  setMoveState(PLANNING, goal_);
   // before we start planning, ensure that we have the latest robot state received...
   auto node = context_->moveit_cpp_->getNode();
   context_->planning_scene_monitor_->waitForCurrentRobotState(node->get_clock()->now());
@@ -111,8 +115,9 @@ void MoveGroupMoveAction::executeMoveCallback(const std::shared_ptr<MGActionGoal
     goal->abort(action_res);
   }
 
-  setMoveState(IDLE, goal);
+  setMoveState(IDLE, goal_);
   preempt_requested_ = false;
+  goal_.reset();
 }
 
 void MoveGroupMoveAction::executeMoveCallbackPlanAndExecute(const std::shared_ptr<MGActionGoal>& goal,
@@ -222,7 +227,7 @@ void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const std::shared_ptr<MGAc
 bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::MotionPlanRequest& req,
                                                     plan_execution::ExecutableMotionPlan& plan)
 {
-  setMoveState(PLANNING, nullptr);
+  setMoveState(PLANNING, goal_);
 
   bool solved = false;
   planning_interface::MotionPlanResponse res;
@@ -258,12 +263,12 @@ bool MoveGroupMoveAction::planUsingPlanningPipeline(const planning_interface::Mo
 
 void MoveGroupMoveAction::startMoveExecutionCallback()
 {
-  setMoveState(MONITOR, nullptr);
+  setMoveState(MONITOR, goal_);
 }
 
 void MoveGroupMoveAction::startMoveLookCallback()
 {
-  setMoveState(LOOK, nullptr);
+  setMoveState(LOOK, goal_);
 }
 
 void MoveGroupMoveAction::preemptMoveCallback()

diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.h b/moveit_ros/move_group/src/default_capabilities/move_action_capability.h
@@ -72,5 +72,6 @@ class MoveGroupMoveAction : public MoveGroupCapability
 
   MoveGroupState move_state_;
   bool preempt_requested_;
+  std::shared_ptr<MGActionGoal> goal_;
 };
 }  // namespace move_group