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;
}