3D costmap as obstacle for MoveIt!

edit

While not entirely identical to the way the 2DCostmap works, the Octomap is probably the closest alternative for 3D. Usually, the Octomap is fed by a depth camera, but it is also possible to specify the occupied voxels in any other way. The occupied voxels are then treated as collision obstacles, so MoveIt! planners will avoid any collisions with these voxels.

Have a look at this tutorial to see how it is used with a depth camera. This will also function for any other node publishing sensor_msgs/PointCloud2 messages on the input topic.