Esempio n. 1
0
  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;
    }
  }
Esempio n. 2
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());
  }
}
Esempio n. 3
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);
}