SPARSdb.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rutgers the State University of New Jersey, New Brunswick
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 Rutgers 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: Andrew Dobson, Dave Coleman */
36 
37 #ifndef OMPL_TOOLS_THUNDER_SPARS_DB_
38 #define OMPL_TOOLS_THUNDER_SPARS_DB_
39 
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 #include "ompl/geometric/PathSimplifier.h"
43 #include "ompl/util/Time.h"
44 #include "ompl/util/Hash.h"
45 
46 #include <boost/range/adaptor/map.hpp>
47 #include <unordered_map>
48 #include <boost/graph/graph_traits.hpp>
49 #include <boost/graph/adjacency_list.hpp>
50 #include <boost/graph/filtered_graph.hpp>
51 #include <boost/graph/graph_utility.hpp>
52 #include <boost/graph/astar_search.hpp>
53 #include <boost/graph/connected_components.hpp>
54 #include <boost/property_map/property_map.hpp>
55 #include <boost/pending/disjoint_sets.hpp>
56 #include <thread>
57 #include <iostream>
58 #include <fstream>
59 #include <utility>
60 #include <vector>
61 #include <map>
62 
63 namespace ompl
64 {
65  namespace geometric
66  {
85  class SPARSdb : public base::Planner
86  {
87  public:
89  enum GuardType
90  {
91  START,
92  GOAL,
93  COVERAGE,
94  CONNECTIVITY,
95  INTERFACE,
96  QUALITY,
97  };
98 
100  // BOOST GRAPH DETAILS
102 
104  using VertexIndexType = unsigned long int;
105 
107  using VertexPair = std::pair<VertexIndexType, VertexIndexType>;
108 
110 
112  {
115  base::State *pointB_;
116 
119  base::State *sigmaB_;
120 
122  double d_;
123 
126  : pointA_(nullptr)
127  , pointB_(nullptr)
128  , sigmaA_(nullptr)
129  , sigmaB_(nullptr)
130  , d_(std::numeric_limits<double>::infinity())
131  {
132  }
133 
136  {
137  if (pointA_)
138  {
139  si->freeState(pointA_);
140  pointA_ = nullptr;
141  }
142  if (pointB_)
143  {
144  si->freeState(pointB_);
145  pointB_ = nullptr;
146  }
147  if (sigmaA_)
148  {
149  si->freeState(sigmaA_);
150  sigmaA_ = nullptr;
151  }
152  if (sigmaB_)
153  {
154  si->freeState(sigmaB_);
155  sigmaB_ = nullptr;
156  }
157  d_ = std::numeric_limits<double>::infinity();
158  }
159 
161  void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
162  {
163  if (pointA_)
164  si->copyState(pointA_, p);
165  else
166  pointA_ = si->cloneState(p);
167  if (sigmaA_)
168  si->copyState(sigmaA_, s);
169  else
170  sigmaA_ = si->cloneState(s);
171  if (pointB_)
172  d_ = si->distance(pointA_, pointB_);
173  }
174 
176  void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
177  {
178  if (pointB_)
179  si->copyState(pointB_, p);
180  else
181  pointB_ = si->cloneState(p);
182  if (sigmaB_)
183  si->copyState(sigmaB_, s);
184  else
185  sigmaB_ = si->cloneState(s);
186  if (pointA_)
187  d_ = si->distance(pointA_, pointB_);
188  }
189  };
190 
192  using InterfaceHash = std::unordered_map<VertexPair, InterfaceData>;
193 
195  // The InterfaceHash structure is wrapped inside of this struct due to a compilation error on
196  // GCC 4.6 with Boost 1.48. An implicit assignment operator overload does not compile with these
197  // components, so an explicit overload is given here.
198  // Remove this struct when the minimum Boost requirement is > v1.48.
200  {
201  InterfaceHashStruct &operator=(const InterfaceHashStruct &rhs) = default;
202  InterfaceHash interfaceHash;
203  };
204 
206  // Vertex properties
207 
209  {
210  using kind = boost::vertex_property_tag;
211  };
212 
214  {
215  using kind = boost::vertex_property_tag;
216  };
217 
219  {
220  using kind = boost::vertex_property_tag;
221  };
222 
224  // Edge properties
225 
227  {
228  using kind = boost::edge_property_tag;
229  };
230 
233  {
234  NOT_CHECKED,
235  IN_COLLISION,
236  FREE
237  };
238 
241  {
242  bool isApproximate_;
243  base::PathPtr path_;
244  // Edge 0 is edge from vertex 0 to vertex 1. Thus, there is n-1 edges for n vertices
245  std::vector<EdgeCollisionState> edgeCollisionStatus_;
246  // TODO save the collision state of the vertexes also?
247 
248  std::size_t getStateCount()
249  {
250  return static_cast<ompl::geometric::PathGeometric &>(*path_).getStateCount();
251  }
252 
253  ompl::geometric::PathGeometric &getGeometricPath()
254  {
255  return static_cast<ompl::geometric::PathGeometric &>(*path_);
256  }
257  };
258 
260 
284  using VertexProperties = boost::property<
286  boost::property<
287  boost::vertex_predecessor_t, VertexIndexType,
288  boost::property<boost::vertex_rank_t, VertexIndexType,
289  boost::property<vertex_color_t, GuardType,
290  boost::property<vertex_interface_data_t, InterfaceHashStruct>>>>>;
291 
293  using EdgeProperties =
294  boost::property<boost::edge_weight_t, double, boost::property<edge_collision_state_t, int>>;
295 
297  using Graph = boost::adjacency_list<boost::vecS, // store in std::vector
298  boost::vecS, // store in std::vector
299  boost::undirectedS, VertexProperties, EdgeProperties>;
300 
302  using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
303 
305  using Edge = boost::graph_traits<Graph>::edge_descriptor;
306 
308  // Typedefs for property maps
309 
311  using EdgeCollisionStateMap = boost::property_map<Graph, edge_collision_state_t>::type;
312 
314 
319  {
320  private:
321  const Graph &g_; // Graph used
322  const EdgeCollisionStateMap &collisionStates_;
323 
324  public:
326  using key_type = Edge;
328  using value_type = double;
330  using reference = double &;
332  using category = boost::readable_property_map_tag;
333 
338  edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates);
339 
345  double get(Edge e) const;
346  };
347 
349 
353  {
354  };
355 
357 
361  class CustomVisitor : public boost::default_astar_visitor
362  {
363  private:
364  Vertex goal; // Goal Vertex of the search
365 
366  public:
371  CustomVisitor(Vertex goal);
372 
379  void examine_vertex(Vertex u, const Graph &g) const;
380  };
381 
383  // SPARS MEMBER FUNCTIONS
385 
388 
390  ~SPARSdb() override;
391 
392  void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override;
393 
395  void setStretchFactor(double t)
396  {
397  stretchFactor_ = t;
398  }
399 
401  void setSparseDeltaFraction(double D)
402  {
404  if (sparseDelta_ > 0.0) // setup was previously called
405  sparseDelta_ = D * si_->getMaximumExtent();
406  }
407 
409  void setDenseDeltaFraction(double d)
410  {
412  if (denseDelta_ > 0.0) // setup was previously called
413  denseDelta_ = d * si_->getMaximumExtent();
414  }
415 
417  void setMaxFailures(unsigned int m)
418  {
419  maxFailures_ = m;
420  }
421 
423  unsigned int getMaxFailures() const
424  {
425  return maxFailures_;
426  }
427 
429  double getDenseDeltaFraction() const
430  {
431  return denseDeltaFraction_;
432  }
433 
435  double getSparseDeltaFraction() const
436  {
437  return sparseDeltaFraction_;
438  }
439 
441  double getStretchFactor() const
442  {
443  return stretchFactor_;
444  }
445 
446  bool getGuardSpacingFactor(const double pathLength, double &numGuards, double &spacingFactor);
447 
455  bool getGuardSpacingFactor(const double pathLength, int &numGuards, double &spacingFactor);
456 
457  bool addPathToRoadmap(const base::PlannerTerminationCondition &ptc,
458  ompl::geometric::PathGeometric &solutionPath);
459 
460  bool checkStartGoalConnection(ompl::geometric::PathGeometric &solutionPath);
461 
462  bool addStateToRoadmap(const base::PlannerTerminationCondition &ptc, base::State *newState);
463 
477 
482  void clearQuery();
483 
484  void clear() override;
485 
487  template <template <typename T> class NN>
489  {
490  if (nn_ && nn_->size() != 0)
491  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
492  clear();
493  nn_ = std::make_shared<NN<Vertex>>();
494  if (isSetup())
495  setup();
496  }
497 
506  bool getSimilarPaths(int nearestK, const base::State *start, const base::State *goal,
507  CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc);
508 
509  void setup() override;
510 
512  const Graph &getRoadmap() const
513  {
514  return g_;
515  }
516 
518  unsigned int getNumVertices() const
519  {
520  return boost::num_vertices(g_);
521  }
522 
524  unsigned int getNumEdges() const
525  {
526  return boost::num_edges(g_);
527  }
528 
530  unsigned int getNumConnectedComponents() const
531  {
532  // Make sure graph is populated
533  if (!getNumVertices())
534  return 0;
535 
536  std::vector<int> components(boost::num_vertices(g_));
537 
538  // it always overcounts by 1, i think because it is missing vertex 0 which is the new state insertion
539  // component
540  return boost::connected_components(g_, &components[0]) - 1;
541  }
542 
545  unsigned int getNumPathInsertionFailed() const
546  {
548  }
549 
551  unsigned int getNumConsecutiveFailures() const
552  {
553  return consecutiveFailures_;
554  }
555 
557  long unsigned int getIterations() const
558  {
559  return iterations_;
560  }
561 
571  bool convertVertexPathToStatePath(std::vector<Vertex> &vertexPath, const base::State *actualStart,
572  const base::State *actualGoal, CandidateSolution &candidateSolution,
573  bool disableCollisionWarning = false);
574 
575  void getPlannerData(base::PlannerData &data) const override;
576 
581  void setPlannerData(const base::PlannerData &data);
582 
584  bool reachedFailureLimit() const;
585 
587  void printDebug(std::ostream &out = std::cout) const;
588 
591 
592  protected:
594  void freeMemory();
595 
598 
600  bool checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood);
601 
603  bool checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood);
604 
607  bool checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
608  std::vector<Vertex> &visibleNeighborhood);
609 
611  bool checkAddPath(Vertex v);
612 
614  void resetFailures();
615 
617  void findGraphNeighbors(base::State *state, std::vector<Vertex> &graphNeighborhood,
618  std::vector<Vertex> &visibleNeighborhood);
619 
626  bool findGraphNeighbors(const base::State *state, std::vector<Vertex> &graphNeighborhood);
627 
629  void approachGraph(Vertex v);
630 
632  Vertex findGraphRepresentative(base::State *st);
633 
635  void findCloseRepresentatives(base::State *workState, const base::State *qNew, Vertex qRep,
636  std::map<Vertex, base::State *> &closeRepresentatives,
638 
640  void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s);
641 
643  void computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs);
644 
646  void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs);
647 
649  VertexPair index(Vertex vp, Vertex vpp);
650 
653 
655  void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp);
656 
659  void abandonLists(base::State *st);
660 
663  Vertex addGuard(base::State *state, GuardType type);
664 
666  void connectGuards(Vertex v, Vertex vp);
667 
671  bool getPaths(const std::vector<Vertex> &candidateStarts, const std::vector<Vertex> &candidateGoals,
672  const base::State *actualStart, const base::State *actualGoal,
673  CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc);
674 
679  bool lazyCollisionSearch(const Vertex &start, const Vertex &goal, const base::State *actualStart,
680  const base::State *actualGoal, CandidateSolution &candidateSolution,
682 
684  bool lazyCollisionCheck(std::vector<Vertex> &vertexPath, const base::PlannerTerminationCondition &ptc);
685 
688 
696  bool constructSolution(const Vertex start, const Vertex goal, std::vector<Vertex> &vertexPath) const;
697 
700  bool sameComponent(Vertex m1, Vertex m2);
701 
704  double distanceFunction(const Vertex a, const Vertex b) const
705  {
706  return si_->distance(stateProperty_[a], stateProperty_[b]);
707  }
708 
711 
713  std::shared_ptr<NearestNeighbors<Vertex>> nn_;
714 
717 
719  std::vector<Vertex> startM_;
720 
722  std::vector<Vertex> goalM_;
723 
726 
729 
732 
736 
738  unsigned int maxFailures_;
739 
742 
744  unsigned int nearSamplePoints_;
745 
748 
750  boost::property_map<Graph, boost::edge_weight_t>::type edgeWeightProperty_; // TODO: this is not used
751 
754 
756  boost::property_map<Graph, vertex_state_t>::type stateProperty_;
757 
759  boost::property_map<Graph, vertex_color_t>::type colorProperty_;
760 
762  boost::property_map<Graph, vertex_interface_data_t>::type interfaceDataProperty_;
763 
765  boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type,
766  boost::property_map<Graph, boost::vertex_predecessor_t>::type> disjointSets_;
769 
772 
774  unsigned int consecutiveFailures_;
775 
777  long unsigned int iterations_;
778 
780  double sparseDelta_;
781 
783  double denseDelta_;
784 
786  std::vector<Vertex> startVertexCandidateNeighbors_;
787  std::vector<Vertex> goalVertexCandidateNeighbors_;
788 
790  bool verbose_;
791  };
792  }
793 }
794 
795 #endif
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition: SPARSdb.h:122
long unsigned int getIterations() const
Get the number of iterations the algorithm performed.
Definition: SPARSdb.h:557
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition: SPARSdb.h:429
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent...
Definition: SPARSdb.h:735
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition: SPARSdb.h:409
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
unsigned int getNumPathInsertionFailed() const
Get the number of times a path was inserted into the database and it failed to have connectivity...
Definition: SPARSdb.h:545
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition: SPARSdb.h:759
Interface information storage class, which does bookkeeping for criterion four.
Definition: SPARSdb.h:111
bool reachedFailureLimit() const
Returns whether we have reached the iteration failures limit, maxFailures_.
Definition: SPARSdb.cpp:568
boost::readable_property_map_tag category
Definition: SPARSdb.h:332
A shared pointer wrapper for ompl::base::ProblemDefinition.
EdgeCollisionState
Possible collision states of an edge.
Definition: SPARSdb.h:232
void findCloseRepresentatives(base::State *workState, const base::State *qNew, Vertex qRep, std::map< Vertex, base::State *> &closeRepresentatives, const base::PlannerTerminationCondition &ptc)
Finds representatives of samples near qNew_ which are not his representative.
Definition: SPARSdb.cpp:1389
bool convertVertexPathToStatePath(std::vector< Vertex > &vertexPath, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, bool disableCollisionWarning=false)
Convert astar results to correctly ordered path.
Definition: SPARSdb.cpp:1669
SPARSdb(const base::SpaceInformationPtr &si)
Constructor.
Definition: SPARSdb.cpp:98
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: SPARSdb.h:766
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition: SPARSdb.h:417
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARSdb.cpp:573
A shared pointer wrapper for ompl::base::ValidStateSampler.
Graph g_
Connectivity graph.
Definition: SPARSdb.h:716
unsigned int getNumConsecutiveFailures() const
description
Definition: SPARSdb.h:551
void findGraphNeighbors(base::State *state, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near state.
Definition: SPARSdb.cpp:1292
boost::property_map< Graph, boost::edge_weight_t >::type edgeWeightProperty_
Access to the weights of each Edge.
Definition: SPARSdb.h:750
unsigned int getNumConnectedComponents() const
Get the number of disjoint sets in the sparse roadmap.
Definition: SPARSdb.h:530
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition: SPARSdb.h:135
bool checkAddCoverage(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure coverage of the space.
Definition: SPARSdb.cpp:1080
bool lazyCollisionCheck(std::vector< Vertex > &vertexPath, const base::PlannerTerminationCondition &ptc)
Check recalled path for collision and disable as needed.
Definition: SPARSdb.cpp:499
unsigned int getNumVertices() const
Get the number of vertices in the sparse roadmap.
Definition: SPARSdb.h:518
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
bool verbose_
Option to enable debugging output.
Definition: SPARSdb.h:790
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition: SPARSdb.h:762
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool addedSolution_
A flag indicating that a solution has been added during solve()
Definition: SPARSdb.h:771
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Definition: SPARSdb.h:114
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition: SPARSdb.h:305
STL namespace.
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition: SPARSdb.cpp:1188
void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp)
Performs distance checking for the candidate new state, q against the current information.
Definition: SPARSdb.cpp:1554
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition: SPARSdb.h:731
bool checkAddInterface(const base::State *qNew, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the current sample reveals the existence of an interface, and if so...
Definition: SPARSdb.cpp:1145
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:240
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARSdb.cpp:148
boost::property< boost::edge_weight_t, double, boost::property< edge_collision_state_t, int > > EdgeProperties
Definition: SPARSdb.h:294
SPArse Roadmap Spanner Version 2.0
Definition: SPARSdb.h:85
std::unordered_map< VertexPair, InterfaceData > InterfaceHash
the hash which maps pairs of neighbor points to pairs of states
Definition: SPARSdb.h:192
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition: SPARSdb.h:725
void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector< Vertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v&#39;, and v".
Definition: SPARSdb.cpp:1525
unsigned long int VertexIndexType
The type used internally for representing vertex IDs.
Definition: SPARSdb.h:104
void resetFailures()
A reset function for resetting the failures count.
Definition: SPARSdb.cpp:1287
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARSdb.cpp:189
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition: SPARSdb.h:738
bool getSimilarPaths(int nearestK, const base::State *start, const base::State *goal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Search the roadmap for the best path close to the given start and goal states that is valid...
Definition: SPARSdb.cpp:208
unsigned int numPathInsertionFailures_
Track how many solutions fail to have connectivity at end.
Definition: SPARSdb.h:741
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: SPARSdb.h:704
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
Definition: SPARSdb.h:118
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st.
Definition: SPARSdb.cpp:1361
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
Definition: SPARSdb.cpp:1065
std::vector< Vertex > startVertexCandidateNeighbors_
Used by getSimilarPaths.
Definition: SPARSdb.h:786
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition: SPARSdb.h:107
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition: SPARSdb.h:777
Base class for a planner.
Definition: Planner.h:232
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition: SPARSdb.h:302
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: SPARSdb.h:488
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition: SPARSdb.cpp:1539
void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: SPARSdb.cpp:165
boost::property_map< Graph, edge_collision_state_t >::type EdgeCollisionStateMap
Access map that stores the lazy collision checking status of each edge.
Definition: SPARSdb.h:311
void clearEdgeCollisionStates()
Clear all past edge state information about in collision or not.
Definition: SPARSdb.cpp:1850
void setPlannerData(const base::PlannerData &data)
Set the sparse graph from file.
Definition: SPARSdb.cpp:1785
~SPARSdb() override
Destructor.
Definition: SPARSdb.cpp:143
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: SPARSdb.h:710
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
A shared pointer wrapper for ompl::base::SpaceInformation.
bool getPaths(const std::vector< Vertex > &candidateStarts, const std::vector< Vertex > &candidateGoals, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition: SPARSdb.cpp:268
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition: SPARSdb.h:744
void computeVPP(Vertex v, Vertex vp, std::vector< Vertex > &VPPs)
Computes all nodes which qualify as a candidate v" for v and vp.
Definition: SPARSdb.cpp:1516
bool checkAddConnectivity(const base::State *qNew, std::vector< Vertex > &visibleNeighborhood)
Checks to see if the sample needs to be added to ensure connectivity.
Definition: SPARSdb.cpp:1094
Definition of an abstract state.
Definition: State.h:49
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition: SPARSdb.h:401
#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: SPARSdb.h:756
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality) ...
Definition: SPARSdb.h:728
void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the second interface (i.e. interface with larger index vertex).
Definition: SPARSdb.h:176
A shared pointer wrapper for ompl::geometric::PathSimplifier.
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition: SPARSdb.h:780
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition: SPARSdb.h:774
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: SPARSdb.h:722
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition: SPARSdb.cpp:1347
unsigned int getNumEdges() const
Get the number of edges in the sparse roadmap.
Definition: SPARSdb.h:524
EdgeCollisionStateMap edgeCollisionStateProperty_
Access to the collision checking state of each Edge.
Definition: SPARSdb.h:753
RNG rng_
Random number generator.
Definition: SPARSdb.h:768
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:108
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition: SPARSdb.cpp:1549
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties > Graph
Definition: SPARSdb.h:299
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: SPARSdb.cpp:563
void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s)
High-level method which updates pair point information for repV_ with neighbor r. ...
Definition: SPARSdb.cpp:1504
std::vector< Vertex > startM_
Array of start milestones.
Definition: SPARSdb.h:719
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition: SPARSdb.h:747
bool constructSolution(const Vertex start, const Vertex goal, std::vector< Vertex > &vertexPath) const
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition: SPARSdb.cpp:430
Vertex addGuard(base::State *state, GuardType type)
Construct a guard for a given state (state) and store it in the nearest neighbors data structure...
Definition: SPARSdb.cpp:1616
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARSdb.h:423
boost::property< vertex_state_t, base::State *, boost::property< boost::vertex_predecessor_t, VertexIndexType, boost::property< boost::vertex_rank_t, VertexIndexType, boost::property< vertex_color_t, GuardType, boost::property< vertex_interface_data_t, InterfaceHashStruct > >> >> VertexProperties
The underlying roadmap graph.
Definition: SPARSdb.h:290
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition: SPARSdb.h:713
double getStretchFactor() const
Retrieve the spanner&#39;s set stretch factor.
Definition: SPARSdb.h:441
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: SPARSdb.cpp:1074
Definition of a geometric path.
Definition: PathGeometric.h:60
void abandonLists(base::State *st)
When a new guard is added at state st, finds all guards who must abandon their interface information ...
Definition: SPARSdb.cpp:1599
bool lazyCollisionSearch(const Vertex &start, const Vertex &goal, const base::State *actualStart, const base::State *actualGoal, CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc)
Repeatidly search through graph for connection then check for collisions then repeat.
Definition: SPARSdb.cpp:322
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition: SPARSdb.cpp:1641
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARSdb.cpp:171
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: SPARSdb.cpp:1736
void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
Sets information for the first interface (i.e. interface with smaller index vertex).
Definition: SPARSdb.h:161
void setStretchFactor(double t)
Sets the stretch factor.
Definition: SPARSdb.h:395
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition: SPARSdb.h:89
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition: SPARSdb.h:435
const Graph & getRoadmap() const
Retrieve the computed roadmap.
Definition: SPARSdb.h:512
A shared pointer wrapper for ompl::base::Path.
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition: SPARSdb.h:783