How to stop the synchronous execution of a trajectory?

[David Brodeur]

Hi,

I have a ROS node that executes pose commands (move_group interface) and publishes status information about the current pose of the robot and other data : 


int main(int argc, char **argv) {

    ros::init(argc, argv, "actuator_fanuc");
    ros::NodeHandle nh, np("~");

    ros::AsyncSpinner spinner(1);
    ros::Rate rate(5);

    ActuatorFanuc actuator(nh, np);

    spinner.start();

    actuator.init();

    while (ros::ok) {

        actuator.execute();
        actuator.status();

        rate.sleep();
    }

    spinner.stop();
}


The ActuatorFanuc class includes a subscriber/callback to receive new target poses or stop requests. ActuatorFanuc::execute() uses move_group blocking method to plan and execute a trajectory to a given target.

This uses an asyncSpinner like in the examples I read on different MoveIt! tutorials. My concern is that when an execution occurs and since the method is blocking, I am not able to receive any new incoming command through a callback. So if I need to stop the execution for an emergency issue, I don't know what to do. I thought that putting 2 threads instead of one in the spinner would help, but not. Should I had more threads or do something different?

Also, I would prefer not to use the asynchronous (non blocking) version move_group::asyncExecute() because even thought it lets me receive new commands through callback, I cannot know when the trajectory has completed and I get bad behaviors.

Thanks for your help!

Regards,

David Brodeur

[David Brodeur]

Found a similar question/answer as mine here : https://groups.google.com/forum/#!searchin/moveit-users/synchronous/moveit-users/vy9u5GllJWQ/x92WfDHGjVIJ

Will try to do what they say first.

Regards,

David

[David Brodeur]

That made it. In the end, there is no way to stop a blocking trajectory execution.

However, using the asynchronous version, you get out of the function before the end of the execution. The thing is that if you want to know when the trajectory is completed, you have to subscribe to a ROS message of the following type : control_msgs/FollowJointTrajectoryActionResult. In my case with a Fanuc robot, it was /joint_trajectory_action/result.

That way, I can wait for completion of a trajectory before planning a new one, but I can still receive messages through callbacks during the execution. I can thus stop the robot.