Feature to allow the global planner to find a close by point with a better clearance from obstacles

Added option to search in a local region around the original goal (or obstacle free goal) and select a new goal if the obstacle cost is smaller than the original goal point 

Use clear_goal_tolerance to set this value. 

Note : Since there is no explicit connectivity check from the original goal to the new one - avoid setting high values for this - this can potentially cause the goal to jump to the other side of walls.