Using actionlib client within subscriber callback

Question

Hi Guys,

I have been using the actionlib client to send goals to the navigation stack. Now the case is I have to get the goal position from a subscriber and I put the actionlib client in the callback of the subscriber as follows. The problem is we want the robot to be able to deal with new coming goal even the current goal has not reached, i.e. the robot will always moving towards the new goal. I don't know how to deal with the callback to deal with the latest coming message from the subscriber. Thanks.

void PoseCallback(const geometry_msgs::PoseStamped::ConstPtr& goal, MoveBaseClient* ac)
{
move_base_msgs::MoveBaseGoal mb_goal;
  
// fill in the mb_goal
  ...

  ac->sendGoal(mb_goal);
 
 ac->waitForResult();

  if(ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)

    ROS_INFO("The robot has reached the goal");
  else

    ROS_INFO("The base failed to get to the goal");

}

int main(int argc, char** argv)

{

  ros::init(argc, argv, "move_base_client");

  ros::NodeHandle nh;
 
  MoveBaseClient ac("move_base", true);

  while(!ac.waitForServer(ros::Duration(5.0)))

{
  
  ROS_INFO("Waiting for the move_base action server to come up");

  }
 
 ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("nav_topics", 1, boost::bind(PoseCallback, _1, &ac));
 
 ros::spin();
  
  return 0;

}

---

Originally posted by dalishi on ROS Answers with karma: 89 on 2014-08-21

Post score: 1

Answer 1

The problem is that you're blocking on the poseCb with:

ac->waitForResult();

Take a look at the SimpleActionClient API. The 'actionlib' way to do what you want is to register another callback - SimpleDoneCallback (e.g. doneCb) - when you send the goal, and have your poseCb return then and there.

In the doneCb, you will receive something every time a goal terminates. If you send goal 1, and then goal 2 before goal 1 completes, you will receive a doneCb with a SimpleClientGoalState reflecting the preempted or cancelled status, as per the SimpleActionClient contract that only one goal can be active at a time. For more reference check the flowchart and policies in http://wiki.ros.org/actionlib/DetailedDescription.

Of course, you could always just skip actionlib entirely and send goals to the move_base_simple/goaltopic instead, depending on your usecase.

---

Originally posted by paulbovbel with karma: 4518 on 2014-08-21

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by dalishi on 2014-08-21:
Thanks paulbovbel, I am switching to publishing to the move_base_simple/goal and will explore the actionlib way later. Thanks.

Answer 2

I believe you're more than capable to just send a new goal. Right now it seems that you're halting any future goals with this line of code:

  ac->waitForResult();

  if(ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)

    ROS_INFO("The robot has reached the goal");
  else

    ROS_INFO("The base failed to get to the goal");

To my knowledge, removing these lines will just send the most recent goal to the action server, and it will try to execute that until a new goal arrives.

Does this answer your question or is there something I've misread?

---

Originally posted by ahubers with karma: 301 on 2014-08-21

This answer was NOT ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by dalishi on 2014-08-21:
Thanks ahubers, you workaround should solve my problem. But in this way, I am not able to track the execution status of the goal that I have sent.