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