예제 #1
0
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());
  }
}
예제 #2
0
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);
}