Fixed a bug on isGoalReached method

[LucasNolasco]

There was a bug on goal_reached_ flag where the robot wouldn't realize that a goal is reached. It was caused by a new global plan arriving before the move_base check if the goal is reached.

How the bug happens: The computeVelocityCommands method set the flag to true, then a new plan arrives and the setPlan method is called. Then, on the setPlan, the flag is set back to false before it was actually checked. So when the isGoalReached method is called, it keeps returning false.

Proposed Solution: The solution proposed here is to check the goal using the global plan, so it doesn't depend on the goal_reached_ flag anymore and even with a new plan be capable to realize that the goal is reached.

[amakarow]

Sounds good, hope to get some time to test it.

[RohanDwivedi]

Is there a particular reason why this is not merged yet? I have ran into the same issue.

[VicenteQueiroz]

I only had the early goal reach problem when I had planner_frequency > 0
With this solution, I would get a continuous error saying I had an empty global plan. My workaround was to, instead of clearing the global plan, I would return false, and the next iteration would have the correct goal.

if (isGoalReached())
{
  return false; // global_plan_.clear();
}

[LucasNolasco]

I haven't returned to this issue in a long time, but I'd be willing to try to replicate the problem mentioned by @VicenteQueiroz if there's still any interest in merging this PR