Add octomap 1.6.0