37 #include "ompl/geometric/planners/prm/SPARStwo.h"
38 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include <boost/lambda/bind.hpp>
42 #include <boost/graph/astar_search.hpp>
43 #include <boost/graph/incremental_components.hpp>
44 #include <boost/property_map/vector_property_map.hpp>
45 #include <boost/foreach.hpp>
47 #include "GoalVisitor.hpp"
49 #define foreach BOOST_FOREACH
50 #define foreach_reverse BOOST_REVERSE_FOREACH
53 base::Planner(si,
"SPARStwo"),
55 sparseDeltaFraction_(.25),
56 denseDeltaFraction_(.001),
58 nearSamplePoints_((2*si_->getStateDimension())),
60 weightProperty_(boost::get(boost::edge_weight, g_)),
63 disjointSets_(boost::get(boost::vertex_rank, g_),
64 boost::get(boost::vertex_predecessor, g_)),
65 addedSolution_(false),
66 consecutiveFailures_(0),
92 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
94 double maxExt = si_->getMaximumExtent();
95 sparseDelta_ = sparseDeltaFraction_ * maxExt;
96 denseDelta_ = denseDeltaFraction_ * maxExt;
101 Planner::setProblemDefinition(pdef);
127 simpleSampler_.reset();
129 foreach (
Vertex v, boost::vertices(g_))
131 foreach (
InterfaceData &d, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_values)
133 if( stateProperty_[v] != NULL )
134 si_->freeState(stateProperty_[v]);
135 stateProperty_[v] = NULL;
146 foreach (
Vertex start, starts)
147 foreach (
Vertex goal, goals)
151 bool same_component = sameComponent(start, goal);
152 graphMutex_.unlock();
156 solution = constructSolution(start, goal);
165 return boost::same_component(m1, m2, disjointSets_);
170 return consecutiveFailures_ >= maxFailures_;
175 return consecutiveFailures_ >= maxFailures_ || addedSolution_;
185 constructRoadmap(ptcOrFail);
188 constructRoadmap(ptc);
193 checkQueryStateInitialization();
196 sampler_ = si_->allocValidStateSampler();
198 simpleSampler_ = si_->allocStateSampler();
204 std::vector<Vertex> graphNeighborhood;
206 std::vector<Vertex> visibleNeighborhood;
211 ++consecutiveFailures_;
214 if (!sampler_->sample(qNew))
217 findGraphNeighbors(qNew, graphNeighborhood, visibleNeighborhood);
219 if (!checkAddCoverage(qNew, visibleNeighborhood))
220 if (!checkAddConnectivity(qNew, visibleNeighborhood))
221 if (!checkAddInterface(qNew, graphNeighborhood, visibleNeighborhood))
223 if (visibleNeighborhood.size() > 0)
225 std::map<Vertex, base::State*> closeRepresentatives;
226 findCloseRepresentatives(workState, qNew, visibleNeighborhood[0], closeRepresentatives, ptc);
227 for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it)
229 updatePairPoints(visibleNeighborhood[0], qNew, it->first, it->second);
230 updatePairPoints(it->first, it->second, visibleNeighborhood[0], qNew);
232 checkAddPath(visibleNeighborhood[0]);
233 for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it)
235 checkAddPath(it->first);
236 si_->freeState(it->second);
241 si_->freeState(workState);
242 si_->freeState(qNew);
247 boost::mutex::scoped_lock _(graphMutex_);
248 if (boost::num_vertices(g_) < 1)
250 queryVertex_ = boost::add_vertex( g_ );
251 stateProperty_[queryVertex_] = NULL;
258 checkQueryStateInitialization();
264 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
270 startM_.push_back(addGuard(si_->cloneState(st), START));
273 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
279 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region", getName().c_str());
284 while (
const base::State *st = (goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal()))
285 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
288 OMPL_ERROR(
"%s: Unable to find any valid goal states", getName().c_str());
292 unsigned int nrStartStates = boost::num_vertices(g_) - 1;
293 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), nrStartStates);
296 addedSolution_ =
false;
306 constructRoadmap(ptcOrStop);
311 OMPL_INFORM(
"%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
314 pdef_->addSolutionPath(sol,
false);
323 while (!ptc && !addedSolution_)
330 goalM_.push_back(addGuard(si_->cloneState(st), GOAL));
334 addedSolution_ = haveSolution(startM_, goalM_, solution);
337 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
343 if (visibleNeighborhood.size() > 0)
346 addGuard(si_->cloneState(qNew), COVERAGE);
352 std::vector<Vertex> links;
353 if (visibleNeighborhood.size() > 1)
356 for (std::size_t i = 0; i < visibleNeighborhood.size(); ++i)
358 for (std::size_t j = i + 1; j < visibleNeighborhood.size(); ++j)
360 if (!sameComponent(visibleNeighborhood[i], visibleNeighborhood[j]))
362 links.push_back(visibleNeighborhood[i]);
363 links.push_back(visibleNeighborhood[j]);
366 if (links.size() > 0)
369 Vertex g = addGuard(si_->cloneState(qNew), CONNECTIVITY);
371 for (std::size_t i = 0; i < links.size() ; ++i)
373 if (!boost::edge(g, links[i], g_).second)
375 if (!sameComponent(links[i], g))
376 connectGuards(g, links[i]);
386 if (visibleNeighborhood.size() > 1)
387 if (graphNeighborhood[0] == visibleNeighborhood[0] && graphNeighborhood[1] == visibleNeighborhood[1])
389 if (!boost::edge(visibleNeighborhood[0], visibleNeighborhood[1], g_).second)
392 if (si_->checkMotion(stateProperty_[visibleNeighborhood[0]], stateProperty_[visibleNeighborhood[1]]))
395 connectGuards(visibleNeighborhood[0], visibleNeighborhood[1]);
404 Vertex v = addGuard(si_->cloneState(qNew), INTERFACE);
405 connectGuards(v, visibleNeighborhood[0]);
406 connectGuards(v, visibleNeighborhood[1]);
418 std::vector< Vertex > rs;
419 foreach(
Vertex r, boost::adjacent_vertices( v, g_ ) )
423 std::vector<Vertex> Xs;
426 std::vector<Vertex> VPPs;
428 for (std::size_t i = 0; i < rs.size() && !ret; ++i)
431 computeVPP(v, r, VPPs);
435 computeX(v, r, rp, Xs);
436 double rm_dist = 0.0;
439 double tmp_dist = (si_->distance( stateProperty_[r], stateProperty_[v] )
440 + si_->distance( stateProperty_[v], stateProperty_[rpp] ) )/2.0;
441 if( tmp_dist > rm_dist )
448 if (rm_dist > stretchFactor_ * d.
d_)
451 if (si_->checkMotion(stateProperty_[r], stateProperty_[rp]))
452 connectGuards(r, rp);
460 p->
append(stateProperty_[v]);
468 p->
append(stateProperty_[v]);
473 psimp_->reduceVertices(*p, 10);
474 psimp_->shortcutPath(*p, 50);
480 std::vector<base::State*>& states = p->
getStates();
485 vnew = addGuard(st , QUALITY);
487 connectGuards(prior, vnew);
492 connectGuards(prior, rp);
506 consecutiveFailures_ = 0;
511 visibleNeighborhood.clear();
512 stateProperty_[ queryVertex_ ] = st;
513 nn_->nearestR( queryVertex_, sparseDelta_, graphNeighborhood);
514 stateProperty_[ queryVertex_ ] = NULL;
517 for (std::size_t i = 0; i < graphNeighborhood.size() ; ++i )
518 if (si_->checkMotion(st, stateProperty_[graphNeighborhood[i]]))
519 visibleNeighborhood.push_back(graphNeighborhood[i]);
524 std::vector< Vertex > hold;
525 nn_->nearestR( v, sparseDelta_, hold );
527 std::vector< Vertex > neigh;
528 for (std::size_t i = 0; i < hold.size(); ++i)
529 if (si_->checkMotion( stateProperty_[v], stateProperty_[hold[i]]))
530 neigh.push_back( hold[i] );
532 foreach (
Vertex vp, neigh)
533 connectGuards(v, vp);
538 std::vector<Vertex> nbh;
539 stateProperty_[ queryVertex_ ] = st;
540 nn_->nearestR( queryVertex_, sparseDelta_, nbh);
541 stateProperty_[queryVertex_] = NULL;
543 Vertex result = boost::graph_traits<Graph>::null_vertex();
545 for (std::size_t i = 0 ; i< nbh.size() ; ++i)
546 if (si_->checkMotion(st, stateProperty_[nbh[i]]))
555 std::map<Vertex, base::State*> &closeRepresentatives,
558 for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it)
559 si_->freeState(it->second);
560 closeRepresentatives.clear();
563 for (
unsigned int i = 0 ; i < nearSamplePoints_ ; ++i)
567 sampler_->sampleNear(workArea, qNew, denseDelta_);
568 }
while ((!si_->isValid(workArea) || si_->distance(qNew, workArea) > denseDelta_ || !si_->checkMotion(qNew, workArea)) && ptc ==
false);
575 Vertex representative = findGraphRepresentative(workArea);
578 if (representative != boost::graph_traits<Graph>::null_vertex())
581 if (qRep != representative)
583 if (closeRepresentatives.find(representative) == closeRepresentatives.end())
585 closeRepresentatives[representative] = si_->cloneState(workArea);
590 addGuard(si_->cloneState(workArea), COVERAGE);
593 for (std::map<Vertex, base::State*>::iterator it = closeRepresentatives.begin(); it != closeRepresentatives.end(); ++it)
594 si_->freeState(it->second);
595 closeRepresentatives.clear();
604 std::vector<Vertex> VPPs;
605 computeVPP(rep, r, VPPs);
610 distanceCheck(rep, q, r, s, rp);
616 foreach(
Vertex cvpp, boost::adjacent_vertices( v, g_ ) )
618 if( !boost::edge( cvpp, vp, g_ ).second )
619 VPPs.push_back( cvpp );
626 foreach (
Vertex cx, boost::adjacent_vertices(vpp, g_))
627 if (boost::edge(cx, v, g_).second && !boost::edge(cx, vp, g_).second)
630 if ((vpp < cx && d.
pointA_) || (cx < vpp && d.pointB_))
643 throw Exception( name_,
"Trying to get an index where the pairs are the same point!");
648 return interfaceDataProperty_[v].interfaceHash[index( vp, vpp )];
663 if (d.pointB_ == NULL)
669 if (si_->distance(q, d.pointB_) < si_->distance(d.
pointA_, d.pointB_))
676 if (d.pointB_ == NULL)
686 if (si_->distance(q, d.
pointA_) < si_->distance(d.pointB_, d.
pointA_))
693 interfaceDataProperty_[rep].interfaceHash[index(r, rp)] = d;
698 stateProperty_[ queryVertex_ ] = st;
700 std::vector< Vertex > hold;
701 nn_->nearestR( queryVertex_, sparseDelta_, hold );
703 stateProperty_[queryVertex_] = NULL;
708 foreach (
VertexPair r, interfaceDataProperty_[v].interfaceHash | boost::adaptors::map_keys)
709 interfaceDataProperty_[v].interfaceHash[r].clear(si_);
715 boost::mutex::scoped_lock _(graphMutex_);
717 Vertex m = boost::add_vertex(g_);
718 stateProperty_[m] = state;
719 colorProperty_[m] = type;
721 assert(si_->isValid(state));
724 disjointSets_.make_set(m);
733 assert(v <= milestoneCount());
734 assert(vp <= milestoneCount());
736 const double weight = distanceFunction(v, vp);
737 const Graph::edge_property_type properties(weight);
738 boost::mutex::scoped_lock _(graphMutex_);
739 boost::add_edge(v, vp, properties, g_);
740 disjointSets_.union_set(v, vp);
745 boost::mutex::scoped_lock _(graphMutex_);
747 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
751 boost::astar_search(g_, start,
753 boost::predecessor_map(prev).
754 visitor(AStarGoalVisitor<Vertex>(goal)));
756 catch (AStarFoundGoal&)
760 if (prev[goal] == goal)
761 throw Exception(name_,
"Could not find solution path");
765 for (
Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
766 p->
append(stateProperty_[pos]);
767 p->
append(stateProperty_[start]);
776 Planner::getPlannerData(data);
779 for (
size_t i = 0; i < startM_.size(); ++i)
782 for (
size_t i = 0; i < goalM_.size(); ++i)
786 if (boost::num_edges( g_ ) > 0)
789 foreach (
const Edge e, boost::edges(g_))
791 const Vertex v1 = boost::source(e, g_);
792 const Vertex v2 = boost::target(e, g_);
802 OMPL_INFORM(
"%s: There are no edges in the graph!", getName().c_str());
805 foreach (
const Vertex n, boost::vertices(g_))
806 if (boost::out_degree(n, g_) == 0)
809 data.
properties[
"iterations INTEGER"] = boost::lexical_cast<std::string>(iterations_);
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
bool sameComponent(Vertex m1, Vertex m2)
Check if two milestones (m1 and m2) are part of the same connected component. This is not a const fun...
virtual ~SPARStwo(void)
Destructor.
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true...
bool haveSolution(const std::vector< Vertex > &start, const std::vector< Vertex > &goal, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
The planner failed to find a solution.
void findCloseRepresentatives(base::State *workArea, const base::State *qNew, Vertex qRep, std::map< Vertex, base::State * > &closeRepresentatives, const base::PlannerTerminationCondition &ptc)
Finds representatives of samples near qNew_ which are not his representative.
GoalType recognizedGoal
The type of goal specification the planner can use.
void findGraphNeighbors(base::State *st, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near st.
void setStretchFactor(double t)
Sets the stretch factor.
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp)
Performs distance checking for the candidate new state, q against the current information.
Abstract definition of goals.
SPARStwo(const base::SpaceInformationPtr &si)
Constructor.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st.
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added...
bool reachedFailureLimit(void) const
Returns whether we have reached the iteration failures limit, maxFailures_.
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
std::pair< bool, bool > checkAndRepair(unsigned int attempts)
Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invali...
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
bool checkAddCoverage(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure coverage of the space.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
virtual void setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Invalid start state or no start state specified.
void resetFailures(void)
A reset function for resetting the failures count.
Abstract definition of a goal region that can be sampled.
bool checkAddConnectivity(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure connectivity.
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
void computeVPP(Vertex v, Vertex vp, std::vector< Vertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the second interface (i.e. interface with larger index vertex).
Vertex addGuard(base::State *state, GuardType type)
Construct a guard for a given state (state) and store it in the nearest neighbors data structure...
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition permits, construct the spanner graph.
The goal is of a type that a planner does not recognize.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
The planner found an exact solution.
Interface information storage class, which does bookkeeping for criterion four.
base::PathPtr constructSolution(const Vertex start, const Vertex goal) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
bool checkAddInterface(const base::State *qNew, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the current sample reveals the existence of an interface, and if so...
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
void reverse(void)
Reverse the path.
A class to store the exit status of Planner::solve()
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
This class contains routines that attempt to simplify geometric paths.
virtual bool couldSample(void) const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
virtual unsigned int maxSampleCount(void) const =0
Return the maximum number of samples that can be asked for before repeating.
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
bool reachedTerminationCriterion(void) const
Returns true if we have reached the iteration failures limit, maxFailures_ or if a solution was added...
void checkQueryStateInitialization(void)
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
The exception type for ompl.
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
void freeMemory(void)
Free all the memory allocated by the planner.
void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)
High-level method which updates pair point information for repV_ with neighbor r. ...
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
void clearQuery(void)
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition of a geometric path.
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
SpaceInformationPtr si_
The space information for which planning is done.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
void abandonLists(base::State *st)
When a new guard is added at state st, finds all guards who must abandon their interface information ...
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector< Vertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v', and v".
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the first interface (i.e. interface with smaller index vertex).
A boost shared pointer wrapper for ompl::base::Path.
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.