Esempio n. 1
0
void SBLPRT::CreatePath(int i,int j,MilestonePath& path)
{
  Assert(i >= 0 && i < (int)roadmap.nodes.size());
  Assert(j >= 0 && j < (int)roadmap.nodes.size());
  ConnectedSeedCallback callback(this,j);
  callback.node = j;
  roadmap.NewTraversal();
  roadmap._BFS(i,callback);
  if(!callback.found) {
    cerr<<"SBLPRT::CreatePath: Warning, a path doesn't exist between nodes "<<i<<" and "<<j<<endl;
    return;
  }

  list<PathInfo> subpaths;
  int n=j;
  while(n != i) {
    int p=callback.parents[n];
    Assert(p >= 0);
    PathInfo temp;
    temp.tstart = p;
    temp.tgoal = n;
    temp.path = roadmap.FindEdge(n,p);
    Assert(temp.path != NULL);
    Assert(!temp.path->edges.empty());
    subpaths.push_front(temp);
    n=p;
  }
  for(list<PathInfo>::iterator it=subpaths.begin();it!=subpaths.end();it++) {
    int s=it->tstart;
    int g=it->tgoal;
    Assert(s >= 0 && s <(int)roadmap.nodes.size());
    Assert(g >= 0 && g <(int)roadmap.nodes.size());
    MilestonePath& subpath=*it->path;
    Assert(!subpath.edges.empty());
    //forward path or backward path?
    if(subpath.edges.front()->Start() == *roadmap.nodes[s]->root) {
      Assert(subpath.edges.back()->Goal() == *roadmap.nodes[g]->root);
      //forward path
      path.Concat(subpath);
      if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on %d %d\n",s,g);
    }
    else {
      Assert(subpath.edges.front()->Start() == *roadmap.nodes[g]->root);
      Assert(subpath.edges.back()->Goal() == *roadmap.nodes[s]->root);
      //backward path
      for(int k=(int)subpath.edges.size()-1;k>=0;k--) {
	path.edges.push_back(subpath.edges[k]->ReverseCopy());
      }
      if(!path.IsValid()) fprintf(stderr,"SBLPRT::CreatePath: Path invalidated on backwards %d %d\n",s,g);
    }
  }
  Assert(path.IsValid());
}
Esempio n. 2
0
void ReversePath(MilestonePath& path)
{
  for(size_t k=0;k<path.edges.size()/2;k++) {
    SmartPointer<EdgePlanner> e1 = path.edges[k];
    SmartPointer<EdgePlanner> e2 = path.edges[path.edges.size()-k];
    path.edges[k] = e2->ReverseCopy();
    path.edges[path.edges.size()-k] = e1->ReverseCopy();
  }
  if(path.edges.size()%2 == 1)
    path.edges[path.edges.size()/2] = path.edges[path.edges.size()/2]->ReverseCopy();
  if(!path.IsValid()) fprintf(stderr,"ReversePath : Path invalidated ?!?!\n");
}
Esempio n. 3
0
void RoadmapPlanner::CreatePath(int i,int j,MilestonePath& path)
{
  Assert(ccs.SameComponent(i,j));
  //Graph::PathIntCallback callback(roadmap.nodes.size(),j);
  //roadmap._BFS(i,callback);
  EdgeDistance distanceWeightFunc;
  Graph::ShortestPathProblem<Config,SmartPointer<EdgePlanner> > spp(roadmap);
  spp.InitializeSource(i);
  spp.FindPath_Undirected(j,distanceWeightFunc);
  if(IsInf(spp.d[j])) {
    FatalError("RoadmapPlanner::CreatePath: SameComponent is true, but no shortest path?");
    return;
  }

  list<int> nodes;
  //Graph::GetAncestorPath(callback.parents,j,i,nodes);
  bool res=Graph::GetAncestorPath(spp.p,j,i,nodes);
  if(!res) {
    FatalError("RoadmapPlanner::CreatePath: GetAncestorPath returned false");
    return;
  }
  if(nodes.front() != i || nodes.back() != j) {
    FatalError("RoadmapPlanner::CreatePath: GetAncestorPath didn't return correct path? %d to %d vs %d to %d",nodes.front(),nodes.back(),i,j);
  }
  Assert(nodes.front()==i);
  Assert(nodes.back()==j);
  path.edges.clear();
  path.edges.reserve(nodes.size());
  for(list<int>::const_iterator p=nodes.begin();p!=--nodes.end();++p) {
    list<int>::const_iterator n=p; ++n;
    SmartPointer<EdgePlanner>* e=roadmap.FindEdge(*p,*n);
    Assert(e);
    if(*e == NULL) {
      //edge data not stored
      path.edges.push_back(space->LocalPlanner(roadmap.nodes[*p],roadmap.nodes[*n]));
    }
    else {
      //edge data stored
      if((*e)->Start() == roadmap.nodes[*p]) {
	//path.edges.push_back((*e)->Copy());
	path.edges.push_back(*e);
      }
      else {
	Assert((*e)->Goal() == roadmap.nodes[*p]);
	path.edges.push_back((*e)->ReverseCopy());
      }
    }
  }
  Assert(path.IsValid());
}