move_base action server returning SUCCEEDED immediately after sending goal

asked 2019-03-10 17:24:33 -0500

FabianMene gravatar image

In my turtlebot simulation, I have a queue of move_base_mgs::MoveBaseGoal that I send to move_base's action server via an action client node I wrote. Upon completing an action the next one is sent, until the queue is empty, thus completing a predefined path. Quite often however, the server will return a SUCCEEDED goal state immediately upon sending the goal.

Here's the simplified c++-code:

class ActionClient{

actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> client;
std::queue<move_base_msgs::MoveBaseGoal> goals;

public: void goalCallback (const actionlib::SimpleClientGoalState& state){
  if (!goals.empty()){
    move_base_msgs::MoveBaseGoal next = goals.front();
    goals.pop();
    client.sendGoal(next, boost::bind(&SquarePathClient::goalCallback, this, _1),
         actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>::SimpleActiveCallback());
  }
}

public: ActionClient() : client("/move_base", true) {
  client.waitForServer(); 
  // Fill queue;
  client.sendGoal(firstGoal, boost::bind(&ActionClient::goalCallback, this, _1), 
          actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>::SimpleActiveCallback());
}

int main (int argc, char **argv){
  ros::init(argc, argv, "actionclient_node");
  ActionClient ac;
  ros::Rate r(100);
  while (ros::ok()){
    ros::spinOnce();
    r.sleep();
  }
  return 0;
}

The tolerances are set very tightly, so the goals are definitely not actually reached. I was thinking that the SUCCEEDED state might still be a leftover from the previous goal, thus triggering two callbacks, as the issue appears to occur mostly on every second goal sent, but I have found no way to check this.

edit retag flag offensive close merge delete