Loading...
Searching...
No Matches
Vertex.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2014, University of Toronto
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 the University of Toronto 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/* Authors: Jonathan Gammell, Marlin Strub */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_VERTEX_
38#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_VERTEX_
39
40#include <memory>
41#include <vector>
42
43#include "ompl/base/OptimizationObjective.h"
44#include "ompl/base/SpaceInformation.h"
45#include "ompl/geometric/planners/informedtrees/BITstar.h"
46#include "ompl/geometric/planners/informedtrees/bitstar/SearchQueue.h"
47
48namespace ompl
49{
50 namespace geometric
51 {
65
68 {
69 public:
70 // ---
71 // Construction and destruction.
72 // ---
73
75 Vertex(ompl::base::SpaceInformationPtr spaceInformation, const CostHelper *const costHelpPtr, SearchQueue *const queuePtr,
76 const std::shared_ptr<const unsigned int> &approximationId, bool root = false);
77
79 virtual ~Vertex();
80
81 // ---
82 // State access.
83 // ---
84
87
90
92 ompl::base::State const *state() const;
93
94 // ---
95 // Graph information access.
96 // ---
97
99 bool isRoot() const;
100
102 bool hasParent() const;
103
105 bool isInTree() const;
106
108 unsigned int getDepth() const;
109
112
115
117 bool isConsistent() const;
118
120 bool isPruned() const;
121
124
126 bool isExpandedOnCurrentSearch() const;
127
130
131 // ---
132 // Graph modification.
133 // ---
134
137 void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost);
138
140 void removeParent(bool updateChildCosts);
141
143 bool hasChildren() const;
144
146 void getChildren(VertexConstPtrVector *children) const;
147
149 void getChildren(VertexPtrVector *children);
150
153 void addChild(const VertexPtr &newChild);
154
158 void removeChild(const VertexPtr &oldChild);
159
161 void blacklistChild(const VertexConstPtr &vertex);
162
164 void whitelistChild(const VertexConstPtr &vertex);
165
167 bool isBlacklistedAsChild(const VertexConstPtr &vertex) const;
168
170 bool isWhitelistedAsChild(const VertexConstPtr &vertex) const;
171
173 void clearBlacklist();
174
176 void clearWhitelist();
177
180
183
185 void registerExpansion();
186
189
191 void markPruned();
192
194 void markUnpruned();
195
196 // ---
197 // Edge queue lookups.
198 // ---
199
202
205
208
211
213 void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &inEdge);
214
216 void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &outEdge);
217
219 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstBegin();
220
222 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstBegin();
223
225 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstEnd();
226
228 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstEnd();
229
231 unsigned int edgeQueueInLookupSize();
232
234 unsigned int edgeQueueOutLookupSize();
235
238
241
242 private:
243 // ---
244 // Internal bookkeeping.
245 // ---
246
248 void updateCostAndDepth(bool cascadeUpdates = true);
249
250 // ---
251 // Member variables.
252 // ---
253
256
258 ompl::base::SpaceInformationPtr si_;
259
261 const CostHelper *const costHelpPtr_;
262
264 SearchQueue *const queuePtr_;
265
267 ompl::base::State *state_;
268
270 bool isRoot_;
271
274 bool isPruned_{false};
275
277 unsigned int depth_{0u};
278
281 VertexPtr parentPtr_;
282
284 ompl::base::Cost edgeCost_;
285
287 ompl::base::Cost cost_;
288
290 ompl::base::Cost costAtExpansion_;
291
294 std::vector<VertexWeakPtr> children_;
295
297 SearchQueue::EdgeQueueElemPtrVector edgeQueueInLookup_;
298
300 SearchQueue::EdgeQueueElemPtrVector edgeQueueOutLookup_;
301
303 std::set<BITstar::VertexId> childIdBlacklist_;
304
306 std::set<BITstar::VertexId> childIdWhitelist_;
307
309 unsigned int lookupApproximationId_{0u};
310
312 unsigned int expansionApproximationId_{0u};
313
315 unsigned int expansionSearchId_{0u};
316
318 bool hasEverBeenExpandedAsRewiring_{false};
319
321 const std::shared_ptr<const unsigned int> currentSearchId_;
322
324 const std::shared_ptr<const unsigned int> currentApproximationId_;
325
327 void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete);
328
330 void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete);
331
333 void clearLookupsIfOutdated();
334 }; // class Vertex
335 } // namespace geometric
336} // namespace ompl
337
338#endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_VERTEX_
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Definition of an abstract state.
Definition State.h:50
A helper class to handle the various heuristic functions in one place.
Definition CostHelper.h:70
A queue of edges, sorted according to a sort key.
Definition SearchQueue.h:65
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition SearchQueue.h:84
std::vector< EdgeQueueElemPtr > EdgeQueueElemPtrVector
A vector of edge queue pointers.
Definition SearchQueue.h:87
The vertex of the underlying graphs in gBITstar BIT*.
Definition Vertex.h:68
void removeChild(const VertexPtr &oldChild)
Remove a child from this vertex. Does not change this vertex's cost or those of its descendants....
Definition Vertex.cpp:370
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstEnd()
Get an iterator to the end of the outgoing edge queue entry vector. Will clear existing in/out lookup...
Definition Vertex.cpp:782
bool isConsistent() const
Whether the vertex is consistent.
Definition Vertex.cpp:487
bool hasParent() const
Returns whether this vertex has a parent.
Definition Vertex.cpp:169
void markPruned()
Mark the vertex as pruned.
Definition Vertex.cpp:527
Vertex(ompl::base::SpaceInformationPtr spaceInformation, const CostHelper *const costHelpPtr, SearchQueue *const queuePtr, const std::shared_ptr< const unsigned int > &approximationId, bool root=false)
Construct a vertex using space information, and helpers to compute various costs.
Definition Vertex.cpp:107
void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge)
Remove an outgoing edge queue entry by value.
Definition Vertex.cpp:704
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstBegin()
Get an iterator to the front of the incoming edge queue entry vector. Will clear existing in/out look...
Definition Vertex.cpp:642
void getChildren(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition Vertex.cpp:299
void insertInEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Add to the list of the edge queue entries that point in to this vertex. Will clear existing in/out lo...
Definition Vertex.cpp:542
virtual ~Vertex()
Destruct a vertex.
Definition Vertex.cpp:130
bool isBlacklistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition Vertex.cpp:446
bool isInTree() const
Get whether a vertex is in the search tree or a sample (i.e., a vertex of the RRG).
Definition Vertex.cpp:176
void blacklistChild(const VertexConstPtr &vertex)
Put the vertex on the blacklist of children.
Definition Vertex.cpp:436
bool hasEverBeenExpandedAsRewiring() const
Returns whether the vertex has ever been expanded as a rewiring.
Definition Vertex.cpp:507
void removeParent(bool updateChildCosts)
Remove the parent of this vertex. Will always update this vertex's cost, and can update the descenden...
Definition Vertex.cpp:267
void clearEdgeQueueInLookup()
Clear the pointers to all of the incoming edge queue entries.
Definition Vertex.cpp:635
void markUnpruned()
Mark the vertex as unpruned.
Definition Vertex.cpp:535
unsigned int edgeQueueOutLookupSize()
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition Vertex.cpp:792
void addChild(const VertexPtr &newChild)
Add a child to this vertex. Does not change this vertex's cost or those of its descendants....
Definition Vertex.cpp:343
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstEnd()
Get an iterator to the end of the incoming edge queue entry vector. Will clear existing in/out lookup...
Definition Vertex.cpp:652
bool hasChildren() const
Get whether this vertex has any children.
Definition Vertex.cpp:292
bool isWhitelistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition Vertex.cpp:451
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition Vertex.cpp:198
bool isPruned() const
Whether the vertex has been pruned.
Definition Vertex.cpp:492
void whitelistChild(const VertexConstPtr &vertex)
Put the vertex on the whitelist of children.
Definition Vertex.cpp:441
bool isRoot() const
Returns whether the vertex is the root of the search tree.
Definition Vertex.cpp:162
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstBegin()
Get an iterator to the front of the outgoing edge queue entry vector. Will clear existing in/out look...
Definition Vertex.cpp:772
void clearBlacklist()
Clears the blacklist.
Definition Vertex.cpp:456
bool isExpandedOnCurrentApproximation() const
Returns whether the vertex is expanded on current approximation.
Definition Vertex.cpp:497
void registerExpansion()
Mark the vertex as expanded.
Definition Vertex.cpp:512
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition Vertex.cpp:473
void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost)
Set the parent of this vertex, cannot be used to replace a previous parent. Will always update this v...
Definition Vertex.cpp:240
ompl::base::State * state()
The state of a vertex as a pointer.
Definition Vertex.cpp:152
void registerRewiringExpansion()
Mark expansion to vertices.
Definition Vertex.cpp:522
void clearWhitelist()
Clears the whitelist.
Definition Vertex.cpp:461
bool isExpandedOnCurrentSearch() const
Returns whether the vertex is expaned on current search.
Definition Vertex.cpp:502
void clearEdgeQueueOutLookup()
Clear the pointers to all of the outgoing edge queue entries.
Definition Vertex.cpp:765
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition Vertex.cpp:466
void insertInEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge)
Add to the list of the edge queue entries that point out of this vertex. Will clear existing in/out l...
Definition Vertex.cpp:672
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition Vertex.cpp:138
unsigned int getDepth() const
Get the depth of the vertex from the root.
Definition Vertex.cpp:183
void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Remove an incoming edge queue entry by value to the member vector.
Definition Vertex.cpp:574
unsigned int edgeQueueInLookupSize()
Get the number of edge queue entries incoming to this vertex. Will clear existing in/out lookups if t...
Definition Vertex.cpp:662
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
Definition BITstar.h:128
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition BITstar.h:125
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition BITstar.h:119
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition BITstar.h:116
unsigned int VertexId
The vertex id type.
Definition BITstar.h:131
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.