The wait command speeds the action server thread so its unable to wake up and exit when a navigation request is preempted. We should update that with a while loop and check every 100ms if it should exit.