Trajectory planner waypoint threshold better

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.