Revision history [back]

I have solved that problem. You mustn't put all poses in your vector plan. I think makePlan() is called every time when the robot passes the next PoseStamped and you only have to put start, your next pose and the goal into it. I changed the makePlan() to:

plan.clear();

plan.push_back(start);
if(pose_num <= pose_num_max) plan.push_back(poses[pose_num]);
plan.push_back(goal);
pose_num++;

return true;
}

The start and goal argument is sent by move_base.