PRM.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Rice University
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Rice University nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, James D. Marble, Ryan Luna, Henning Kayser */
36 
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/base/objectives/PathLengthOptimizationObjective.h"
41 #include "ompl/datastructures/PDF.h"
42 #include "ompl/tools/config/SelfConfig.h"
43 #include "ompl/tools/config/MagicConstants.h"
44 #include <boost/graph/astar_search.hpp>
45 #include <boost/graph/incremental_components.hpp>
46 #include <boost/property_map/vector_property_map.hpp>
47 #include <boost/foreach.hpp>
48 #include <thread>
49 #include <typeinfo>
50 
51 #include "GoalVisitor.hpp"
52 
53 #define foreach BOOST_FOREACH
54 
55 namespace ompl
56 {
57  namespace magic
58  {
61  static const unsigned int MAX_RANDOM_BOUNCE_STEPS = 5;
62 
64  static const double ROADMAP_BUILD_TIME = 0.2;
65 
68  static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
69  } // namespace magic
70 } // namespace ompl
71 
72 ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy)
73  : base::Planner(si, "PRM")
74  , starStrategy_(starStrategy)
75  , stateProperty_(boost::get(vertex_state_t(), g_))
76  , totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_))
77  , successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_))
78  , weightProperty_(boost::get(boost::edge_weight, g_))
79  , disjointSets_(boost::get(boost::vertex_rank, g_), boost::get(boost::vertex_predecessor, g_))
80 {
83  specs_.optimizingPaths = true;
84  specs_.multithreaded = true;
85 
86  if (!starStrategy_)
87  Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors,
88  &PRM::getMaxNearestNeighbors, std::string("8:1000"));
89 
90  addPlannerProgressProperty("iterations INTEGER", [this] { return getIterationCount(); });
91  addPlannerProgressProperty("best cost REAL", [this] { return getBestCost(); });
92  addPlannerProgressProperty("milestone count INTEGER", [this] { return getMilestoneCountString(); });
93  addPlannerProgressProperty("edge count INTEGER", [this] { return getEdgeCountString(); });
94 }
95 
96 ompl::geometric::PRM::PRM(const base::PlannerData &data, bool starStrategy)
97  : PRM(data.getSpaceInformation(), starStrategy)
98 {
99  if (data.numVertices() > 0)
100  {
101  // mapping between vertex id from PlannerData and Vertex in Boost.Graph
102  std::map<unsigned int, Vertex> vertices;
103  // helper function to create vertices as needed and update the vertices mapping
104  const auto &getOrCreateVertex = [&](unsigned int vertex_index) {
105  if (!vertices.count(vertex_index))
106  {
107  const auto &data_vertex = data.getVertex(vertex_index);
108  Vertex graph_vertex = boost::add_vertex(g_);
109  stateProperty_[graph_vertex] = si_->cloneState(data_vertex.getState());
110  totalConnectionAttemptsProperty_[graph_vertex] = 1;
111  successfulConnectionAttemptsProperty_[graph_vertex] = 0;
112  vertices[vertex_index] = graph_vertex;
113  }
114  return vertices.at(vertex_index);
115  };
116 
117  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
118  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
119  specs_.multithreaded = true;
120  nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
121 
122  for (size_t vertex_index = 0; vertex_index < data.numVertices(); ++vertex_index)
123  {
124  Vertex m = getOrCreateVertex(vertex_index);
125  std::vector<unsigned int> neighbor_indices;
126  data.getEdges(vertex_index, neighbor_indices);
127  if (neighbor_indices.empty())
128  {
129  disjointSets_.make_set(m);
130  }
131  else
132  {
133  for (const unsigned int neighbor_index : neighbor_indices)
134  {
135  Vertex n = getOrCreateVertex(neighbor_index);
138  base::Cost weight;
139  data.getEdgeWeight(vertex_index, neighbor_index, &weight);
140  const Graph::edge_property_type properties(weight);
141  boost::add_edge(m, n, properties, g_);
142  uniteComponents(m, n);
143  }
144  }
145  nn_->add(m);
146  }
147  }
148 }
149 
150 ompl::geometric::PRM::~PRM()
151 {
152  freeMemory();
153 }
154 
156 {
157  Planner::setup();
158  if (!nn_)
159  {
160  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
161  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
162  specs_.multithreaded = true;
163  nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
164  }
165  if (!connectionStrategy_)
166  setDefaultConnectionStrategy();
167  if (!connectionFilter_)
168  connectionFilter_ = [](const Vertex &, const Vertex &) { return true; };
169 
170  // Setup optimization objective
171  //
172  // If no optimization objective was specified, then default to
173  // optimizing path length as computed by the distance() function
174  // in the state space.
175  if (pdef_)
176  {
177  if (pdef_->hasOptimizationObjective())
178  opt_ = pdef_->getOptimizationObjective();
179  else
180  {
181  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
182  if (!starStrategy_)
183  opt_->setCostThreshold(opt_->infiniteCost());
184  }
185  }
186  else
187  {
188  OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
189  setup_ = false;
190  }
191 }
192 
194 {
195  if (starStrategy_)
196  throw Exception("Cannot set the maximum nearest neighbors for " + getName());
197  if (!nn_)
198  {
199  specs_.multithreaded = false; // temporarily set to false since nn_ is used only in single thread
200  nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(this));
201  specs_.multithreaded = true;
202  nn_->setDistanceFunction([this](const Vertex a, const Vertex b) { return distanceFunction(a, b); });
203  }
204  if (!userSetConnectionStrategy_)
205  connectionStrategy_ = KStrategy<Vertex>(k, nn_);
206  if (isSetup())
207  setup();
208 }
209 
211 {
212  const auto strategy = connectionStrategy_.target<KStrategy<Vertex>>();
213  return strategy ? strategy->getNumNeighbors() : 0u;
214 }
215 
217 {
218  if (starStrategy_)
219  connectionStrategy_ = KStarStrategy<Vertex>([this] { return milestoneCount(); }, nn_, si_->getStateDimension());
220  else
221  connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
222 }
223 
224 void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
225 {
226  Planner::setProblemDefinition(pdef);
227  clearQuery();
228 }
229 
231 {
232  startM_.clear();
233  goalM_.clear();
234  pis_.restart();
235 }
236 
238 {
239  Planner::clear();
240  sampler_.reset();
241  simpleSampler_.reset();
242  freeMemory();
243  if (nn_)
244  nn_->clear();
245  clearQuery();
246 
247  iterations_ = 0;
248  bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
249 }
250 
252 {
253  foreach (Vertex v, boost::vertices(g_))
254  si_->freeState(stateProperty_[v]);
255  g_.clear();
256 }
257 
258 void ompl::geometric::PRM::expandRoadmap(double expandTime)
259 {
260  expandRoadmap(base::timedPlannerTerminationCondition(expandTime));
261 }
262 
264 {
265  if (!simpleSampler_)
266  simpleSampler_ = si_->allocStateSampler();
267 
268  std::vector<base::State *> states(magic::MAX_RANDOM_BOUNCE_STEPS);
269  si_->allocStates(states);
270  expandRoadmap(ptc, states);
271  si_->freeStates(states);
272 }
273 
275  std::vector<base::State *> &workStates)
276 {
277  // construct a probability distribution over the vertices in the roadmap
278  // as indicated in
279  // "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
280  // Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
281 
282  PDF<Vertex> pdf;
283  foreach (Vertex v, boost::vertices(g_))
284  {
285  const unsigned long int t = totalConnectionAttemptsProperty_[v];
286  pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
287  }
288 
289  if (pdf.empty())
290  return;
291 
292  while (!ptc)
293  {
294  iterations_++;
295  Vertex v = pdf.sample(rng_.uniform01());
296  unsigned int s =
297  si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
298  if (s > 0)
299  {
300  s--;
301  Vertex last = addMilestone(si_->cloneState(workStates[s]));
302 
303  graphMutex_.lock();
304  for (unsigned int i = 0; i < s; ++i)
305  {
306  // add the vertex along the bouncing motion
307  Vertex m = boost::add_vertex(g_);
308  stateProperty_[m] = si_->cloneState(workStates[i]);
309  totalConnectionAttemptsProperty_[m] = 1;
310  successfulConnectionAttemptsProperty_[m] = 0;
311  disjointSets_.make_set(m);
312 
313  // add the edge to the parent vertex
314  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
315  const Graph::edge_property_type properties(weight);
316  boost::add_edge(v, m, properties, g_);
317  uniteComponents(v, m);
318 
319  // add the vertex to the nearest neighbors data structure
320  nn_->add(m);
321  v = m;
322  }
323 
324  // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
325  // we add an edge
326  if (s > 0 || !sameComponent(v, last))
327  {
328  // add the edge to the parent vertex
329  const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
330  const Graph::edge_property_type properties(weight);
331  boost::add_edge(v, last, properties, g_);
332  uniteComponents(v, last);
333  }
334  graphMutex_.unlock();
335  }
336  }
337 }
338 
340 {
341  growRoadmap(base::timedPlannerTerminationCondition(growTime));
342 }
343 
345 {
346  if (!isSetup())
347  setup();
348  if (!sampler_)
349  sampler_ = si_->allocValidStateSampler();
350 
351  base::State *workState = si_->allocState();
352  growRoadmap(ptc, workState);
353  si_->freeState(workState);
354 }
355 
357 {
358  /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
359  while (!ptc)
360  {
361  iterations_++;
362  // search for a valid state
363  bool found = false;
364  while (!found && !ptc)
365  {
366  unsigned int attempts = 0;
367  do
368  {
369  found = sampler_->sample(workState);
370  attempts++;
371  } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
372  }
373  // add it as a milestone
374  if (found)
375  addMilestone(si_->cloneState(workState));
376  }
377 }
378 
380 {
381  auto *goal = static_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
382  while (!ptc && !addedNewSolution_)
383  {
384  // Check for any new goal states
385  if (goal->maxSampleCount() > goalM_.size())
386  {
387  const base::State *st = pis_.nextGoal();
388  if (st != nullptr)
389  goalM_.push_back(addMilestone(si_->cloneState(st)));
390  }
391 
392  // Check for a solution
393  addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution);
394  // Sleep for 1ms
395  if (!addedNewSolution_)
396  std::this_thread::sleep_for(std::chrono::milliseconds(1));
397  }
398 }
399 
400 bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
401  base::PathPtr &solution)
402 {
403  base::Goal *g = pdef_->getGoal().get();
404  base::Cost sol_cost(opt_->infiniteCost());
405  foreach (Vertex start, starts)
406  {
407  foreach (Vertex goal, goals)
408  {
409  // we lock because the connected components algorithm is incremental and may change disjointSets_
410  graphMutex_.lock();
411  bool same_component = sameComponent(start, goal);
412  graphMutex_.unlock();
413 
414  if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
415  {
416  base::PathPtr p = constructSolution(start, goal);
417  if (p)
418  {
419  base::Cost pathCost = p->cost(opt_);
420  if (opt_->isCostBetterThan(pathCost, bestCost_))
421  bestCost_ = pathCost;
422  // Check if optimization objective is satisfied
423  if (opt_->isSatisfied(pathCost))
424  {
425  solution = p;
426  return true;
427  }
428  if (opt_->isCostBetterThan(pathCost, sol_cost))
429  {
430  solution = p;
431  sol_cost = pathCost;
432  }
433  }
434  }
435  }
436  }
437 
438  return false;
439 }
440 
442 {
443  return addedNewSolution_;
444 }
445 
447 {
448  checkValidity();
449  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
450 
451  if (goal == nullptr)
452  {
453  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
455  }
456 
457  // Add the valid start states as milestones
458  while (const base::State *st = pis_.nextStart())
459  startM_.push_back(addMilestone(si_->cloneState(st)));
460 
461  if (startM_.empty())
462  {
463  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
465  }
466 
467  if (!goal->couldSample())
468  {
469  OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
471  }
472 
473  // Ensure there is at least one valid goal state
474  if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
475  {
476  const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
477  if (st != nullptr)
478  goalM_.push_back(addMilestone(si_->cloneState(st)));
479 
480  if (goalM_.empty())
481  {
482  OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
484  }
485  }
486 
487  unsigned long int nrStartStates = boost::num_vertices(g_);
488  OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
489 
490  // Reset addedNewSolution_ member and create solution checking thread
491  addedNewSolution_ = false;
492  base::PathPtr sol;
493  std::thread slnThread([this, &ptc, &sol] { checkForSolution(ptc, sol); });
494 
495  // construct new planner termination condition that fires when the given ptc is true, or a solution is found
496  base::PlannerTerminationCondition ptcOrSolutionFound([this, &ptc] { return ptc || addedNewSolution(); });
497 
498  constructRoadmap(ptcOrSolutionFound);
499 
500  // Ensure slnThread is ceased before exiting solve
501  slnThread.join();
502 
503  OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
504 
505  if (sol)
506  {
507  base::PlannerSolution psol(sol);
508  psol.setPlannerName(getName());
509  // if the solution was optimized, we mark it as such
510  psol.setOptimized(opt_, bestCost_, addedNewSolution());
511  pdef_->addSolutionPath(psol);
512  }
513  else
514  {
515  // Return an approximate solution.
516  ompl::base::Cost diff = constructApproximateSolution(startM_, goalM_, sol);
517  if (!opt_->isFinite(diff))
518  {
519  OMPL_INFORM("Closest path is still start and goal");
521  }
522  OMPL_INFORM("Using approximate solution, heuristic cost-to-go is %f", diff.value());
523  pdef_->addSolutionPath(sol, true, diff.value(), getName());
525  }
526 
528 }
529 
531 {
532  if (!isSetup())
533  setup();
534  if (!sampler_)
535  sampler_ = si_->allocValidStateSampler();
536  if (!simpleSampler_)
537  simpleSampler_ = si_->allocStateSampler();
538 
539  std::vector<base::State *> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
540  si_->allocStates(xstates);
541  bool grow = true;
542 
543  bestCost_ = opt_->infiniteCost();
544  while (!ptc())
545  {
546  // maintain a 2:1 ratio for growing/expansion of roadmap
547  // call growRoadmap() twice as long for every call of expandRoadmap()
548  if (grow)
551  xstates[0]);
552  else
555  xstates);
556  grow = !grow;
557  }
558 
559  si_->freeStates(xstates);
560 }
561 
563 {
564  std::lock_guard<std::mutex> _(graphMutex_);
565 
566  Vertex m = boost::add_vertex(g_);
567  stateProperty_[m] = state;
568  totalConnectionAttemptsProperty_[m] = 1;
569  successfulConnectionAttemptsProperty_[m] = 0;
570 
571  // Initialize to its own (dis)connected component.
572  disjointSets_.make_set(m);
573 
574  // Which milestones will we attempt to connect to?
575  const std::vector<Vertex> &neighbors = connectionStrategy_(m);
576 
577  foreach (Vertex n, neighbors)
578  if (connectionFilter_(n, m))
579  {
580  totalConnectionAttemptsProperty_[m]++;
581  totalConnectionAttemptsProperty_[n]++;
582  if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
583  {
584  successfulConnectionAttemptsProperty_[m]++;
585  successfulConnectionAttemptsProperty_[n]++;
586  const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
587  const Graph::edge_property_type properties(weight);
588  boost::add_edge(n, m, properties, g_);
589  uniteComponents(n, m);
590  }
591  }
592 
593  nn_->add(m);
594 
595  return m;
596 }
597 
599 {
600  disjointSets_.union_set(m1, m2);
601 }
602 
604 {
605  return boost::same_component(m1, m2, disjointSets_);
606 }
607 
609  const std::vector<Vertex> &goals,
610  base::PathPtr &solution)
611 {
612  std::lock_guard<std::mutex> _(graphMutex_);
613  base::Goal *g = pdef_->getGoal().get();
614  base::Cost closestVal(opt_->infiniteCost());
615  bool approxPathJustStart = true;
616 
617  foreach (Vertex start, starts)
618  {
619  foreach (Vertex goal, goals)
620  {
621  base::Cost heuristicCost(costHeuristic(start, goal));
622  if (opt_->isCostBetterThan(heuristicCost, closestVal))
623  {
624  closestVal = heuristicCost;
625  approxPathJustStart = true;
626  }
627  if (!g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
628  {
629  continue;
630  }
631  base::PathPtr p;
632  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
633  boost::vector_property_map<base::Cost> dist(boost::num_vertices(g_));
634  boost::vector_property_map<base::Cost> rank(boost::num_vertices(g_));
635 
636  try
637  {
638  // Consider using a persistent distance_map if it's slow
639  boost::astar_search(
640  g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
641  boost::predecessor_map(prev)
642  .distance_map(dist)
643  .rank_map(rank)
644  .distance_compare(
645  [this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
646  .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
647  .distance_inf(opt_->infiniteCost())
648  .distance_zero(opt_->identityCost())
649  .visitor(AStarGoalVisitor<Vertex>(goal)));
650  }
651  catch (AStarFoundGoal &)
652  {
653  }
654 
655  Vertex closeToGoal = start;
656  for (auto vp = vertices(g_); vp.first != vp.second; vp.first++)
657  {
658  // We want to get the distance of each vertex to the goal.
659  // Boost lets us get cost-to-come, cost-to-come+dist-to-goal,
660  // but not just dist-to-goal.
661  ompl::base::Cost dist_to_goal(costHeuristic(*vp.first, goal));
662  if (opt_->isFinite(rank[*vp.first]) && opt_->isCostBetterThan(dist_to_goal, closestVal))
663  {
664  closeToGoal = *vp.first;
665  closestVal = dist_to_goal;
666  approxPathJustStart = false;
667  }
668  }
669  if (closeToGoal != start)
670  {
671  auto p(std::make_shared<PathGeometric>(si_));
672  for (Vertex pos = closeToGoal; prev[pos] != pos; pos = prev[pos])
673  p->append(stateProperty_[pos]);
674  p->append(stateProperty_[start]);
675  p->reverse();
676 
677  solution = p;
678  }
679  }
680  }
681  if (approxPathJustStart)
682  {
683  return opt_->infiniteCost();
684  }
685  return closestVal;
686 }
687 
689 {
690  std::lock_guard<std::mutex> _(graphMutex_);
691  boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
692 
693  try
694  {
695  // Consider using a persistent distance_map if it's slow
696  boost::astar_search(
697  g_, start, [this, goal](Vertex v) { return costHeuristic(v, goal); },
698  boost::predecessor_map(prev)
699  .distance_compare([this](base::Cost c1, base::Cost c2) { return opt_->isCostBetterThan(c1, c2); })
700  .distance_combine([this](base::Cost c1, base::Cost c2) { return opt_->combineCosts(c1, c2); })
701  .distance_inf(opt_->infiniteCost())
702  .distance_zero(opt_->identityCost())
703  .visitor(AStarGoalVisitor<Vertex>(goal)));
704  }
705  catch (AStarFoundGoal &)
706  {
707  }
708 
709  if (prev[goal] == goal)
710  throw Exception(name_, "Could not find solution path");
711 
712  auto p(std::make_shared<PathGeometric>(si_));
713  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
714  p->append(stateProperty_[pos]);
715  p->append(stateProperty_[start]);
716  p->reverse();
717 
718  return p;
719 }
720 
722 {
723  Planner::getPlannerData(data);
724 
725  // Explicitly add start and goal states:
726  for (unsigned long i : startM_)
727  data.addStartVertex(
728  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
729 
730  for (unsigned long i : goalM_)
731  data.addGoalVertex(
732  base::PlannerDataVertex(stateProperty_[i], const_cast<PRM *>(this)->disjointSets_.find_set(i)));
733 
734  // Adding edges and all other vertices simultaneously
735  foreach (const Edge e, boost::edges(g_))
736  {
737  const Vertex v1 = boost::source(e, g_);
738  const Vertex v2 = boost::target(e, g_);
739  data.addEdge(base::PlannerDataVertex(stateProperty_[v1]), base::PlannerDataVertex(stateProperty_[v2]));
740 
741  // Add the reverse edge, since we're constructing an undirected roadmap
742  data.addEdge(base::PlannerDataVertex(stateProperty_[v2]), base::PlannerDataVertex(stateProperty_[v1]));
743 
744  // Add tags for the newly added vertices
745  data.tagState(stateProperty_[v1], const_cast<PRM *>(this)->disjointSets_.find_set(v1));
746  data.tagState(stateProperty_[v2], const_cast<PRM *>(this)->disjointSets_.find_set(v2));
747  }
748 }
749 
751 {
752  return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
753 }
ompl::base::Cost constructApproximateSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
(Assuming that there is always an approximate solution), finds an approximate solution.
Definition: PRM.cpp:608
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:132
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element,...
Definition: PDF.h:97
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
bool getEdgeWeight(unsigned int v1, unsigned int v2, Cost *weight) const
Returns the weight of the edge between the given vertex indices. If there exists an edge between v1 a...
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
A shared pointer wrapper for ompl::base::Path.
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: PRM.h:391
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:199
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
Definition: PlannerStatus.h:64
unsigned int getEdges(unsigned int v, std::vector< unsigned int > &edgeList) const
Returns a list of the vertex indexes directly connected to vertex with index v (outgoing edges)....
void growRoadmap(double growTime)
If the user desires, the roadmap can be improved for the given time (seconds). The solve() method wil...
Definition: PRM.cpp:339
void setDefaultConnectionStrategy()
Definition: PRM.cpp:216
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition: PRM.h:345
Definition of an abstract state.
Definition: State.h:50
base::PathPtr constructSolution(const Vertex &start, const Vertex &goal)
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition: PRM.cpp:688
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:410
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:379
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: PRM.cpp:721
boost::disjoint_sets< boost::property_map< Graph, boost::vertex_rank_t >::type, boost::property_map< Graph, boost::vertex_predecessor_t >::type > disjointSets_
Data structure that maintains the connected components.
Definition: PRM.h:405
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition: PRM.cpp:193
double value() const
The value of the cost.
Definition: Cost.h:56
Representation of a solution to a planning problem.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
bool tagState(const State *st, int tag)
Set the integer tag associated with the given state. If the given state does not exist in a vertex,...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:49
Make the minimal number of connections required to ensure asymptotic optimality.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PRM.cpp:155
bool empty() const
Returns whether the PDF contains no data.
Definition: PDF.h:263
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition: PRM.cpp:258
unsigned int getMaxNearestNeighbors() const
return the maximum number of nearest neighbors to connect a sample to
Definition: PRM.cpp:210
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition: PRM.cpp:562
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. Grows a roadmap using constructRoadmap()....
Definition: PRM.cpp:446
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:206
bool maybeConstructSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition: PRM.cpp:400
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition: PRM.h:127
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:441
static const unsigned int DEFAULT_NEAREST_NEIGHBORS
The number of nearest neighbors to consider by default in the construction of the PRM roadmap.
Definition: PRM.cpp:68
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:530
static const unsigned int MAX_RANDOM_BOUNCE_STEPS
The number of steps to take for a random bounce motion generated as part of the expansion step of PRM...
Definition: PRM.cpp:61
void freeMemory()
Free all the memory allocated by the planner.
Definition: PRM.cpp:251
Abstract definition of goals.
Definition: Goal.h:63
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
boost::property_map< Graph, vertex_successful_connection_attempts_t >::type successfulConnectionAttemptsProperty_
Access to the number of successful connection attempts for a vertex.
Definition: PRM.h:398
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true,...
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: PRM.h:370
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:72
static const double ROADMAP_BUILD_TIME
The time in seconds for a single roadmap building operation (dt)
Definition: PRM.cpp:64
void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
Set the optimization objective used to optimize this solution, the cost of the solution and whether i...
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...
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: PRM.h:379
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
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...
Definition: Goal.h:136
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: PRM.cpp:230
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: PRM.h:125
boost::property_map< Graph, vertex_total_connection_attempts_t >::type totalConnectionAttemptsProperty_
Access to the number of total connection attempts for a vertex.
Definition: PRM.h:394
void uniteComponents(Vertex m1, Vertex m2)
Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...
Definition: PRM.cpp:598
base::Cost costHeuristic(Vertex u, Vertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition: PRM.cpp:750
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PRM.cpp:237
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...
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...
static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK
Maximum number of sampling attempts to find a valid state, without checking whether the allowed time ...
Graph g_
Connectivity graph.
Definition: PRM.h:382
Probabilistic RoadMap planner.
Definition: PRM.h:81
Abstract definition of a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:47
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
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...
Definition: PRM.cpp:603
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:196
Main namespace. Contains everything in this library.
Definition: AppBase.h:22