move_base action server returning SUCCEEDED immediately after sending goal
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.