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 
48 namespace ompl
49 {
50  namespace geometric
51  {
67  class BITstar::Vertex
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 
86  BITstar::VertexId getId() const;
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 
111  VertexConstPtr getParent() const;
112 
115 
117  bool isConsistent() const;
118 
120  bool isPruned() const;
121 
124 
126  bool isExpandedOnCurrentSearch() const;
127 
129  bool hasEverBeenExpandedAsRewiring() const;
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 
179  ompl::base::Cost getCost() const;
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 
237  void clearEdgeQueueInLookup();
238 
241 
242  private:
243  // ---
244  // Internal bookkeeping.
245  // ---
246 
248  void updateCostAndDepth(bool cascadeUpdates = true);
249 
250  // ---
251  // Member variables.
252  // ---
253 
255  BITstar::VertexId id_;
256 
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_
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition: Vertex.cpp:198
void clearEdgeQueueOutLookup()
Clear the pointers to all of the outgoing edge queue entries.
Definition: Vertex.cpp:756
A shared pointer wrapper for ompl::base::SpaceInformation.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:215
A queue of edges, sorted according to a sort key.
Definition: SearchQueue.h:129
bool isExpandedOnCurrentApproximation() const
Returns whether the vertex is expanded on current approximation.
Definition: Vertex.cpp:494
Definition of an abstract state.
Definition: State.h:114
void blacklistChild(const VertexConstPtr &vertex)
Put the vertex on the blacklist of children.
Definition: Vertex.cpp:433
bool isPruned() const
Whether the vertex has been pruned.
Definition: Vertex.cpp:489
void registerExpansion()
Mark the vertex as expanded.
Definition: Vertex.cpp:509
bool isRoot() const
Returns whether the vertex is the root of the search tree.
Definition: Vertex.cpp:162
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
bool hasParent() const
Returns whether this vertex has a parent.
Definition: Vertex.cpp:169
unsigned int getDepth() const
Get the depth of the vertex from the root.
Definition: Vertex.cpp:183
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:463
bool isBlacklistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition: Vertex.cpp:443
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:180
bool isExpandedOnCurrentSearch() const
Returns whether the vertex is expaned on current search.
Definition: Vertex.cpp:499
void markPruned()
Mark the vertex as pruned.
Definition: Vertex.cpp:524
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:134
bool hasEverBeenExpandedAsRewiring() const
Returns whether the vertex has ever been expanded as a rewiring.
Definition: Vertex.cpp:504
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:646
void whitelistChild(const VertexConstPtr &vertex)
Put the vertex on the whitelist of children.
Definition: Vertex.cpp:438
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:539
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
Definition: BITstar.h:224
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:221
void clearEdgeQueueInLookup()
Clear the pointers to all of the incoming edge queue entries.
Definition: Vertex.cpp:629
bool isWhitelistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition: Vertex.cpp:448
void getChildren(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition: Vertex.cpp:299
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:212
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
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
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:763
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 registerRewiringExpansion()
Mark expansion to vertices.
Definition: Vertex.cpp:519
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:636
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition: Vertex.cpp:138
void markUnpruned()
Mark the vertex as unpruned.
Definition: Vertex.cpp:532
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:656
bool hasChildren() const
Get whether this vertex has any children.
Definition: Vertex.cpp:292
virtual ~Vertex()
Destruct a vertex.
Definition: Vertex.cpp:130
bool isConsistent() const
Whether the vertex is consistent.
Definition: Vertex.cpp:484
void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Remove an incoming edge queue entry by value to the member vector.
Definition: Vertex.cpp:571
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:773
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
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:227
void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge)
Remove an outgoing edge queue entry by value.
Definition: Vertex.cpp:698
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:470
void clearWhitelist()
Clears the whitelist.
Definition: Vertex.cpp:458
unsigned int edgeQueueOutLookupSize()
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition: Vertex.cpp:783
std::vector< EdgeQueueElemPtr > EdgeQueueElemPtrVector
A vector of edge queue pointers.
Definition: SearchQueue.h:183
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:666
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
void clearBlacklist()
Clears the blacklist.
Definition: Vertex.cpp:453
ompl::base::State * state()
The state of a vertex as a pointer.
Definition: Vertex.cpp:152