moveit / moveit2

:robot: MoveIt for ROS 2
https://moveit.ai/
BSD 3-Clause "New" or "Revised" License
1.05k stars 512 forks source link

Cancelling /execute_trajectory action doesn't result in stopping motion #2808

Closed TheSpeedM closed 4 months ago

TheSpeedM commented 5 months ago

Description

When sending a RobotTrajectory to the /execute_trajectory action of the move_group and canceling the action while it is executing it doesn't actually stop the movement. I'm not sure if I am doing something wrong or it doesn't work, any help is welcome.

I am aware that this is not a reproducible example. Will add one if necessary and I have some more time.

Your environment

Code snippets

  1. This function sends the robottrajectory to the action (which it accepts and it starts moving).

    def execute_movement(self, robot_trajectory: RobotTrajectory):
    """Execute the given `robot_trajectory`."""
    self.get_logger().info(f"Executing movement")
    goal_msg = ExecuteTrajectory.Goal()
    goal_msg.trajectory = robot_trajectory
    
    while not self._exec_client.wait_for_server(timeout_sec=1.0):
    self.get_logger().info("Waiting for execute_trajectory server to be available...")
    
    return self._exec_client.send_goal_async(goal_msg)
  2. The function gets called here in an async function.
    
    self._goal_handle = await self.execute_movement(self.robot_trajectory)

if not self._goal_handle.accepted: self.state_machine.state = 'error' continue

res = await self._goal_handle.get_result_async() result = res.result status = res.status


3. I have another interface that triggers this on request.
```python
if self.state_machine.state == 'execute' and self._goal_handle:
    self.get_logger().info(f"Canceling execution")
    print(self._goal_handle)
    future = self._goal_handle.cancel_goal_async()
    future.add_done_callback(self.cancel_callback)
  1. But the done callback gets called only AFTER the execution is finished, which is weird.
    def cancel_callback(self, future):
    print(future.result())

Backtrace or Console output

[INFO] [1713945195.611464680] [program_logic]: Canceling execution

# The print from snippet 3
ClientGoalHandle <id=[ 83 143 114 178  33 153  72 245 149 134  46 109  92 239   9 244], accepted=True, status=2>

# Notice the times (it just continued execution until it reached it goal)
[INFO] [1713945208.822939205] [program_logic]: Goal succeeded! Result: moveit_msgs.action.ExecuteTrajectory_Result(error_code=moveit_msgs.msg.MoveItErrorCodes(val=1))
[INFO] [1713945208.823380712] [program_logic]: Current state: set_io
[INFO] [1713945208.823911415] [program_logic]: Setting pin 2 to False

# The print from snippet 4
action_msgs.srv.CancelGoal_Response(return_code=3, goals_canceling=[])
TheSpeedM commented 4 months ago

For anyone wondering how to stop the execution, this is what i found in the pymoveit2 repo:

self.__cancellation_pub = self._node.create_publisher(
    String, "/trajectory_execution_event", 1
)

cancel_string = String()
cancel_string.data = "stop"
self.__cancellation_pub.publish(cancel_string)

Closing issue, even though I think this should be documented and the cancel action method should be supported.