Ask Your Question
0

Stop joint_trajectory_controller execution at rostopic event

asked 2018-03-14 04:31:07 -0600

dhindhimathai gravatar image

I'm on Ubuntu 14.04 with ROS Indigo.

I am using joint_trajectory_controller to control a single actuated joint robotic hand. After having send a control_msgs::FollowJointTrajectoryGoal to joint_trajectory_controller through an actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>, I'm listening for an event on a particular topic.

When this event occurs, I would like to interrupt the execution of the previously sent trajectory and stop the hand joint at the current position.

I have tried to get the joint value from the joint state topic and to publish this of the joint_trajectory_controller but this does not stop the execution of the previous trajectory.

I also tried to send an empty goal to the topic /hand_name/joint_trajectory_controller/follow_joint_trajectory/goal but this didn't work either.

An outline of the code is the following:

std::shared_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>> client;
client = std::make_shared<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>>("/hand_name/joint_trajectory_controller/follow_joint_trajectory", true);

if (client->waitForServer(ros::Duration(1,0))){
        control_msgs::FollowJointTrajectoryGoal joint_goal = create_trajectory_goal(joint_position);
        client->sendGoalAndWait(joint_goal, ros::Duration(0.5));
}

int id = 0;
while(id == 0){
    ROS_INFO("I'm waiting for event!\n");
    std_msgs::Int8::ConstPtr id_ptr;
    id_ptr = ros::topic::waitForMessage<std_msgs::Int8>("/event_topic", n);
    id = id_ptr->data;

    if (!id) {
        ROS_INFO("No event yet!\n");
    } else {
        ROS_INFO("Event detected with id %d!\n", id);

        // THIS IS WHERE I NEED TO STOP THE JOINT TRAJECTORY CONTROLLER AND SEND ANOTHER TRAJECTORY

    }
}

Thank you in advance for the help.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
2

answered 2018-03-14 04:46:04 -0600

gvdhoorn gravatar image

updated 2018-03-14 05:26:24 -0600

client->sendGoalAndWait(joint_goal, ros::Duration(0.5));

It will depend on joint_trajectory_controller playing nice (ie: be properly implemented, but I believe it is), but if you don't use client->sendGoalAndWait(..), but instead client->sendGoal(..), you don't block and can then use any of SimpleActionClient::cancel*Goal(..) methods (documentation) to cancel the current goal.

int id = 0;
while(id == 0){
    ROS_INFO("I'm waiting for event!\n");
    std_msgs::Int8::ConstPtr id_ptr;
    id_ptr = ros::topic::waitForMessage<std_msgs::Int8>("/event_topic", n);
    [..]

Please don't do this. This is explicitly not what waitForMessage(..) was created for. Under the hood, you're now creating and destroying ros::Publisher instances, which takes time. As they need to resubscribe you run the risk of missing events, undersampling (aliasing) and other nasty effects.

ROS is an event based framework. Subscribe to /event_topic in the normal way, and then in your callback check whether you have an active goal (that you sent using client->sendGoal(..) earlier). If you don't, return from your callback without doing anything (you could of course also only subcribe to /event_topic right before you submit your goal). If there is an active goal, check your threshold (or the id if you have to) and then cancel the goal from the msg callback using client->cancelGoal().

edit flag offensive delete link more

Comments

Sorry for the delay and thank you so much for the perfect answer. Everything now works fine. Thank you also for the corrections on the waitForMessage(...) issue.

dhindhimathai gravatar imagedhindhimathai ( 2018-03-18 05:31:02 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2018-03-14 04:31:07 -0600

Seen: 218 times

Last updated: Mar 14 '18