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
virtual std::size_t size() const =0
Get the number of elements in the datastructure.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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.
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
edgeWeightMap(const Graph &graph, const EdgeCollisionStateMap &collisionStates)
Definition SPARSdb.cpp:60
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
boost::property< boost::edge_weight_t, double, boost::property< edge_collision_state_t, int > > EdgeProperties
Definition SPARSdb.h:286
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
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition SPARSdb.cpp:134
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, VertexProperties, EdgeProperties > Graph
Definition SPARSdb.h:290
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
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:277
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
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::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
SPARSdb(const base::SpaceInformationPtr &si)
Constructor.
Definition SPARSdb.cpp:99
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()
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