Added octomap package and visualizer