Finish implement all the process of "mission_set_new_current_waypoint"