move_base bug when get a new goal before makePlan() returns a planner_plan for old goal

[shiningjuly]

This log file records what I met. After the first goal arrived, the robot received a second goal(different from the first, 180 degrees inverse), then move_base returned "goal arrived" immediately, while the robot didn't move at all.

And I think I've found the reason.
The planThread is making plan for the first goal(planner_frequency > 0), but the executeCycle() has reached goal and returned. Then the second goal arrives and executeCb() enters loop again, waiting for new_global_plan available to set plan to local planner. Now makePlan() returns, setting new_global_plan true, but the global plan is for the first goal. So the plan for the first goal is passed to local planner. Since the robot is already there, local planner returns "goal reached" immediately.

To simulate the delay in makePlan(), I add 15 seconds sleep before makePlan(), and I can easily repeat this phenomenon.

Here is my solution.
After makePlan() returns with a planner_plan, the planThread should check if a new planner_goal is available. If that's the case, the planner_plan should be abandoned, and planThread should go back to make plan for the new planner_goal.
I think this is reasonable, because as soon as a new goal arrives, the plan for the old goal should not be used.