octomap wrapper works for simple example