Stop joint_trajectory_controller execution at rostopic event

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

dhindhimathai
136 ●11 ●12 ●21

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.

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

gvdhoorn
86574 ●283 ●1432 ●1054 http://cor.tudelft.nl/

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

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 ( 2018-03-18 05:31:02 -0600 )edit

add a comment

Stop joint_trajectory_controller execution at rostopic event

1 Answer

Comments

Question Tools

Stats

Related questions

Stop joint_trajectory_controller execution at rostopic event edit

1 Answer

Comments

Question Tools

Stats

Related questions

Stop joint_trajectory_controller execution at rostopic event