Loading...
Searching...
No Matches
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
63namespace ompl
64{
65 namespace geometric
66 {
85 class SPARSdb : public base::Planner
86 {
87 public:
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_{nullptr};
116
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.
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
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
380 SPARSdb(const base::SpaceInformationPtr &si);
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
395 {
397 if (sparseDelta_ > 0.0) // setup was previously called
398 sparseDelta_ = D * si_->getMaximumExtent();
399 }
400
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
423 {
424 return denseDeltaFraction_;
425 }
426
429 {
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() override;
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 {
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
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
645 InterfaceData &getData(Vertex v, Vertex vp, Vertex vpp);
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
680 void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution);
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
703 base::ValidStateSamplerPtr sampler_;
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
725
729
731 unsigned int maxFailures_{5000u};
732
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
780 std::vector<Vertex> goalVertexCandidateNeighbors_;
781
783 bool verbose_{false};
784 };
785 }
786}
787
788#endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:113
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
Definition of an abstract state.
Definition: State.h:50
Definition of a geometric path.
Definition: PathGeometric.h:66
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
A shared pointer wrapper for ompl::geometric::PathSimplifier.
void examine_vertex(Vertex u, const Graph &g) const
Definition: SPARSdb.cpp:91
boost::readable_property_map_tag category
Definition: SPARSdb.h:325
SPArse Roadmap Spanner Version 2.0
Definition: SPARSdb.h:86
Vertex queryVertex_
Vertex for performing nearest neighbor queries.
Definition: SPARSdb.h:718
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition: SPARSdb.h:434
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 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
std::vector< Vertex > goalM_
Array of goal milestones.
Definition: SPARSdb.h:715
unsigned int getNumConsecutiveFailures() const
description
Definition: SPARSdb.h:544
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
std::unordered_map< VertexPair, InterfaceData > InterfaceHash
the hash which maps pairs of neighbor points to pairs of states
Definition: SPARSdb.h:185
unsigned int maxFailures_
The number of consecutive failures to add to the graph before termination.
Definition: SPARSdb.h:731
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition: SPARSdb.h:724
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
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition: SPARSdb.h:481
RNG rng_
Random number generator.
Definition: SPARSdb.h:761
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
void setStretchFactor(double t)
Sets the stretch factor.
Definition: SPARSdb.h:388
std::vector< Vertex > startVertexCandidateNeighbors_
Used by getSimilarPaths.
Definition: SPARSdb.h:779
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition: SPARSdb.h:428
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition: SPARSdb.h:298
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
bool addedSolution_
A flag indicating that a solution has been added during solve()
Definition: SPARSdb.h:764
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition: SPARSdb.h:295
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition: SPARSdb.h:773
unsigned int getNumConnectedComponents() const
Get the number of disjoint sets in the sparse roadmap.
Definition: SPARSdb.h:523
unsigned int getNumEdges() const
Get the number of edges in the sparse roadmap.
Definition: SPARSdb.h:517
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition: SPARSdb.h:767
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition: SPARSdb.cpp:157
~SPARSdb() override
Destructor.
Definition: SPARSdb.cpp:129
VertexPair index(Vertex vp, Vertex vpp)
Rectifies indexing order for accessing the vertex data.
Definition: SPARSdb.cpp:1524
EdgeCollisionState
Possible collision states of an edge.
Definition: SPARSdb.h:226
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
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
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
void resetFailures()
A reset function for resetting the failures count.
Definition: SPARSdb.cpp:1272
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition: SPARSdb.h:703
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition: SPARSdb.h:394
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
bool reachedFailureLimit() const
Returns whether we have reached the iteration failures limit, maxFailures_.
Definition: SPARSdb.cpp:554
std::vector< Vertex > startM_
Array of start milestones.
Definition: SPARSdb.h:712
bool checkAddPath(Vertex v)
Checks vertex v for short paths through its region and adds when appropriate.
Definition: SPARSdb.cpp:1173
unsigned int numPathInsertionFailures_
Track how many solutions fail to have connectivity at end.
Definition: SPARSdb.h:734
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
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition: SPARSdb.h:755
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
boost::property_map< Graph, boost::edge_weight_t >::type edgeWeightProperty_
Access to the weights of each Edge.
Definition: SPARSdb.h:743
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
EdgeCollisionStateMap edgeCollisionStateProperty_
Access to the collision checking state of each Edge.
Definition: SPARSdb.h:746
long unsigned int getIterations() const
Get the number of iterations the algorithm performed.
Definition: SPARSdb.h:550
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent.
Definition: SPARSdb.h:728
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition: SPARSdb.h:752
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
boost::property< boost::edge_weight_t, double, boost::property< edge_collision_state_t, int > > EdgeProperties
Definition: SPARSdb.h:287
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SPARSdb.cpp:134
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SPARSdb.cpp:164
InterfaceData & getData(Vertex v, Vertex vp, Vertex vpp)
Retrieves the Vertex data associated with v,vp,vpp.
Definition: SPARSdb.cpp:1534
void printDebug(std::ostream &out=std::cout) const
Print debug information about planner.
Definition: SPARSdb.cpp:559
void checkQueryStateInitialization()
Check that the query vertex is initialized (used for internal nearest neighbor searches)
Definition: SPARSdb.cpp:1050
PathSimplifierPtr psimp_
A path simplifier used to simplify dense paths added to the graph.
Definition: SPARSdb.h:740
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition: SPARSdb.h:410
void approachGraph(Vertex v)
Approaches the graph from a given vertex.
Definition: SPARSdb.cpp:1332
void setDenseDeltaFraction(double d)
Sets interface support tolerance as a fraction of max. extent.
Definition: SPARSdb.h:402
bool lazyCollisionCheck(std::vector< Vertex > &vertexPath, const base::PlannerTerminationCondition &ptc)
Check recalled path for collision and disable as needed.
Definition: SPARSdb.cpp:485
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
double stretchFactor_
Stretch Factor as per graph spanner literature (multiplicative bound on path quality)
Definition: SPARSdb.h:721
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 denseDelta_
Maximum range for allowing two samples to support an interface.
Definition: SPARSdb.h:776
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
Graph g_
Connectivity graph.
Definition: SPARSdb.h:709
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition: SPARSdb.h:416
unsigned long int VertexIndexType
The type used internally for representing vertex IDs.
Definition: SPARSdb.h:104
std::shared_ptr< NearestNeighbors< Vertex > > nn_
Nearest neighbors data structure.
Definition: SPARSdb.h:706
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
const Graph & getRoadmap() const
Retrieve the computed roadmap.
Definition: SPARSdb.h:505
void freeMemory()
Free all the memory allocated by the planner.
Definition: SPARSdb.cpp:175
unsigned int nearSamplePoints_
Number of sample points to use when trying to detect interfaces.
Definition: SPARSdb.h:737
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
Vertex findGraphRepresentative(base::State *st)
Finds the representative of the input state, st
Definition: SPARSdb.cpp:1346
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
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
std::pair< VertexIndexType, VertexIndexType > VertexPair
Pair of vertices which support an interface.
Definition: SPARSdb.h:107
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 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
void clearEdgeCollisionStates()
Clear all past edge state information about in collision or not.
Definition: SPARSdb.cpp:1835
double getDenseDeltaFraction() const
Retrieve the dense graph interface support delta.
Definition: SPARSdb.h:422
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition: SPARSdb.h:770
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
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
unsigned int getNumVertices() const
Get the number of vertices in the sparse roadmap.
Definition: SPARSdb.h:511
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties > Graph
Definition: SPARSdb.h:292
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition: SPARSdb.h:749
bool verbose_
Option to enable debugging output.
Definition: SPARSdb.h:783
void setPlannerData(const base::PlannerData &data)
Set the sparse graph from file.
Definition: SPARSdb.cpp:1770
GuardType
Enumeration which specifies the reason a guard is added to the spanner.
Definition: SPARSdb.h:90
void connectGuards(Vertex v, Vertex vp)
Connect two guards in the roadmap.
Definition: SPARSdb.cpp:1626
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
Struct for passing around partially solved solutions.
Definition: SPARSdb.h:234
Interface information storage class, which does bookkeeping for criterion four.
Definition: SPARSdb.h:112
double d_
Last known distance between the two interfaces supported by points_ and sigmas.
Definition: SPARSdb.h:122
void clear(const base::SpaceInformationPtr &si)
Clears the given interface data.
Definition: SPARSdb.h:128
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
base::State * pointA_
States which lie inside the visibility region of a vertex and support an interface.
Definition: SPARSdb.h:114
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
base::State * sigmaA_
States which lie just outside the visibility region of a vertex and support an interface.
Definition: SPARSdb.h:118