actionlib status update problem
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
What is the terminal output?
Can you provide more info to find the error? Do you have a repository? If you want, I have one where I used action server to send a robot to a point
http://www.theconstructsim.com/go-poi...
https://bitbucket.org/TheConstruct/th...