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 InterfaceHash interfaceHash;
195 };
196
198 // Vertex properties
199
201 {
202 using kind = boost::vertex_property_tag;
203 };
204
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
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
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
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
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
394 {
396 if (sparseDelta_ > 0.0) // setup was previously called
397 sparseDelta_ = D * si_->getMaximumExtent();
398 }
399
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
422 {
423 return denseDeltaFraction_;
424 }
425
428 {
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>
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 {
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
606 void resetFailures();
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
644 InterfaceData &getData(Vertex v, Vertex vp, Vertex vpp);
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
709
711 std::vector<Vertex> startM_;
712
714 std::vector<Vertex> goalM_;
715
718
720 double stretchFactor_{3.};
721
724
728
730 unsigned int maxFailures_{5000u};
731
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_;
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
779 std::vector<Vertex> goalVertexCandidateNeighbors_;
780
782 bool verbose_{false};
783 };
784 }
785}
786
787#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:216
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:410
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:324
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:717
double getStretchFactor() const
Retrieve the spanner's set stretch factor.
Definition SPARSdb.h:433
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:714
unsigned int getNumConsecutiveFailures() const
description
Definition SPARSdb.h:543
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:730
double sparseDeltaFraction_
Maximum visibility range for nodes in the graph as a fraction of maximum extent.
Definition SPARSdb.h:723
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:480
RNG rng_
Random number generator.
Definition SPARSdb.h:760
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:387
std::vector< Vertex > startVertexCandidateNeighbors_
Used by getSimilarPaths.
Definition SPARSdb.h:778
double getSparseDeltaFraction() const
Retrieve the sparse graph visibility range delta.
Definition SPARSdb.h:427
boost::graph_traits< Graph >::edge_descriptor Edge
Edge in Graph.
Definition SPARSdb.h:297
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:763
boost::graph_traits< Graph >::vertex_descriptor Vertex
Vertex in Graph.
Definition SPARSdb.h:294
double sparseDelta_
Maximum visibility range for nodes in the graph.
Definition SPARSdb.h:772
unsigned int getNumConnectedComponents() const
Get the number of disjoint sets in the sparse roadmap.
Definition SPARSdb.h:522
unsigned int getNumEdges() const
Get the number of edges in the sparse roadmap.
Definition SPARSdb.h:516
unsigned int consecutiveFailures_
A counter for the number of consecutive failed iterations of the algorithm.
Definition SPARSdb.h:766
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:225
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:285
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:702
void setSparseDeltaFraction(double D)
Sets vertex visibility range as a fraction of max. extent.
Definition SPARSdb.h:393
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:303
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:711
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:733
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:758
boost::property_map< Graph, vertex_interface_data_t >::type interfaceDataProperty_
Access to the interface pair information for the vertices.
Definition SPARSdb.h:754
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:742
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:696
EdgeCollisionStateMap edgeCollisionStateProperty_
Access to the collision checking state of each Edge.
Definition SPARSdb.h:745
long unsigned int getIterations() const
Get the number of iterations the algorithm performed.
Definition SPARSdb.h:549
double denseDeltaFraction_
Maximum range for allowing two samples to support an interface as a fraction of maximum extent.
Definition SPARSdb.h:727
boost::property_map< Graph, vertex_color_t >::type colorProperty_
Access to the colors for the vertices.
Definition SPARSdb.h:751
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:289
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:276
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:739
void setMaxFailures(unsigned int m)
Sets the maximum failures until termination.
Definition SPARSdb.h:409
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:401
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:720
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:775
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:708
unsigned int getMaxFailures() const
Retrieve the maximum consecutive failure limit.
Definition SPARSdb.h:415
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:705
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:504
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:736
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:537
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:421
long unsigned int iterations_
A counter for the number of iterations of the algorithm.
Definition SPARSdb.h:769
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:510
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition SPARSdb.h:748
bool verbose_
Option to enable debugging output.
Definition SPARSdb.h:782
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:233
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