added a method that works - but going to test out a better one
alternate method working - we find the valid one by working backwards from the last point - which means that it is a little less efficient
made the distance configurable - also fixed a bug that was not declaring faliure when it didnt find a valid waypoint - and removed the drawing stuff from libbot
removed the verbose outputs
removed the verbose outputs
updated the logic and added new param to enable waypoint pruning - right now it will pick the first one outside the threshold
fixed the logic such that it will take the first waypoint inside the threshold if valid - otherwise uses the closest one to the robot that is valid
updated with the comments for the pull request
Merge pull request #15 from bosch-ros-pkg/trajectory_planner_waypoint_threshold_better

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