Goals skipped while sending multiple goals to navigation-stack
Hello, so i have been writing a node based on simple action client in ROS that sends a list of goals to navigation stack one by one. the issue is some goals are skipped, looks like the move_base server gets the goal, plans a path (it is shown on RViz) and then immediately skips it and processes another goal, so what happens is that some goals are processed some are not. here is the code:
#include <iostream>
#include <ros/ros.h>
#include <move_base/move_base.h>
#include <actionlib/client/simple_action_client.h>
#include <vector>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> xbotclient;
int main(int argc, char **argv){
ROS_INFO("Simulation is begining");
// initialize the node
ros::init(argc, argv, "xbot_planner");
// declare the client
xbotclient op("move_base",true);
std::vector <std::vector<float>> mcn_cord_nd_orent{{9,9.3,1,0},{3.5,9.3,1,0},{-3,9.3,1,0},{-9,9.3,1,0},{9,9.3,1,0},{3.5,9.3,1,0},{-3,9.3,1,0},{-9,9.3,1,0},{7.2,-6,-0.707,0.707},{3,-6,-0.707,0.707},{-1.4,-6,-0.707,0.707},{-6,-6,-0.707,0.707}};
while(!op.waitForServer(ros::Duration(5.0))){
ROS_INFO("waiting for move base server to come up");
}
ros::Time startTime = ros::Time::now();
ros::Duration cycleDuration(600);
int i = 0;
while(ros::Time::now() < startTime+cycleDuration){
move_base_msgs::MoveBaseGoal goal1;
goal1.target_pose.header.frame_id = "map";
goal1.target_pose.header.stamp = ros::Time::now();
goal1.target_pose.pose.position.x = mcn_cord_nd_orent[i][0];
goal1.target_pose.pose.position.y = mcn_cord_nd_orent[i][1];
goal1.target_pose.pose.orientation.z = mcn_cord_nd_orent[i][2];
goal1.target_pose.pose.orientation.w = mcn_cord_nd_orent[i][3];
ROS_INFO("SENDING GOAL");
op.sendGoal(goal1);
ROS_INFO("goal 1 sent");
op.waitForResult();
if(op.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
ROS_INFO("OPERATION completed");
i = i + 1;
}
else{
ROS_INFO("operation failed");
return 0;
}
if (i == 7){
i = 0;
}
}
return 0;
}