Recursive bug algorithm implemented for initial waypoint check on input from globalnav
changes in obstacle detector thread.