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  {
114  base::State *pointA_{nullptr};
115  base::State *pointB_{nullptr};
116 
118  base::State *sigmaA_{nullptr};
119  base::State *sigmaB_{nullptr};
120 
122  double d_{std::numeric_limits<double>::infinity()};
123 
125  InterfaceData() = default;
126 
129  {
130  if (pointA_ != nullptr)
131  {
132  si->freeState(pointA_);
133  pointA_ = nullptr;
134  }
135  if (pointB_ != nullptr)
136  {
137  si->freeState(pointB_);
138  pointB_ = nullptr;
139  }
140  if (sigmaA_ != nullptr)
141  {
142  si->freeState(sigmaA_);
143  sigmaA_ = nullptr;
144  }
145  if (sigmaB_ != nullptr)
146  {
147  si->freeState(sigmaB_);
148  sigmaB_ = nullptr;
149  }
150  d_ = std::numeric_limits<double>::infinity();
151  }
152 
154  void setFirst(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
155  {
156  if (pointA_ != nullptr)
157  si->copyState(pointA_, p);
158  else
159  pointA_ = si->cloneState(p);
160  if (sigmaA_ != nullptr)
161  si->copyState(sigmaA_, s);
162  else
163  sigmaA_ = si->cloneState(s);
164  if (pointB_ != nullptr)
165  d_ = si->distance(pointA_, pointB_);
166  }
167 
169  void setSecond(const base::State *p, const base::State *s, const base::SpaceInformationPtr &si)
170  {
171  if (pointB_ != nullptr)
172  si->copyState(pointB_, p);
173  else
174  pointB_ = si->cloneState(p);
175  if (sigmaB_ != nullptr)
176  si->copyState(sigmaB_, s);
177  else
178  sigmaB_ = si->cloneState(s);
179  if (pointA_ != nullptr)
180  d_ = si->distance(pointA_, pointB_);
181  }
182  };
183 
185  using InterfaceHash = std::unordered_map<VertexPair, InterfaceData>;
186 
188  // The InterfaceHash structure is wrapped inside of this struct due to a compilation error on
189  // GCC 4.6 with Boost 1.48. An implicit assignment operator overload does not compile with these
190  // components, so an explicit overload is given here.
191  // Remove this struct when the minimum Boost requirement is > v1.48.
193  {
194  InterfaceHashStruct &operator=(const InterfaceHashStruct &rhs) = default;
195  InterfaceHash interfaceHash;
196  };
197 
199  // Vertex properties
200 
202  {
203  using kind = boost::vertex_property_tag;
204  };
205 
207  {
208  using kind = boost::vertex_property_tag;
209  };
210 
212  {
213  using kind = boost::vertex_property_tag;
214  };
215 
217  // Edge properties
218 
220  {
221  using kind = boost::edge_property_tag;
222  };
223 
226  {
227  NOT_CHECKED,
228  IN_COLLISION,
229  FREE
230  };
231 
234  {
235  bool isApproximate_;
236  base::PathPtr path_;
237  // Edge 0 is edge from vertex 0 to vertex 1. Thus, there is n-1 edges for n vertices
238  std::vector<EdgeCollisionState> edgeCollisionStatus_;
239  // TODO save the collision state of the vertexes also?
240 
241  std::size_t getStateCount()
242  {
243  return static_cast<ompl::geometric::PathGeometric &>(*path_).getStateCount();
244  }
245 
246  ompl::geometric::PathGeometric &getGeometricPath()
247  {
248  return static_cast<ompl::geometric::PathGeometric &>(*path_);
249  }
250  };
251 
253 
277  using VertexProperties = boost::property<
279  boost::property<
280  boost::vertex_predecessor_t, VertexIndexType,
281  boost::property<boost::vertex_rank_t, VertexIndexType,
282  boost::property<vertex_color_t, GuardType,
283  boost::property<vertex_interface_data_t, InterfaceHashStruct>>>>>;
284 
286  using EdgeProperties =
287  boost::property<boost::edge_weight_t, double, boost::property<edge_collision_state_t, int>>;
288 
290  using Graph = boost::adjacency_list<boost::vecS, // store in std::vector
291  boost::vecS, // store in std::vector
292  boost::undirectedS, VertexProperties, EdgeProperties>;
293 
295  using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
296 
298  using Edge = boost::graph_traits<Graph>::edge_descriptor;
299 
301  // Typedefs for property maps
302 
304  using EdgeCollisionStateMap = boost::property_map<Graph, edge_collision_state_t>::type;
305 
307 
312  {
313  private:
314  const Graph &g_; // Graph used
315  const EdgeCollisionStateMap &collisionStates_;
316 
317  public:
319  using key_type = Edge;
321  using value_type = double;
323  using reference = double &;
325  using category = boost::readable_property_map_tag;
326 
331  edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates);
332 
338  double get(Edge e) const;
339  };
340 
342 
346  {
347  };
348 
350 
354  class CustomVisitor : public boost::default_astar_visitor
355  {
356  private:
357  Vertex goal; // Goal Vertex of the search
358 
359  public:
364  CustomVisitor(Vertex goal);
365 
372  void examine_vertex(Vertex u, const Graph &g) const;
373  };
374 
376  // SPARS MEMBER FUNCTIONS
378 
381 
383  ~SPARSdb() override;
384 
385  void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override;
386 
388  void setStretchFactor(double t)
389  {
390  stretchFactor_ = t;
391  }
392 
394  void setSparseDeltaFraction(double D)
395  {
397  if (sparseDelta_ > 0.0) // setup was previously called
398  sparseDelta_ = D * si_->getMaximumExtent();
399  }
400 
402  void setDenseDeltaFraction(double d)
403  {
405  if (denseDelta_ > 0.0) // setup was previously called
406  denseDelta_ = d * si_->getMaximumExtent();
407  }
408 
410  void setMaxFailures(unsigned int m)
411  {
412  maxFailures_ = m;
413  }
414 
416  unsigned int getMaxFailures() const
417  {
418  return maxFailures_;
419  }
420 
422  double getDenseDeltaFraction() const
423  {
424  return denseDeltaFraction_;
425  }
426 
428  double getSparseDeltaFraction() const
429  {
430  return sparseDeltaFraction_;
431  }
432 
434  double getStretchFactor() const
435  {
436  return stretchFactor_;
437  }
438 
439  bool getGuardSpacingFactor(double pathLength, double &numGuards, double &spacingFactor);
440 
448  bool getGuardSpacingFactor(double pathLength, int &numGuards, double &spacingFactor);
449 
450  bool addPathToRoadmap(const base::PlannerTerminationCondition &ptc,
451  ompl::geometric::PathGeometric &solutionPath);
452 
453  bool checkStartGoalConnection(ompl::geometric::PathGeometric &solutionPath);
454 
455  bool addStateToRoadmap(const base::PlannerTerminationCondition &ptc, base::State *newState);
456 
470 
475  void clearQuery();
476 
477  void clear() override;
478 
480  template <template <typename T> class NN>
482  {
483  if (nn_ && nn_->size() != 0)
484  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
485  clear();
486  nn_ = std::make_shared<NN<Vertex>>();
487  if (isSetup())
488  setup();
489  }
490 
499  bool getSimilarPaths(int nearestK, const base::State *start, const base::State *goal,
500  CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc);
501 
502  void setup() override;
503 
505  const Graph &getRoadmap() const
506  {
507  return g_;
508  }
509 
511  unsigned int getNumVertices() const
512  {
513  return boost::num_vertices(g_);
514  }
515 
517  unsigned int getNumEdges() const
518  {
519  return boost::num_edges(g_);
520  }
521 
523  unsigned int getNumConnectedComponents() const
524  {
525  // Make sure graph is populated
526  if (getNumVertices() == 0u)
527  return 0;
528 
529  std::vector<int> components(boost::num_vertices(g_));
530 
531  // it always overcounts by 1, i think because it is missing vertex 0 which is the new state insertion
532  // component
533  return boost::connected_components(g_, &components[0]) - 1;
534  }
535 
538  unsigned int getNumPathInsertionFailed() const
539  {
541  }
542 
544  unsigned int getNumConsecutiveFailures() const
545  {
546  return consecutiveFailures_;
547  }
548 
550  long unsigned int getIterations() const
551  {
552  return iterations_;
553  }
554 
564  bool convertVertexPathToStatePath(std::vector<Vertex> &vertexPath, const base::State *actualStart,
565  const base::State *actualGoal, CandidateSolution &candidateSolution,
566  bool disableCollisionWarning = false);
567 
568  void getPlannerData(base::PlannerData &data) const override;
569 
574  void setPlannerData(const base::PlannerData &data);
575 
577  bool reachedFailureLimit() const;
578 
580  void printDebug(std::ostream &out = std::cout) const;
581 
584 
585  protected:
587  void freeMemory();
588 
591 
593  bool checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood);
594 
596  bool checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood);
597 
600  bool checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
601  std::vector<Vertex> &visibleNeighborhood);
602 
604  bool checkAddPath(Vertex v);
605 
607  void resetFailures();
608 
610  void findGraphNeighbors(base::State *state, std::vector<Vertex> &graphNeighborhood,
611  std::vector<Vertex> &visibleNeighborhood);
612 
619  bool findGraphNeighbors(const base::State *state, std::vector<Vertex> &graphNeighborhood);
620 
622  void approachGraph(Vertex v);
623 
625  Vertex findGraphRepresentative(base::State *st);
626 
628  void findCloseRepresentatives(base::State *workState, const base::State *qNew, Vertex qRep,
629  std::map<Vertex, base::State *> &closeRepresentatives,
631 
633  void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s);
634 
636  void computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs);
637 
639  void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs);
640 
642  VertexPair index(Vertex vp, Vertex vpp);
643 
646 
648  void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp);
649 
652  void abandonLists(base::State *st);
653 
656  Vertex addGuard(base::State *state, GuardType type);
657 
659  void connectGuards(Vertex v, Vertex vp);
660 
664  bool getPaths(const std::vector<Vertex> &candidateStarts, const std::vector<Vertex> &candidateGoals,
665  const base::State *actualStart, const base::State *actualGoal,
666  CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc);
667 
672  bool lazyCollisionSearch(const Vertex &start, const Vertex &goal, const base::State *actualStart,
673  const base::State *actualGoal, CandidateSolution &candidateSolution,
675 
677  bool lazyCollisionCheck(std::vector<Vertex> &vertexPath, const base::PlannerTerminationCondition &ptc);
678 
681 
689  bool constructSolution(Vertex start, Vertex goal, std::vector<Vertex> &vertexPath) const;
690 
693  bool sameComponent(Vertex m1, Vertex m2);
694 
697  double distanceFunction(const Vertex a, const Vertex b) const
698  {
699  return si_->distance(stateProperty_[a], stateProperty_[b]);
700  }
701 
704 
706  std::shared_ptr<NearestNeighbors<Vertex>> nn_;
707 
710 
712  std::vector<Vertex> startM_;
713 
715  std::vector<Vertex> goalM_;
716 
719 
721  double stretchFactor_{3.};
722 
724  double sparseDeltaFraction_{.25};
725 
728  double denseDeltaFraction_{.001};
729 
731  unsigned int maxFailures_{5000u};
732 
734  unsigned int numPathInsertionFailures_{0u};
735 
737  unsigned int nearSamplePoints_;
738 
741 
743  boost::property_map<Graph, boost::edge_weight_t>::type edgeWeightProperty_; // TODO: this is not used
744 
747 
749  boost::property_map<Graph, vertex_state_t>::type stateProperty_;
750 
752  boost::property_map<Graph, vertex_color_t>::type colorProperty_;
753 
755  boost::property_map<Graph, vertex_interface_data_t>::type interfaceDataProperty_;
756 
758  boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type,
759  boost::property_map<Graph, boost::vertex_predecessor_t>::type> disjointSets_;
762 
764  bool addedSolution_{false};
765 
767  unsigned int consecutiveFailures_{0u};
768 
770  long unsigned int iterations_{0ul};
771 
773  double sparseDelta_{0.};
774 
776  double denseDelta_{0.};
777 
779  std::vector<Vertex> startVertexCandidateNeighbors_;
780  std::vector<Vertex> goalVertexCandidateNeighbors_;
781 
783  bool verbose_{false};
784  };
785  }
786 }
787 
788 #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:550
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition: SPARSdb.h:422
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent...
Definition: SPARSdb.h:728
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition: SPARSdb.h:402
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:538
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition: SPARSdb.h:752
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:553
boost::readable_property_map_tag category
Definition: SPARSdb.h:325
A shared pointer wrapper for ompl::base::ProblemDefinition.
EdgeCollisionState
Possible collision states of an edge.
Definition: SPARSdb.h:225
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:1374
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:1654
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:759
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition: SPARSdb.h:410
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARSdb.cpp:558
A shared pointer wrapper for ompl::base::ValidStateSampler.
Graph g_
Connectivity graph.
Definition: SPARSdb.h:709
unsigned int getNumConsecutiveFailures() const
description
Definition: SPARSdb.h:544
void findGraphNeighbors(base::State *state, std::vector< Vertex > &graphNeighborhood, std::vector< Vertex > &visibleNeighborhood)
Finds visible nodes in the graph near state.
Definition: SPARSdb.cpp:1277
boost::property_map< Graph, boost::edge_weight_t >::type edgeWeightProperty_
Access to the weights of each Edge.
Definition: SPARSdb.h:743
unsigned int getNumConnectedComponents() const
Get the number of disjoint sets in the sparse roadmap.
Definition: SPARSdb.h:523
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition: SPARSdb.h:128
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:1065
bool lazyCollisionCheck(std::vector< Vertex > &vertexPath, const base::PlannerTerminationCondition &ptc)
Check recalled path for collision and disable as needed.
Definition: SPARSdb.cpp:484
unsigned int getNumVertices() const
Get the number of vertices in the sparse roadmap.
Definition: SPARSdb.h:511
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:783
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition: SPARSdb.h:755
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:764
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:298
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition: SPARSdb.cpp:1173
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:1539
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition: SPARSdb.h:724
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:1130
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:233
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARSdb.cpp:133
boost::property< boost::edge_weight_t, double, boost::property< edge_collision_state_t, int > > EdgeProperties
Definition: SPARSdb.h:287
InterfaceData()=default
Constructor.
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:185
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition: SPARSdb.h:718
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:1510
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:1272
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARSdb.cpp:174
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition: SPARSdb.h:731
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:193
unsigned int numPathInsertionFailures_
Track how many solutions fail to have connectivity at end.
Definition: SPARSdb.h:734
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:697
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:1346
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches) ...
Definition: SPARSdb.cpp:1050
std::vector< Vertex > startVertexCandidateNeighbors_
Used by getSimilarPaths.
Definition: SPARSdb.h:779
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:770
Base class for a planner.
Definition: Planner.h:223
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition: SPARSdb.h:295
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: SPARSdb.h:481
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition: SPARSdb.cpp:1524
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:150
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:304
void clearEdgeCollisionStates()
Clear all past edge state information about in collision or not.
Definition: SPARSdb.cpp:1835
void setPlannerData(const base::PlannerData &data)
Set the sparse graph from file.
Definition: SPARSdb.cpp:1770
~SPARSdb() override
Destructor.
Definition: SPARSdb.cpp:128
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:703
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:253
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition: SPARSdb.h:737
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:1501
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:1079
Definition of an abstract state.
Definition: State.h:49
bool constructSolution(Vertex start, 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:415
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition: SPARSdb.h:394
#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:749
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality) ...
Definition: SPARSdb.h:721
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:169
A shared pointer wrapper for ompl::geometric::PathSimplifier.
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition: SPARSdb.h:773
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition: SPARSdb.h:767
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: SPARSdb.h:715
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition: SPARSdb.cpp:1332
unsigned int getNumEdges() const
Get the number of edges in the sparse roadmap.
Definition: SPARSdb.h:517
EdgeCollisionStateMap edgeCollisionStateProperty_
Access to the collision checking state of each Edge.
Definition: SPARSdb.h:746
RNG rng_
Random number generator.
Definition: SPARSdb.h:761
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:1534
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties > Graph
Definition: SPARSdb.h:292
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:548
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:1489
std::vector< Vertex > startM_
Array of start milestones.
Definition: SPARSdb.h:712
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition: SPARSdb.h:740
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:1601
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARSdb.h:416
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:283
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition: SPARSdb.h:706
double getStretchFactor() const
Retrieve the spanner&#39;s set stretch factor.
Definition: SPARSdb.h:434
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:1059
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:1584
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:307
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition: SPARSdb.cpp:1626
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
void clearQuery()
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARSdb.cpp:156
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:1721
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:154
void setStretchFactor(double t)
Sets the stretch factor.
Definition: SPARSdb.h:388
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:428
const Graph & getRoadmap() const
Retrieve the computed roadmap.
Definition: SPARSdb.h:505
A shared pointer wrapper for ompl::base::Path.
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition: SPARSdb.h:776