Revision history [back]

Collision checking based on OctoMap is already implemented in the arm_navigation stack or better in its successor MoveIt, so the simplest thing would be to use that for collision checks. As an alternative you will have to iterate over the occupied voxels of OctoMap (best in a local bounding box) and see if they intersect with the robot.

Distance queries based on an existing OctoMap can be answered with the dynamicEDT3D library, which is distributed along with the source of OctoMap (although not yet released in ROS).