WIP: Visualize navigation plan if user wants

To close #81.