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 }
The exception type for ompl.
Definition: Exception.h:47
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:49
bool empty() const
Returns whether the PDF contains no data.
Definition: PDF.h:263
_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
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
double value() const
The value of the cost.
Definition: Cost.h:56
Abstract definition of a goal region that can be sampled.
Abstract definition of goals.
Definition: Goal.h:63
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
A shared pointer wrapper for ompl::base::Path.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
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,...
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...
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...
unsigned int numVertices() const
Retrieve the number of vertices in this structure.
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)....
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
Definition of an abstract state.
Definition: State.h:50
Make the minimal number of connections required to ensure asymptotic optimality.
Probabilistic RoadMap planner.
Definition: PRM.h:81
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
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: PRM.h:370
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
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
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:441
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 clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: PRM.cpp:230
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
void freeMemory()
Free all the memory allocated by the planner.
Definition: PRM.cpp:251
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: PRM.h:379
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition: PRM.h:127
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: PRM.h:125
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:72
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:379
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition: PRM.cpp:258
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
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
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 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
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:530
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
Graph g_
Connectivity graph.
Definition: PRM.h:382
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
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
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PRM.cpp:155
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PRM.cpp:237
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition: PRM.cpp:193
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
unsigned int getMaxNearestNeighbors() const
return the maximum number of nearest neighbors to connect a sample to
Definition: PRM.cpp:210
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 setDefaultConnectionStrategy()
Definition: PRM.cpp:216
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: PRM.h:391
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
PlannerTerminationCondition plannerOrTerminationCondition(const PlannerTerminationCondition &c1, const PlannerTerminationCondition &c2)
Combine two termination conditions into one. If either termination condition returns true,...
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
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 ...
static const double ROADMAP_BUILD_TIME
The time in seconds for a single roadmap building operation (dt)
Definition: PRM.cpp:64
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
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
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
Representation of a solution to a planning problem.
void setPlannerName(const std::string &name)
Set the name of the planner used to compute this solution.
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...
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:199
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:206
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:196
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ INVALID_START
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:66
@ INVALID_GOAL
Invalid goal state.
Definition: PlannerStatus.h:58
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
Definition: PlannerStatus.h:64
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62