Work on rviz visualization

- copied stuff from roamfree node
- added launchfile with also rviz