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