fix bug where setting destination killed waypoints in some cases