PRM.h
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 the 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 */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
38 #define OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
39 
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 #include <boost/graph/graph_traits.hpp>
43 #include <boost/graph/adjacency_list.hpp>
44 #include <boost/pending/disjoint_sets.hpp>
45 #include <mutex>
46 #include <utility>
47 #include <vector>
48 #include <map>
49 
50 namespace ompl
51 {
52  namespace base
53  {
54  // Forward declare for use in implementation
55  OMPL_CLASS_FORWARD(OptimizationObjective);
56  }
57 
58  namespace geometric
59  {
80  class PRM : public base::Planner
81  {
82  public:
84  {
85  typedef boost::vertex_property_tag kind;
86  };
87 
89  {
90  typedef boost::vertex_property_tag kind;
91  };
92 
94  {
95  typedef boost::vertex_property_tag kind;
96  };
97 
113  typedef boost::adjacency_list<
114  boost::vecS, boost::vecS, boost::undirectedS,
115  boost::property<
117  boost::property<
118  vertex_total_connection_attempts_t, unsigned long int,
119  boost::property<vertex_successful_connection_attempts_t, unsigned long int,
120  boost::property<boost::vertex_predecessor_t, unsigned long int,
121  boost::property<boost::vertex_rank_t, unsigned long int>>>>>,
122  boost::property<boost::edge_weight_t, base::Cost>>
124 
126  typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
128  typedef boost::graph_traits<Graph>::edge_descriptor Edge;
129 
131  typedef std::shared_ptr<NearestNeighbors<Vertex>> RoadmapNeighbors;
132 
135  typedef std::function<const std::vector<Vertex> &(const Vertex)> ConnectionStrategy;
136 
142  typedef std::function<bool(const Vertex &, const Vertex &)> ConnectionFilter;
143 
145  PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
146 
147  ~PRM() override;
148 
149  void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override;
150 
164  void setConnectionStrategy(const ConnectionStrategy &connectionStrategy)
165  {
166  connectionStrategy_ = connectionStrategy;
168  }
172  void setMaxNearestNeighbors(unsigned int k);
173 
187  void setConnectionFilter(const ConnectionFilter &connectionFilter)
188  {
189  connectionFilter_ = connectionFilter;
190  }
191 
192  void getPlannerData(base::PlannerData &data) const override;
193 
198 
202  void growRoadmap(double growTime);
203 
208 
212  void expandRoadmap(double expandTime);
213 
218 
233 
238  void clearQuery();
239 
240  void clear() override;
241 
243  template <template <typename T> class NN>
245  {
246  if (nn_ && nn_->size() == 0)
247  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
248  clear();
249  nn_ = std::make_shared<NN<Vertex>>();
252  if (isSetup())
253  setup();
254  }
255 
256  void setup() override;
257 
258  const Graph &getRoadmap() const
259  {
260  return g_;
261  }
262 
264  unsigned long int milestoneCount() const
265  {
266  return boost::num_vertices(g_);
267  }
268 
270  unsigned long int edgeCount() const
271  {
272  return boost::num_edges(g_);
273  }
274 
275  const RoadmapNeighbors &getNearestNeighbors()
276  {
277  return nn_;
278  }
279 
280  protected:
282  void freeMemory();
283 
287  Vertex addMilestone(base::State *state);
288 
291  void uniteComponents(Vertex m1, Vertex m2);
292 
295  bool sameComponent(Vertex m1, Vertex m2);
296 
300  void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState);
301 
305  void expandRoadmap(const base::PlannerTerminationCondition &ptc, std::vector<base::State *> &workStates);
306 
309 
313  bool maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
314  base::PathPtr &solution);
315 
318  ompl::base::Cost constructApproximateSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution);
319 
321  bool addedNewSolution() const;
322 
325  base::PathPtr constructSolution(const Vertex &start, const Vertex &goal);
326 
329  base::Cost costHeuristic(Vertex u, Vertex v) const;
330 
333  double distanceFunction(const Vertex a, const Vertex b) const
334  {
335  return si_->distance(stateProperty_[a], stateProperty_[b]);
336  }
337 
339  // Planner progress property functions
340  std::string getIterationCount() const
341  {
342  return std::to_string(iterations_);
343  }
344  std::string getBestCost() const
345  {
346  return std::to_string(bestCost_.value());
347  }
348  std::string getMilestoneCountString() const
349  {
350  return std::to_string(milestoneCount());
351  }
352  std::string getEdgeCountString() const
353  {
354  return std::to_string(edgeCount());
355  }
356 
359 
362 
365 
368 
371 
373  std::vector<Vertex> startM_;
374 
376  std::vector<Vertex> goalM_;
377 
379  boost::property_map<Graph, vertex_state_t>::type stateProperty_;
380 
382  boost::property_map<Graph, vertex_total_connection_attempts_t>::type totalConnectionAttemptsProperty_;
383 
385  boost::property_map<Graph, vertex_successful_connection_attempts_t>::type
387 
389  boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
390 
392  boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type,
393  boost::property_map<Graph, boost::vertex_predecessor_t>::type> disjointSets_;
394 
397 
400 
404 
407 
409  bool addedNewSolution_{false};
410 
412  mutable std::mutex graphMutex_;
413 
416 
418  // Planner progress properties
420  unsigned long int iterations_{0};
422  base::Cost bestCost_{std::numeric_limits<double>::quiet_NaN()};
423  };
424  }
425 }
426 
427 #endif
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition: PRM.cpp:71
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
Definition: PRM.h:264
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:333
unsigned long int iterations_
Number of iterations the algorithm performed.
Definition: PRM.h:420
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: PRM.h:376
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: PRM.cpp:192
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition: PRM.cpp:341
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: PRM.h:361
A shared pointer wrapper for ompl::base::ProblemDefinition.
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:393
A shared pointer wrapper for ompl::base::ValidStateSampler.
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:725
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. Grows a roadmap using constructRoadmap(). This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work for more time on an unsolved problem, for example. Start and goal states from the currently specified ProblemDefinition are cached. This means that between calls to solve(), input states are only added, not removed. When using PRM as a multi-query planner, the input states should be however cleared, without clearing the roadmap itself. This can be done using the clearQuery() function.
Definition: PRM.cpp:408
std::mutex graphMutex_
Mutex to guard access to the Graph member (g_)
Definition: PRM.h:412
A shared pointer wrapper for ompl::base::StateSampler.
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition: PRM.cpp:220
ConnectionStrategy connectionStrategy_
Function that returns the milestones to attempt connections with.
Definition: PRM.h:396
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:571
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:576
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: PRM.cpp:112
void setConnectionFilter(const ConnectionFilter &connectionFilter)
Set the function that can reject a milestone connection.
Definition: PRM.h:187
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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:301
bool addedNewSolution_
A flag indicating that a solution has been added during solve()
Definition: PRM.h:409
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: PRM.h:244
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
Definition: PRM.h:389
std::shared_ptr< NearestNeighbors< Vertex > > RoadmapNeighbors
A nearest neighbors data structure for roadmap vertices.
Definition: PRM.h:131
std::function< bool(const Vertex &, const Vertex &)> ConnectionFilter
A function that can reject connections.
Definition: PRM.h:142
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::property< vertex_state_t, base::State *, boost::property< vertex_total_connection_attempts_t, unsigned long int, boost::property< vertex_successful_connection_attempts_t, unsigned long int, boost::property< boost::vertex_predecessor_t, unsigned long int, boost::property< boost::vertex_rank_t, unsigned long int > > > > >, boost::property< boost::edge_weight_t, base::Cost > > Graph
The underlying roadmap graph.
Definition: PRM.h:123
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition: PRM.cpp:403
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:696
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:386
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition: PRM.h:128
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
std::vector< Vertex > startM_
Array of start milestones.
Definition: PRM.h:373
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
void freeMemory()
Free all the memory allocated by the planner.
Definition: PRM.cpp:213
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition: PRM.h:358
Base class for a planner.
Definition: Planner.h:223
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:530
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:654
std::function< const std::vector< Vertex > &(const Vertex)> ConnectionStrategy
A function returning the milestones that should be attempted to connect to.
Definition: PRM.h:135
base::StateSamplerPtr simpleSampler_
Sampler user for generating random in the state space.
Definition: PRM.h:364
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
void setConnectionStrategy(const ConnectionStrategy &connectionStrategy)
Set the connection strategy function that specifies the milestones that connection attempts will be m...
Definition: PRM.h:164
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: PRM.h:379
bool userSetConnectionStrategy_
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
Definition: PRM.h:403
Graph g_
Connectivity graph.
Definition: PRM.h:370
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: PRM.cpp:186
A shared pointer wrapper for ompl::base::OptimizationObjective.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: PRM.cpp:199
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:566
Probabilistic RoadMap planner.
Definition: PRM.h:80
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:108
ConnectionFilter connectionFilter_
Function that can reject a milestone connection.
Definition: PRM.h:399
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors...
Definition: PRM.cpp:166
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition: PRM.h:415
double value() const
The value of the cost.
Definition: Cost.h:56
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:362
base::Cost bestCost_
Best cost found so far by algorithm.
Definition: PRM.h:422
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition: PRM.h:126
unsigned long int edgeCount() const
Return the number of edges currently in the graph.
Definition: PRM.h:270
RNG rng_
Random number generator.
Definition: PRM.h:406
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition: PRM.cpp:498
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition: PRM.h:367
A shared pointer wrapper for ompl::base::Path.
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:382