Using actionlib client within subscriber callback

edit

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;

}