0
$\begingroup$

Rosanswers logo

I am using the move_arm package in the arm_navigation stack to control a joint trajectory, and I'd like to be able to interrupt (completely cancel) the trajectory while it's in progress.

Should I be able to do this by cancelling the MoveArmAction containing the arm pose goal? I've tried that, and while the MoveArmAction gets cancelled the trajectory itself does not (I don't get a preempt request on the joint controller action server).

I tried looking at the source for the move_arm node, and in move_arm_simple_action.cpp I found a private method called 'stopTrajectory()' that is never called anywhere in the MoveArm class. Is this a bug, or am I just not supposed to be able to cancel a joint trajectory with a MoveArmAction cancel request?


Originally posted by Teddy on ROS Answers with karma: 21 on 2012-07-19

Post score: 2


Original comments

Comment by Lorenz on 2012-07-19:
I guess this is a bug.

Comment by bit-pirate on 2012-07-19:
I think, it's the (joint trajectory) controllers job to stop the execution of the trajectory, if the corresponding action goal gets cancelled. Does the controller you use implement this?

Comment by Lorenz on 2012-07-19:
The move_arm action started the joint trajectory action. When it is cancelled, it doesn't seem to cancel the joint trajectory action and that is exactly the problem. The node that starts an action should also cancel it if it is cancelled itself.

Comment by bit-pirate on 2012-07-19:
Correct. If that doesn't happen, then it seems to be a bug indeed.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Finally I ran into this issue myself and started digging.

The existence of this "stopTrajectory()" function you mentioned makes me think, that the authors of move_arm actually had this situation in mind, but might just forgot to finally use/implement it.

Anyway, I found a good place to make use of this function and now when cancelling move_arm's action goal it in turn also cancels the controller's action goal, which makes my robot stop executing the trajectory. Here is the small patch:

--- a/move_arm/src/move_arm_simple_action.cpp   Thu Jun 28 18:29:21 2012 -0700
+++ b/move_arm/src/move_arm_simple_action.cpp   Thu Sep 27 09:40:43 2012 +0900
@@ -1140,6 +1140,7 @@
         else               //if we've been preempted explicitly we need to shut things down
         {
           ROS_DEBUG("The move arm action was preempted by the action client. Preempting this goal.");
+          stopTrajectory();
           revertPlanningScene();
           resetStateMachine();
           action_server_->setPreempted();

I also sent this patch in via this ticket.


Originally posted by bit-pirate with karma: 2062 on 2012-09-26

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.