37 #include "ompl/geometric/planners/prm/PRM.h"
38 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/datastructures/NearestNeighborsGNAT.h"
41 #include "ompl/datastructures/PDF.h"
42 #include <boost/lambda/bind.hpp>
43 #include <boost/graph/astar_search.hpp>
44 #include <boost/graph/incremental_components.hpp>
45 #include <boost/property_map/vector_property_map.hpp>
46 #include <boost/foreach.hpp>
47 #include <boost/thread.hpp>
49 #define foreach BOOST_FOREACH
50 #define foreach_reverse BOOST_REVERSE_FOREACH
76 base::Planner(si,
"PRM"),
77 starStrategy_(starStrategy),
81 weightProperty_(boost::get(boost::edge_weight, g_)),
82 edgeIDProperty_(boost::get(boost::edge_index, g_)),
83 disjointSets_(boost::get(boost::vertex_rank, g_),
84 boost::get(boost::vertex_predecessor, g_)),
86 userSetConnectionStrategy_(false),
96 ompl::geometric::PRM::~PRM(
void)
107 if (!connectionStrategy_)
114 if (!connectionFilter_)
115 connectionFilter_ = boost::lambda::constant(
true);
127 Planner::setProblemDefinition(pdef);
142 simpleSampler_.reset();
152 foreach (Vertex v, boost::vertices(g_))
153 si_->freeState(stateProperty_[v]);
165 simpleSampler_ = si_->allocStateSampler();
168 si_->allocStates(states);
169 expandRoadmap(ptc, states);
170 si_->freeStates(states);
174 std::vector<base::State*> &workStates)
182 foreach (Vertex v, boost::vertices(g_))
184 const unsigned int t = totalConnectionAttemptsProperty_[v];
185 pdf.
add(v, (
double)(t - successfulConnectionAttemptsProperty_[v]) / (
double)t);
191 while (ptc() ==
false)
193 Vertex v = pdf.
sample(rng_.uniform01());
194 unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates,
false);
198 Vertex last = addMilestone(si_->cloneState(workStates[s]));
201 for (
unsigned int i = 0 ; i < s ; ++i)
204 Vertex m = boost::add_vertex(g_);
205 stateProperty_[m] = si_->cloneState(workStates[i]);
206 totalConnectionAttemptsProperty_[m] = 1;
207 successfulConnectionAttemptsProperty_[m] = 0;
208 disjointSets_.make_set(m);
211 const double weight = distanceFunction(v, m);
212 const unsigned int id = maxEdgeID_++;
213 const Graph::edge_property_type properties(weight,
id);
214 boost::add_edge(v, m, properties, g_);
215 uniteComponents(v, m);
224 if (s > 0 || !boost::same_component(v, last, disjointSets_))
227 const double weight = distanceFunction(v, last);
228 const unsigned int id = maxEdgeID_++;
229 const Graph::edge_property_type properties(weight,
id);
230 boost::add_edge(v, last, properties, g_);
231 uniteComponents(v, last);
233 graphMutex_.unlock();
248 sampler_ = si_->allocValidStateSampler();
251 growRoadmap (ptc, workState);
252 si_->freeState(workState);
258 while (ptc() ==
false)
262 while (!found && ptc() ==
false)
264 unsigned int attempts = 0;
267 found = sampler_->sample(workState);
273 addMilestone(si_->cloneState(workState));
281 while (!ptc() && !addedSolution_)
288 goalM_.push_back(addMilestone(si_->cloneState(st)));
292 addedSolution_ = haveSolution (startM_, goalM_, solution);
294 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
302 foreach (Vertex start, starts)
304 foreach (Vertex goal, goals)
306 if (boost::same_component(start, goal, disjointSets_) &&
313 double pl = p->length();
314 if (pl < g->getMaximumPathLength())
321 if (solution && sl < 0.0)
322 sl = solution->length();
323 if (!solution || (solution && pl < sl))
332 solution = constructSolution(start, goal);
344 return addedSolution_;
354 logError(
"Goal undefined or unknown type of goal");
360 startM_.push_back(addMilestone(si_->cloneState(st)));
362 if (startM_.size() == 0)
364 logError(
"There are no valid initial states!");
370 logError(
"Insufficient states in sampleable goal region");
377 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
379 goalM_.push_back(addMilestone(si_->cloneState(st)));
383 logError(
"Unable to find any valid goal states");
389 sampler_ = si_->allocValidStateSampler();
391 simpleSampler_ = si_->allocStateSampler();
393 unsigned int nrStartStates = boost::num_vertices(g_);
394 logInform(
"Starting with %u states", nrStartStates);
397 si_->allocStates(xstates);
401 addedSolution_ =
false;
409 while (ptcOrSolutionFound() ==
false)
423 logInform(
"Created %u states", boost::num_vertices(g_) - nrStartStates);
427 if (addedNewSolution())
428 pdef_->addSolutionPath (sol);
431 pdef_->addSolutionPath (sol,
true, 0.0);
434 si_->freeStates(xstates);
443 Vertex m = boost::add_vertex(g_);
444 stateProperty_[m] = state;
445 totalConnectionAttemptsProperty_[m] = 1;
446 successfulConnectionAttemptsProperty_[m] = 0;
449 disjointSets_.make_set(m);
450 graphMutex_.unlock();
453 if (!connectionStrategy_)
454 throw Exception(name_,
"No connection strategy!");
456 const std::vector<Vertex>& neighbors = connectionStrategy_(m);
458 foreach (Vertex n, neighbors)
459 if ((boost::same_component(m, n, disjointSets_) || connectionFilter_(m, n)))
461 totalConnectionAttemptsProperty_[m]++;
462 totalConnectionAttemptsProperty_[n]++;
463 if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
465 successfulConnectionAttemptsProperty_[m]++;
466 successfulConnectionAttemptsProperty_[n]++;
467 const double weight = distanceFunction(m, n);
468 const unsigned int id = maxEdgeID_++;
469 const Graph::edge_property_type properties(weight,
id);
472 boost::add_edge(m, n, properties, g_);
473 uniteComponents(n, m);
474 graphMutex_.unlock();
484 disjointSets_.union_set(m1, m2);
492 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
494 boost::astar_search(g_, start,
496 boost::predecessor_map(prev));
497 graphMutex_.unlock();
499 if (prev[goal] == goal)
500 throw Exception(name_,
"Could not find solution path");
502 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
503 p->
append(stateProperty_[pos]);
504 p->
append(stateProperty_[start]);
512 Planner::getPlannerData(data);
515 for (
size_t i = 0; i < startM_.size(); ++i)
518 for (
size_t i = 0; i < goalM_.size(); ++i)
522 foreach(
const Edge e, boost::edges(g_))
524 const Vertex v1 = boost::source(e, g_);
525 const Vertex v2 = boost::target(e, g_);