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 =
// we look only at obj->shapes_ because shape_poses_ is identity for
octrees (currently) and there is just one shape.
const shapes::OcTree *ot_shape = dynamic_cast<const
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@...
> 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