Fixed remove waypoint bug