void ompl::geometric::LazyPRM::uniteComponents(Vertex a, Vertex b)
{
unsigned long int componentA = vertexComponentProperty_[a];
unsigned long int componentB = vertexComponentProperty_[b];
if (componentA == componentB) return;
if (componentSize_[componentA] > componentSize_[componentB])
{
std::swap(componentA, componentB);
std::swap(a, b);
}
markComponent(a, componentB);
}
void StatusTracker::markComponentActive(int id) {
markComponent(id, ACTIVE, Fence::NO_FENCE);
}
void StatusTracker::markComponentIdle(int id, const sp<Fence>& componentFence) {
markComponent(id, IDLE, componentFence);
}
ompl::base::PathPtr ompl::geometric::LazyPRM::constructSolution(const Vertex &start, const Vertex &goal)
{
// Need to update the index map here, becuse nodes may have been removed and
// the numbering will not be 0 .. N-1 otherwise.
unsigned long int index = 0;
boost::graph_traits<Graph>::vertex_iterator vi, vend;
for(boost::tie(vi, vend) = boost::vertices(g_); vi != vend; ++vi, ++index)
indexProperty_[*vi] = index;
boost::property_map<Graph, boost::vertex_predecessor_t>::type prev;
try
{
// Consider using a persistent distance_map if it's slow
boost::astar_search(g_, start,
boost::bind(&LazyPRM::costHeuristic, this, _1, goal),
boost::predecessor_map(prev).
distance_compare(boost::bind(&base::OptimizationObjective::
isCostBetterThan, opt_.get(), _1, _2)).
distance_combine(boost::bind(&base::OptimizationObjective::
combineCosts, opt_.get(), _1, _2)).
distance_inf(opt_->infiniteCost()).
distance_zero(opt_->identityCost()).
visitor(AStarGoalVisitor<Vertex>(goal)));
}
catch (AStarFoundGoal&)
{
}
if (prev[goal] == goal)
throw Exception(name_, "Could not find solution path");
// First, get the solution states without copying them, and check them for validity.
// We do all the node validity checks for the vertices, as this may remove a larger
// part of the graph (compared to removing an edge).
std::vector<const base::State*> states(1, stateProperty_[goal]);
std::set<Vertex> milestonesToRemove;
for (Vertex pos = prev[goal]; prev[pos] != pos; pos = prev[pos])
{
const base::State *st = stateProperty_[pos];
unsigned int &vd = vertexValidityProperty_[pos];
if ((vd & VALIDITY_TRUE) == 0)
if (si_->isValid(st))
vd |= VALIDITY_TRUE;
if ((vd & VALIDITY_TRUE) == 0)
milestonesToRemove.insert(pos);
if (milestonesToRemove.empty())
states.push_back(st);
}
// We remove *all* invalid vertices. This is not entirely as described in the original LazyPRM
// paper, as the paper suggest removing the first vertex only, and then recomputing the
// shortest path. Howeve, the paper says the focus is on efficient vertex & edge removal,
// rather than collision checking, so this modification is in the spirit of the paper.
if (!milestonesToRemove.empty())
{
unsigned long int comp = vertexComponentProperty_[start];
// Remember the current neighbors.
std::set<Vertex> neighbors;
for (std::set<Vertex>::iterator it = milestonesToRemove.begin() ; it != milestonesToRemove.end() ; ++it)
{
boost::graph_traits<Graph>::adjacency_iterator nbh, last;
for (boost::tie(nbh, last) = boost::adjacent_vertices(*it, g_) ; nbh != last ; ++nbh)
if (milestonesToRemove.find(*nbh) == milestonesToRemove.end())
neighbors.insert(*nbh);
// Remove vertex from nearest neighbors data structure.
nn_->remove(*it);
// Free vertex state.
si_->freeState(stateProperty_[*it]);
// Remove all edges.
boost::clear_vertex(*it, g_);
// Remove the vertex.
boost::remove_vertex(*it, g_);
}
// Update the connected component ID for neighbors.
for (std::set<Vertex>::iterator it = neighbors.begin() ; it != neighbors.end() ; ++it)
{
if (comp == vertexComponentProperty_[*it])
{
unsigned long int newComponent = componentCount_++;
componentSize_[newComponent] = 0;
markComponent(*it, newComponent);
}
}
return base::PathPtr();
}
// start is checked for validity already
states.push_back(stateProperty_[start]);
// Check the edges too, if the vertices were valid. Remove the first invalid edge only.
std::vector<const base::State*>::const_iterator prevState = states.begin(), state = prevState + 1;
Vertex prevVertex = goal, pos = prev[goal];
do
{
Edge e = boost::lookup_edge(pos, prevVertex, g_).first;
unsigned int &evd = edgeValidityProperty_[e];
if ((evd & VALIDITY_TRUE) == 0)
{
if (si_->checkMotion(*state, *prevState))
evd |= VALIDITY_TRUE;
}
if ((evd & VALIDITY_TRUE) == 0)
{
boost::remove_edge(e, g_);
unsigned long int newComponent = componentCount_++;
componentSize_[newComponent] = 0;
markComponent(pos, newComponent);
return base::PathPtr();
}
prevState = state;
++state;
prevVertex = pos;
pos = prev[pos];
}
while (prevVertex != pos);
PathGeometric *p = new PathGeometric(si_);
for (std::vector<const base::State*>::const_reverse_iterator st = states.rbegin(); st != states.rend(); ++st)
p->append(*st);
return base::PathPtr(p);
}
