Making sure it navigates to the next waypoint when next action is not move_base type