Adding / extending octomap packages