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/PDF.h"
41 #include "ompl/tools/config/SelfConfig.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)
105 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
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);
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);
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));
301 double sol_cost = -1.;
302 bool sol_cost_set =
false;
303 foreach (Vertex start, starts)
305 foreach (Vertex goal, goals)
307 if (boost::same_component(start, goal, disjointSets_) &&
311 if (pdef_->hasOptimizationObjective())
314 double obj_cost = pdef_->getOptimizationObjective()->getCost(p);
315 if (pdef_->getOptimizationObjective()->isSatisfied(obj_cost))
322 if (solution && !sol_cost_set)
324 sol_cost = pdef_->getOptimizationObjective()->getCost(solution);
328 if (!solution || obj_cost < sol_cost)
338 solution = constructSolution(start, goal);
350 return addedSolution_;
360 OMPL_ERROR(
"Goal undefined or unknown type of goal");
366 startM_.push_back(addMilestone(si_->cloneState(st)));
368 if (startM_.size() == 0)
370 OMPL_ERROR(
"There are no valid initial states!");
376 OMPL_ERROR(
"Insufficient states in sampleable goal region");
383 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
385 goalM_.push_back(addMilestone(si_->cloneState(st)));
389 OMPL_ERROR(
"Unable to find any valid goal states");
395 sampler_ = si_->allocValidStateSampler();
397 simpleSampler_ = si_->allocStateSampler();
399 unsigned int nrStartStates = boost::num_vertices(g_);
400 OMPL_INFORM(
"Starting with %u states", nrStartStates);
403 si_->allocStates(xstates);
407 addedSolution_ =
false;
415 while (ptcOrSolutionFound() ==
false)
429 OMPL_INFORM(
"Created %u states", boost::num_vertices(g_) - nrStartStates);
433 if (addedNewSolution())
434 pdef_->addSolutionPath (sol);
437 pdef_->addSolutionPath (sol,
true, 0.0);
440 si_->freeStates(xstates);
449 Vertex m = boost::add_vertex(g_);
450 stateProperty_[m] = state;
451 totalConnectionAttemptsProperty_[m] = 1;
452 successfulConnectionAttemptsProperty_[m] = 0;
455 disjointSets_.make_set(m);
456 graphMutex_.unlock();
459 if (!connectionStrategy_)
460 throw Exception(name_,
"No connection strategy!");
462 const std::vector<Vertex>& neighbors = connectionStrategy_(m);
464 foreach (Vertex n, neighbors)
465 if ((boost::same_component(m, n, disjointSets_) || connectionFilter_(m, n)))
467 totalConnectionAttemptsProperty_[m]++;
468 totalConnectionAttemptsProperty_[n]++;
469 if (si_->checkMotion(stateProperty_[m], stateProperty_[n]))
471 successfulConnectionAttemptsProperty_[m]++;
472 successfulConnectionAttemptsProperty_[n]++;
473 const double weight = distanceFunction(m, n);
474 const unsigned int id = maxEdgeID_++;
475 const Graph::edge_property_type properties(weight,
id);
478 boost::add_edge(m, n, properties, g_);
479 uniteComponents(n, m);
480 graphMutex_.unlock();
490 disjointSets_.union_set(m1, m2);
498 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
500 boost::astar_search(g_, start,
502 boost::predecessor_map(prev));
503 graphMutex_.unlock();
505 if (prev[goal] == goal)
506 throw Exception(name_,
"Could not find solution path");
508 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
509 p->
append(stateProperty_[pos]);
510 p->
append(stateProperty_[start]);
518 Planner::getPlannerData(data);
521 for (
size_t i = 0; i < startM_.size(); ++i)
524 for (
size_t i = 0; i < goalM_.size(); ++i)
528 foreach(
const Edge e, boost::edges(g_))
530 const Vertex v1 = boost::source(e, g_);
531 const Vertex v2 = boost::target(e, g_);