added default tolerance and fixed behavior when a goal is obstructed

[jungladicitta]

As I remember, I had a problem when I was running navigation and explore_lite: explore_lite sends the goal -> as robot moves closer to goal explore_lite calculates and sends next goal, robot's velocity is not zero -> this next goal is obstructed, somewhere in the wall -> plan is not published, velocity is not zero -> robot moves with old velocity and hits the wall.

To fix this behavior I tried to use tolerance for global_planner and found out that it is not realized, and some similar issues was submitted. This fix simply does:

1. calculates 8 points on tolerance distance around the origin goal https://github.com/ros-planning/navigation/blob/b20495241ae81dd1dbeef4f585fb270cf4c8bda2/global_planner/src/planner_core.cpp#L317 this vector also contains origin goal.

2. sorts them by distance to current robot position

3. iterates them:

    for (auto goal : goals) {
           //bool found_legal = calculatePlan(...)
           //if (found_legal) {
           //        publishPlan(...);
           //        break;
           //}
           //else {
           //   ROS_ERROR("Failed to get a plan.");
           //   publishVelocity(0.0);
           //}
    }

[doisyg]

Thanks for this PR. I ended up re-implementing it targeting the last noetic-devel branch, https://github.com/ros-planning/navigation/pull/1041 Also I am not sure this PR was enabling a replanning to the closest valid point from the origin goal

[jungladicitta]

@doisyg sorry for not providing description of this PR at first. Now I have added it. I hope it helps