working example with all features, but calculating the octomap is slow