Added a poi visualizer for Rviz