Adds path_navigation and a bagfile
Merge pull request #3 from JohanJohansson/master

Adds path_navigation and a bagfile