Adds a check for null waypoint to fix nullpointerexception