fix waypoint bug
