DistanceToCollision within a Moveit Planner

edit

Hello,

I am trying to create a planner plugin within MoveIt, based on the CHOMP Planner already implemented. For this I need a distance function which returns the minimum distance between robot and environment, which DistanceToCollision provides. In order for the obstacles to be recognized and to avoid:

https://github.com/ros-planning/movei...

I use planning_scene_interface_.getObjects(). All of this works when the c++ code is run with moveit running, however when added to the planner this line causes an infinite loop, perhaps because the PlanningSceneInterface requires move_group, and the planner is run from it.

What could be done to solve this? Or alternatively is there another function that calculates distance to environment which does not require the PlanningSceneInterface and would work in the planner?

Thank you in advance.