I have saved a few PoseStamped in a file to describe a fixed path in a fixed map, now I want to make a plan out of them and send it to move_base. I wrote my own globalplanner-plugin for move_base, and stacked all my Poses in the vector plan of makePlan(). When I am starting the navigation, everything starts fine but after the robot passes the first Pose, he starts driving forwards and backwards around that point...
This is my makePlan(), where poses is a vector and contains the different PoseStampeds:
bool MyGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector& plan){
if(!initialized_){
ROS_ERROR("The planner has not been initialized, please call initialize() to use the planner");
return false;
}
plan.clear();
plan.push_back(start);
for(int i=0; i
↧