37 #include "ompl/geometric/planners/prm/LazyPRM.h"
38 #include "ompl/base/OptimizationObjective.h"
39 #include <boost/graph/incremental_components.hpp>
40 #include <boost/graph/lookup_edge.hpp>
41 #include <boost/foreach.hpp>
43 #define foreach BOOST_FOREACH
46 PRM(si, starStrategy),
47 vertexValidityProperty_(boost::get(vertex_flags_t(), g_)),
48 edgeValidityProperty_(boost::get(edge_flags_t(), g_))
53 ompl::geometric::LazyPRM::~LazyPRM()
59 boost::mutex::scoped_lock _(graphMutex_);
61 Vertex m = boost::add_vertex(g_);
62 stateProperty_[m] = state;
63 vertexValidityProperty_[m] = VALIDITY_UNKNOWN;
66 disjointSets_.make_set(m);
72 const std::vector<Vertex>& neighbors = connectionStrategy_(m);
74 foreach (Vertex n, neighbors)
75 if (connectionFilter_(m, n))
77 const base::Cost weight = opt_->motionCost(stateProperty_[m], stateProperty_[n]);
78 const unsigned int id = maxEdgeID_++;
79 const Graph::edge_property_type properties(weight,
id);
80 const Edge &e = boost::add_edge(m, n, properties, g_).first;
81 edgeValidityProperty_[e] = VALIDITY_UNKNOWN;
82 uniteComponents(n, m);
94 simpleSampler_->sampleUniform(workState);
95 addMilestone(si_->cloneState(workState));
104 simpleSampler_ = si_->allocStateSampler();
112 std::vector<const base::State*> states;
113 Vertex prevVertex = goal;
114 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
117 unsigned int &vd = vertexValidityProperty_[pos];
118 if ((vd & VALIDITY_TRUE) == 0)
119 if (si_->isValid(st))
121 if ((vd & VALIDITY_TRUE) == 0)
126 si_->freeState(stateProperty_[pos]);
127 boost::clear_vertex(pos, g_);
128 boost::remove_vertex(pos, g_);
134 if (prevVertex != pos)
136 Edge e = boost::lookup_edge(prevVertex, pos, g_).first;
137 unsigned int &evd = edgeValidityProperty_[e];
138 if ((evd & VALIDITY_TRUE) == 0)
139 if (si_->checkMotion(states.back(), st))
140 evd |= VALIDITY_TRUE;
141 if ((evd & VALIDITY_TRUE) == 0)
144 boost::remove_edge(e, g_);
150 states.push_back(st);
157 states.push_back(stateProperty_[start]);
160 for (std::size_t i = 0 ; i < states.size() ; ++i)
virtual Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
virtual void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState)
Randomly sample the state space, add and connect milestones in the roadmap. Stop this process when th...
void setName(const std::string &name)
Set the name of the planner.
virtual base::PathPtr constructGeometricPath(const boost::vector_property_map< Vertex > &prev, const Vertex &start, const Vertex &goal)
Given a solution represented as a vector of predecesors in the roadmap, construct a geometric path...
virtual void constructRoadmap(const base::PlannerTerminationCondition &ptc)
For LazyPRM, this simply calls growRoadmap()
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
void reverse(void)
Reverse the path.
Definition of an abstract state.
Probabilistic RoadMap planner.
Definition of a geometric path.
A boost shared pointer wrapper for ompl::base::Path.