makePlan of GlobalPlanner plugin

Question

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<geometry_msgs::PoseStamped>& 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<poses.size();i++){
plan.push_back(poses[i]);
} 
return true;

}

---

Originally posted by Timo1804 on ROS Answers with karma: 18 on 2017-08-08

Post score: 0

---

Original comments

Comment by naveedhd on 2017-08-09:
It would help to publish the plan also so you can visualise in rviz if it is what you expect.

Comment by Timo1804 on 2017-08-09:
I have published it as nav_msgs::Path and it looks fine. Also the plan of the LocalPlanner looks good until he passes the first Pose after start-pose.

Answer 1

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.

---

Originally posted by Timo1804 with karma: 18 on 2017-08-09

This answer was ACCEPTED on the original site

Post score: 0

---

Original comments

Comment by naveedhd on 2017-08-11:
This doesn't seems to be the case. Publishing just the plan from standard global planner is a list of all the poses, and if the robot doesn't get stuck, this is only called once by move_base.

Comment by aarontan on 2018-06-29:
I am trying to solve a similar issue, do you have a repo that you may share for this?

Comment by naveedhd on 2018-07-02:
try setting the planner_frequency to 0 in move_base dynamic reconfigure. This way move base will not feed plan again and avoid robot oscillating.