Added option to prevent corner cutting by setting the pruning distance of waypoints

Added params to enable the local planner to select the first valid waypoint within a given distance threshold

prune_waypoints : true enables this behavior
waypoint_prune_distance: pruning_distance (default 0.4) dictates the waypoint distance

Note that setting a very small value (e.g. 0.1) can lead to the robot moving slower because the valid trajectories that get close to the selected waypoint will be at slow velocities.
Added checking in the map_grid to default to the old behavior if the pruning conditions are not met.