actionlib status update problem

edit

Hello :)

I am having a problem with the actionlib library. I use the jade ROS version on ubuntu 14.

The problem is the following, I send goals of the type move_base_msgs::MoveBaseGoal goal to an actionlib server

I have several goals that must be reached by my robot in order. So what I do is : I send goals one by one, a status == SUCCEEDED being the signal for my program to send the next goal. Now this is sort of working. My problem is that when my robot reaches its destination and stops the status is updated only 30 to 45 sec after (from ACTIVE to SUCCEEDED). At some point I thought that it was because the tolerance of the goals both in terms of angle and position were low and that maybe my robot was kind of oscillating around the goal forever but I tried with huge tolerance parameters and it does not change anything. Does anyone know how to fix this problem ?

// notifies the robot that it can go 
greenLight = true;

statusSuscriber = n.subscribe("/move_base/status", 1, getStatus);

while(ros::ok() && (stage != path.size()) && greenLight){

    ROS_INFO("Heading towards target %d", stage+1);

    if(path.size()-1 == stage)
        ROS_INFO("This is my final destination");

    move_base_msgs::MoveBaseGoal goal;

    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    goal.target_pose.pose.position.x = path.at(stage).x;
    goal.target_pose.pose.position.y = path.at(stage).y;
    goal.target_pose.pose.orientation.x = 0;
    goal.target_pose.pose.orientation.y = 0;
    goal.target_pose.pose.orientation.z = 0;
    goal.target_pose.pose.orientation.w = 1;

    if(stage > 0 && path.at(stage-1).waitingTime >= 0){
        std::cout << "Hey I gotta wait for " << path.at(stage-1).waitingTime << std::endl;
        sleep(path.at(stage-1).waitingTime);
    }

    // sends the robot's goal to the server 
    ac->sendGoal(goal);

    // waits for the answer of the server regarding the status of the robot
    ac->waitForResult();

    if(ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
        ROS_INFO("Hooray, the base reached target %d", stage+1);
        stage++;
        // updates the path stage in the file
        std::ofstream path_stage_file(PATH_STAGE_FILE, std::ios::out | std::ios::trunc);

        if(path_stage_file){
            path_stage_file << stage;
            path_stage_file.close();
        } else 
            std::cout << "Sorry we were not able to find the file play_path.txt in order to keep track of the stage of the path to be played" << std::endl; 
    }
    else
        ROS_INFO("The base failed to reach target %d", stage+1);
}
if(stage == path.size() && ac->getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
    // the robot successfully reached all its goals in order 
    std::cout << "I have completed my journey Master Joda, what will you have me do ?" << std::endl;
    // resets the stage of the path to be able to play the path from the start again
    stage = 0;
}
else
    std::cout << "I was stopped trying to reach " << path.at(stage).x << " " << path.at(stage).y << std::endl; here

Thank you very much for your help