void KrisLibraryOMPLPlanner::getPlannerData (ob::PlannerData &data) const
{
if(!planner) return;
OMPLCSpace* nc_cspace = const_cast<OMPLCSpace*>((const OMPLCSpace*)cspace);
RoadmapPlanner roadmap(nc_cspace);
MotionPlannerInterface* iplanner = const_cast<MotionPlannerInterface*>((const MotionPlannerInterface*)planner);
iplanner->GetRoadmap(roadmap);
for(size_t i=0;i<roadmap.roadmap.nodes.size();i++) {
unsigned int id = data.addVertex(ob::PlannerDataVertex(nc_cspace->ToOMPL(roadmap.roadmap.nodes[i]),(int)i));
Assert(id == i);
}
for(size_t i=0;i<roadmap.roadmap.nodes.size();i++) {
RoadmapPlanner::Roadmap::Iterator e;
for(roadmap.roadmap.Begin(i,e);!e.end();e++)
data.addEdge(e.source(),e.target());
}
}
PyObject* PlannerInterface::getRoadmap()
{
if(index < 0 || index >= (int)plans.size() || plans[index]==NULL)
throw PyException("Invalid plan index");
RoadmapPlanner prm(NULL);
plans[index]->GetRoadmap(prm);
PyObject* pyV = PyList_New(prm.roadmap.nodes.size());
for(size_t i=0;i<prm.roadmap.nodes.size();i++)
PyList_SetItem(pyV,(Py_ssize_t)i,PyListFromConfig(prm.roadmap.nodes[i]));
PyObject* pyE = PyList_New(0);
for(size_t i=0;i<prm.roadmap.nodes.size();i++) {
RoadmapPlanner::Roadmap::Iterator e;
for(prm.roadmap.Begin(i,e);!e.end();e++) {
PyObject* pair = Py_BuildValue("(ii)",e.source(),e.target());
PyList_Append(pyE,pair);
Py_XDECREF(pair);
}
}
//this steals the references
return Py_BuildValue("NN",pyV,pyE);
}