Hello Mattia,

Sorry for the delays in answering your related questions. The planning interface that MoveIt uses is abstracting the world representation in some sense. The planner gets access to a planning scene object, and that has functions to access the octomap. In particular, you need to call getWorld() on the PlanningScene, to get the collision world. Then, you need to call getObject(name) on the World to retrieve the object that corresponds to the octree. The name you need to pass must correspond to the octomap object, which has a special name in the PlanningScene: planning_scene::PlanningScene::OCTOMAP_NS. At this point, you can look in the world object, retrieve the first geometry element (a world object can have multiple geoms, but the octomap one has just one: the octree itself). This will give you an object of type shapes::Shape. That object you can cast to an shapes::OcTree and access the octomap::OccTree element.
Code would look something like this:

planning_scene::PlanningScene scene; // we have this from somewhere.
collision_detection::World::ObjectConstPtr obj = scene->getWorld()->getObject(planning_scene::PlanningScene::OCTOMAP_NS);
// we look only at obj->shapes_[0] because shape_poses_[0] is identity for octrees (currently) and there is just one shape.
const shapes::OcTree *ot_shape = dynamic_cast<const shapes::OcTree*>(obj->shapes_[0].get());
if (ot_shape) // if all is well, this should always pass
  boost::shared_ptr<const octomap::OcTree> ot = ot_shape->octree;
  // now use the octomap octree as I please

Additionally to this, if you are using OMPL, there is one more level of abstraction. OMPL does not care about world representations, only about a state validity function (e.g., a collision checker). This means that the planners (currently in OMPL) do not look at the world objects, and they cannot do that either. If you do need that, you will need to modify the ompl_interface package, model_based_planning_context.cpp file, in the solve() function (which calls the OMPL solve() function) to set the octree pointer for your planner. Here you would have to basically hack things a bit, and assume your planner, cast to that class type and set the octree which you obtained as indicated above. This will allow you to have access to the octree in the OMPL planner.
Hope this helps.


On Thu, Nov 14, 2013 at 5:19 AM, Mattia Gallegati <gallegatimattia@gmail.com> wrote:
Hi everyone,
I'm searching for a way to read the Octree of the environment from the new planners interface in MoveIt!. In my specific case I need to call a function (or a group of function) that returns the full Octree from inside the solve method of the new planners interface.
Can you help me please?
Best Regards