virtual void ConnectHint(int n) { if(knn) prm.ConnectToNearestNeighbors(n,knn,!ignoreConnectedComponents); else prm.ConnectToNeighbors(n,connectionThreshold,!ignoreConnectedComponents); if(!storeEdges) { RoadmapPlanner::Roadmap::Iterator e; for(prm.roadmap.Begin(n,e);!e.end();++e) *e = NULL; } }
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); }