Calculating distance to octomap

edit

Hi there

I have followed the steps in the perception pipeline tutorial in order to insert an octomap into into my planning scene as a collision object, this works fine.

Now I would like to be able to calculate the distance from my robots end effector to the closest node of the octomap but I have no idea where to get started. Has anybody got some experiance on this topic?

Also, if it is not possible to directly calculate the distance to the node, can anybody help extract the coordinates of each node from the octree?