Beispiel #1
0
void ompl::tools::Lightning::convertPlannerData(const ob::PlannerDataPtr& plannerData, og::PathGeometric &path)
{
    // Convert the planner data verticies into a vector of states
    for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
        path.append(plannerData->getVertex(i).getState());
}
bool ompl::LTLVis::VRVPlanner::DijkstraSearch(const ompl::base::PlannerTerminationCondition &ptc, ompl::geometric::PathGeometric &solution)
{
  //update the value we use to check that a vertex has been initialized, or visited, in the current search
  ++crVisitation_;
  WorkingVertexVector pqueue;
  pqueue.clear();
  bool reachedGoal = false;
  bool goalDisconnected = false;

  ompl::LTLVis::tIndex goalIndexReached = 0;

  WorkingVertexVector pathCandidate;

  ompl::LTLVis::tIndex maxK = vertices_.size();
  for(ompl::LTLVis::tIndex k = 0; k < maxK; k++)
  {
    if(vertices_[k].getIsStart())
    {
      vertices_[k].setNegativeDistance(0);
      vertices_[k].setLastInitAlg(crVisitation_);
      pqueue.push_back(&vertices_[k]);
    }
  }
  make_heap(pqueue.begin(), pqueue.end(), VRVVertex::VRVVertexCompare);

  while(pqueue.size() && (ptc == false))
  {
    VRVVertex *cVertex(pqueue.front());
    pop_heap(pqueue.begin(), pqueue.end(), VRVVertex::VRVVertexCompare); pqueue.pop_back();

    if(cVertex->getLastVisitation() < crVisitation_)
    {
      cVertex->setLastVisitation(crVisitation_);
      if(cVertex->getIsGoal())
      {
        reachedGoal = true;
        goalIndexReached = cVertex->getIndex();
        break;
      }
      ompl::LTLVis::tIndex kMax = cVertex->getEdgeCount();
      for(ompl::LTLVis::tIndex k = 0; k < kMax; k++)
      {
        double weight;
        ompl::LTLVis::tIndex towards;
        cVertex->getEdge(k, weight, towards);
        VRVVertex *cTarget(&vertices_[towards]);
        if(cTarget->isInteresting())
        {
          //STL heap is a max heap, so we use negative edge weights to 'convert' it to a min-heap
          if(cTarget->getLastInitAlg() < crVisitation_)
          {
            cTarget->setLastInitAlg(crVisitation_);
            cTarget->setNegativeDistance(cVertex->getNegativeDistance() - weight - cTarget->getNodeCost());
            cTarget->setPreceding(cVertex->getIndex());
            cTarget->setPrecedingEdge(k);
            pqueue.push_back(cTarget); push_heap(pqueue.begin(), pqueue.end(), VRVVertex::VRVVertexCompare);
          }
          else
          {
            double negativeDist = cVertex->getNegativeDistance() - weight - cTarget->getNodeCost();
            if(negativeDist > cTarget->getNegativeDistance())
            {
              cTarget->setNegativeDistance(negativeDist);
              cTarget->setPreceding(cVertex->getIndex());
              cTarget->setPrecedingEdge(k);
              pqueue.push_back(cTarget); push_heap(pqueue.begin(), pqueue.end(), VRVVertex::VRVVertexCompare);
            }
          }
        }
      }
    }
  }

  if(reachedGoal)
  {
    pathCandidate.clear();
    ompl::LTLVis::tIndex cIndex = goalIndexReached;
    while(!vertices_[cIndex].getIsStart())
    {
      pathCandidate.push_back(&vertices_[cIndex]);
      cIndex = vertices_[cIndex].getPreceding();
    }
    pathCandidate.push_back(&vertices_[cIndex]);
    ompl::LTLVis::tIndex kMax = pathCandidate.size();
    for(ompl::LTLVis::tIndex k = 0, kPrev = 0; (k < kMax); k++)
    {
      if(0 < k)
      {
        if(0.000001 < si_->distance(pathCandidate[kMax - 1 - k]->getStateData(), pathCandidate[kMax - 1 - kPrev]->getStateData()))
        {
          solution.append(pathCandidate[kMax - 1 - k]->getStateData());
          kPrev = k;
        }
      }
      else
      {
        solution.append(pathCandidate[kMax - 1 - k]->getStateData());
      }
    }
    return true;
  }
  return false;
}