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 
111  struct InterfaceData
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 
128  void clear(const base::SpaceInformationPtr &si)
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.
192  struct InterfaceHashStruct
193  {
194  InterfaceHash interfaceHash;
195  };
196 
198  // Vertex properties
199 
201  {
202  using kind = boost::vertex_property_tag;
203  };
204 
205  struct vertex_color_t
206  {
207  using kind = boost::vertex_property_tag;
208  };
209 
211  {
212  using kind = boost::vertex_property_tag;
213  };
214 
216  // Edge properties
217 
219  {
220  using kind = boost::edge_property_tag;
221  };
222 
225  {
226  NOT_CHECKED,
227  IN_COLLISION,
228  FREE
229  };
230 
232  struct CandidateSolution
233  {
234  bool isApproximate_;
235  base::PathPtr path_;
236  // Edge 0 is edge from vertex 0 to vertex 1. Thus, there is n-1 edges for n vertices
237  std::vector<EdgeCollisionState> edgeCollisionStatus_;
238  // TODO save the collision state of the vertexes also?
239 
240  std::size_t getStateCount()
241  {
242  return static_cast<ompl::geometric::PathGeometric &>(*path_).getStateCount();
243  }
244 
245  ompl::geometric::PathGeometric &getGeometricPath()
246  {
247  return static_cast<ompl::geometric::PathGeometric &>(*path_);
248  }
249  };
250 
252 
276  using VertexProperties = boost::property<
278  boost::property<
279  boost::vertex_predecessor_t, VertexIndexType,
280  boost::property<boost::vertex_rank_t, VertexIndexType,
281  boost::property<vertex_color_t, GuardType,
282  boost::property<vertex_interface_data_t, InterfaceHashStruct>>>>>;
283 
285  using EdgeProperties =
286  boost::property<boost::edge_weight_t, double, boost::property<edge_collision_state_t, int>>;
287 
289  using Graph = boost::adjacency_list<boost::vecS, // store in std::vector
290  boost::vecS, // store in std::vector
291  boost::undirectedS, VertexProperties, EdgeProperties>;
292 
294  using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
295 
297  using Edge = boost::graph_traits<Graph>::edge_descriptor;
298 
300  // Typedefs for property maps
301 
303  using EdgeCollisionStateMap = boost::property_map<Graph, edge_collision_state_t>::type;
304 
306 
310  class edgeWeightMap
311  {
312  private:
313  const Graph &g_; // Graph used
314  const EdgeCollisionStateMap &collisionStates_;
315 
316  public:
318  using key_type = Edge;
320  using value_type = double;
322  using reference = double &;
324  using category = boost::readable_property_map_tag;
325 
330  edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates);
331 
337  double get(Edge e) const;
338  };
339 
341 
344  class foundGoalException
345  {
346  };
347 
349 
353  class CustomVisitor : public boost::default_astar_visitor
354  {
355  private:
356  Vertex goal; // Goal Vertex of the search
357 
358  public:
363  CustomVisitor(Vertex goal);
364 
371  void examine_vertex(Vertex u, const Graph &g) const;
372  };
373 
375  // SPARS MEMBER FUNCTIONS
377 
379  SPARSdb(const base::SpaceInformationPtr &si);
380 
382  ~SPARSdb() override;
383 
384  void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override;
385 
387  void setStretchFactor(double t)
388  {
389  stretchFactor_ = t;
390  }
391 
393  void setSparseDeltaFraction(double D)
394  {
396  if (sparseDelta_ > 0.0) // setup was previously called
397  sparseDelta_ = D * si_->getMaximumExtent();
398  }
399 
401  void setDenseDeltaFraction(double d)
402  {
404  if (denseDelta_ > 0.0) // setup was previously called
405  denseDelta_ = d * si_->getMaximumExtent();
406  }
407 
409  void setMaxFailures(unsigned int m)
410  {
411  maxFailures_ = m;
412  }
413 
415  unsigned int getMaxFailures() const
416  {
417  return maxFailures_;
418  }
419 
421  double getDenseDeltaFraction() const
422  {
423  return denseDeltaFraction_;
424  }
425 
427  double getSparseDeltaFraction() const
428  {
429  return sparseDeltaFraction_;
430  }
431 
433  double getStretchFactor() const
434  {
435  return stretchFactor_;
436  }
437 
438  bool getGuardSpacingFactor(double pathLength, double &numGuards, double &spacingFactor);
439 
447  bool getGuardSpacingFactor(double pathLength, int &numGuards, double &spacingFactor);
448 
449  bool addPathToRoadmap(const base::PlannerTerminationCondition &ptc,
450  ompl::geometric::PathGeometric &solutionPath);
451 
452  bool checkStartGoalConnection(ompl::geometric::PathGeometric &solutionPath);
453 
454  bool addStateToRoadmap(const base::PlannerTerminationCondition &ptc, base::State *newState);
455 
469 
474  void clearQuery() override;
475 
476  void clear() override;
477 
479  template <template <typename T> class NN>
480  void setNearestNeighbors()
481  {
482  if (nn_ && nn_->size() != 0)
483  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
484  clear();
485  nn_ = std::make_shared<NN<Vertex>>();
486  if (isSetup())
487  setup();
488  }
489 
498  bool getSimilarPaths(int nearestK, const base::State *start, const base::State *goal,
499  CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc);
500 
501  void setup() override;
502 
504  const Graph &getRoadmap() const
505  {
506  return g_;
507  }
508 
510  unsigned int getNumVertices() const
511  {
512  return boost::num_vertices(g_);
513  }
514 
516  unsigned int getNumEdges() const
517  {
518  return boost::num_edges(g_);
519  }
520 
522  unsigned int getNumConnectedComponents() const
523  {
524  // Make sure graph is populated
525  if (getNumVertices() == 0u)
526  return 0;
527 
528  std::vector<int> components(boost::num_vertices(g_));
529 
530  // it always overcounts by 1, i think because it is missing vertex 0 which is the new state insertion
531  // component
532  return boost::connected_components(g_, &components[0]) - 1;
533  }
534 
537  unsigned int getNumPathInsertionFailed() const
538  {
540  }
541 
543  unsigned int getNumConsecutiveFailures() const
544  {
545  return consecutiveFailures_;
546  }
547 
549  long unsigned int getIterations() const
550  {
551  return iterations_;
552  }
553 
563  bool convertVertexPathToStatePath(std::vector<Vertex> &vertexPath, const base::State *actualStart,
564  const base::State *actualGoal, CandidateSolution &candidateSolution,
565  bool disableCollisionWarning = false);
566 
567  void getPlannerData(base::PlannerData &data) const override;
568 
573  void setPlannerData(const base::PlannerData &data);
574 
576  bool reachedFailureLimit() const;
577 
579  void printDebug(std::ostream &out = std::cout) const;
580 
583 
584  protected:
586  void freeMemory();
587 
590 
592  bool checkAddCoverage(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood);
593 
595  bool checkAddConnectivity(const base::State *qNew, std::vector<Vertex> &visibleNeighborhood);
596 
599  bool checkAddInterface(const base::State *qNew, std::vector<Vertex> &graphNeighborhood,
600  std::vector<Vertex> &visibleNeighborhood);
601 
603  bool checkAddPath(Vertex v);
604 
607 
609  void findGraphNeighbors(base::State *state, std::vector<Vertex> &graphNeighborhood,
610  std::vector<Vertex> &visibleNeighborhood);
611 
618  bool findGraphNeighbors(const base::State *state, std::vector<Vertex> &graphNeighborhood);
619 
621  void approachGraph(Vertex v);
622 
625 
627  void findCloseRepresentatives(base::State *workState, const base::State *qNew, Vertex qRep,
628  std::map<Vertex, base::State *> &closeRepresentatives,
630 
632  void updatePairPoints(Vertex rep, const base::State *q, Vertex r, const base::State *s);
633 
635  void computeVPP(Vertex v, Vertex vp, std::vector<Vertex> &VPPs);
636 
638  void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector<Vertex> &Xs);
639 
641  VertexPair index(Vertex vp, Vertex vpp);
642 
645 
647  void distanceCheck(Vertex rep, const base::State *q, Vertex r, const base::State *s, Vertex rp);
648 
651  void abandonLists(base::State *st);
652 
655  Vertex addGuard(base::State *state, GuardType type);
656 
658  void connectGuards(Vertex v, Vertex vp);
659 
663  bool getPaths(const std::vector<Vertex> &candidateStarts, const std::vector<Vertex> &candidateGoals,
664  const base::State *actualStart, const base::State *actualGoal,
665  CandidateSolution &candidateSolution, const base::PlannerTerminationCondition &ptc);
666 
671  bool lazyCollisionSearch(const Vertex &start, const Vertex &goal, const base::State *actualStart,
672  const base::State *actualGoal, CandidateSolution &candidateSolution,
674 
676  bool lazyCollisionCheck(std::vector<Vertex> &vertexPath, const base::PlannerTerminationCondition &ptc);
677 
679  void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution);
680 
688  bool constructSolution(Vertex start, Vertex goal, std::vector<Vertex> &vertexPath) const;
689 
692  bool sameComponent(Vertex m1, Vertex m2);
693 
696  double distanceFunction(const Vertex a, const Vertex b) const
697  {
698  return si_->distance(stateProperty_[a], stateProperty_[b]);
699  }
700 
702  base::ValidStateSamplerPtr sampler_;
703 
705  std::shared_ptr<NearestNeighbors<Vertex>> nn_;
706 
708  Graph g_;
709 
711  std::vector<Vertex> startM_;
712 
714  std::vector<Vertex> goalM_;
715 
717  Vertex queryVertex_;
718 
720  double stretchFactor_{3.};
721 
723  double sparseDeltaFraction_{.25};
724 
727  double denseDeltaFraction_{.001};
728 
730  unsigned int maxFailures_{5000u};
731 
733  unsigned int numPathInsertionFailures_{0u};
734 
736  unsigned int nearSamplePoints_;
737 
740 
742  boost::property_map<Graph, boost::edge_weight_t>::type edgeWeightProperty_; // TODO: this is not used
743 
746 
748  boost::property_map<Graph, vertex_state_t>::type stateProperty_;
749 
751  boost::property_map<Graph, vertex_color_t>::type colorProperty_;
752 
754  boost::property_map<Graph, vertex_interface_data_t>::type interfaceDataProperty_;
755 
757  boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type,
758  boost::property_map<Graph, boost::vertex_predecessor_t>::type> disjointSets_;
760  RNG rng_;
761 
763  bool addedSolution_{false};
764 
766  unsigned int consecutiveFailures_{0u};
767 
769  long unsigned int iterations_{0ul};
770 
772  double sparseDelta_{0.};
773 
775  double denseDelta_{0.};
776 
778  std::vector<Vertex> startVertexCandidateNeighbors_;
779  std::vector<Vertex> goalVertexCandidateNeighbors_;
780 
782  bool verbose_{false};
783  };
784  }
785 }
786 
787 #endif
void resetFailures()
A reset function for resetting the failures count.
Definition: SPARSdb.cpp:1272
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition: SPARSdb.h:832
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:792
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition: SPARSdb.h:390
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: SPARSdb.h:798
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition: SPARSdb.h:826
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: SPARSdb.h:576
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: SPARSdb.h:810
void clearEdgeCollisionStates()
Clear all past edge state information about in collision or not.
Definition: SPARSdb.cpp:1835
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition: SPARSdb.h:203
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties > Graph
Definition: SPARSdb.h:387
std::vector< Vertex > startVertexCandidateNeighbors_
Used by getSimilarPaths.
Definition: SPARSdb.h:874
A shared pointer wrapper for ompl::geometric::PathSimplifier.
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
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition: SPARSdb.h:835
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: SPARSdb.h:844
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARSdb.cpp:157
void setPlannerData(const base::PlannerData &data)
Set the sparse graph from file.
Definition: SPARSdb.cpp:1770
~SPARSdb() override
Destructor.
Definition: SPARSdb.cpp:129
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition: SPARSdb.h:489
Definition of an abstract state.
Definition: State.h:113
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
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
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition: SPARSdb.cpp:1626
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition: SPARSdb.h:868
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
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality)
Definition: SPARSdb.h:816
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition: SPARSdb.cpp:1332
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition: SPARSdb.cpp:1534
edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates)
Definition: SPARSdb.cpp:60
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:549
RNG rng_
Random number generator.
Definition: SPARSdb.h:856
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:378
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:254
Interface information storage class, which does bookkeeping for criterion four.
Definition: SPARSdb.h:207
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
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition: SPARSdb.h:862
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:416
void setStretchFactor(double t)
Sets the stretch factor.
Definition: SPARSdb.h:483
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
EdgeCollisionStateMap edgeCollisionStateProperty_
Access to the collision checking state of each Edge.
Definition: SPARSdb.h:841
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition: SPARSdb.h:529
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:633
std::vector< Vertex > startM_
Array of start milestones.
Definition: SPARSdb.h:807
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition: SPARSdb.h:801
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
const Graph & getRoadmap() const
Retrieve the computed roadmap.
Definition: SPARSdb.h:600
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARSdb.h:511
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:265
Definition of a geometric path.
Definition: PathGeometric.h:97
unsigned int getNumEdges() const
Get the number of edges in the sparse roadmap.
Definition: SPARSdb.h:612
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:308
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition: SPARSdb.h:218
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:250
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition: SPARSdb.h:185
bool lazyCollisionCheck(std::vector< Vertex > &vertexPath, const base::PlannerTerminationCondition &ptc)
Check recalled path for collision and disable as needed.
Definition: SPARSdb.cpp:485
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
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition: SPARSdb.h:517
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition: SPARSdb.h:505
A class to store the exit status of Planner::solve()
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition: SPARSdb.h:847
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent.
Definition: SPARSdb.h:823
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition: SPARSdb.h:224
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition: SPARSdb.h:523
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition: SPARSdb.h:850
double denseDelta_
Maximum range for allowing two samples to support an interface.
Definition: SPARSdb.h:871
boost::readable_property_map_tag category
Definition: SPARSdb.h:420
boost::property_map< Graph, boost::edge_weight_t >::type edgeWeightProperty_
Access to the weights of each Edge.
Definition: SPARSdb.h:838
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Definition: SPARSdb.h:210
unsigned int getNumVertices() const
Get the number of vertices in the sparse roadmap.
Definition: SPARSdb.h:606
long unsigned int getIterations() const
Get the number of iterations the algorithm performed.
Definition: SPARSdb.h:645
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition: SPARSdb.h:497
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition: SPARSdb.h:819
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARSdb.cpp:164
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
bool reachedFailureLimit() const
Returns whether we have reached the iteration failures limit, maxFailures_.
Definition: SPARSdb.cpp:554
Graph g_
Connectivity graph.
Definition: SPARSdb.h:804
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARSdb.cpp:134
SPARSdb(const base::SpaceInformationPtr &si)
Constructor.
Definition: SPARSdb.cpp:99
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
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
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARSdb.cpp:175
std::unordered_map< VertexPair, InterfaceData > InterfaceHash
the hash which maps pairs of neighbor points to pairs of states
Definition: SPARSdb.h:281
unsigned int getNumConsecutiveFailures() const
description
Definition: SPARSdb.h:639
EdgeCollisionState
Possible collision states of an edge.
Definition: SPARSdb.h:320
bool verbose_
Option to enable debugging output.
Definition: SPARSdb.h:878
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:113
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARSdb.cpp:559
bool addedSolution_
A flag indicating that a solution has been added during solve()
Definition: SPARSdb.h:859
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st
Definition: SPARSdb.cpp:1346
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
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:854
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition: SPARSdb.h:393
unsigned long int VertexIndexType
The type used internally for representing vertex IDs.
Definition: SPARSdb.h:200
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition: SPARSdb.h:813
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
Definition: SPARSdb.h:214
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches)
Definition: SPARSdb.cpp:1050
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
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
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition: SPARSdb.cpp:1524
unsigned int numPathInsertionFailures_
Track how many solutions fail to have connectivity at end.
Definition: SPARSdb.h:829
unsigned int getNumConnectedComponents() const
Get the number of disjoint sets in the sparse roadmap.
Definition: SPARSdb.h:618
void examine_vertex(Vertex u, const Graph &g) const
Definition: SPARSdb.cpp:91
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition: SPARSdb.cpp:1173
boost::property< boost::edge_weight_t, double, boost::property< edge_collision_state_t, int > > EdgeProperties
Definition: SPARSdb.h:382
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 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:194
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:328
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:399
Main namespace. Contains everything in this library.
void computeX(Vertex v, Vertex vp, Vertex vpp, std::vector< Vertex > &Xs)
Computes all nodes which qualify as a candidate x for v, v', and v".
Definition: SPARSdb.cpp:1510
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition: SPARSdb.h:865