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
   

   navigation/global_planner/src/planner_core.cpp

   Line 317 in b204952

   auto goals = findToleratedPoints(wx, wy, tolerance, 8);

   
   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, #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