working version with octomap