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 */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_VERTEX_
38 #define OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_VERTEX_
39 
40 // vector
41 #include <vector>
42 
43 // shared and weak pointers
44 #include <memory>
45 // For unordered sets of failed children:
46 #include <unordered_set>
47 
48 // OMPL:
49 // The space information
50 #include "ompl/base/SpaceInformation.h"
51 // The optimization objective
52 #include "ompl/base/OptimizationObjective.h"
53 
54 // BIT*:
55 // I am member class of the BITstar class (i.e., I am in it's namespace), so I need to include it's definition to be
56 // aware of the class BITstar. It has a forward declaration to me and the other helper classes.
57 #include "ompl/geometric/planners/bitstar/BITstar.h"
58 // I store data for the SearchQueue, get their definitions.
59 #include "ompl/geometric/planners/bitstar/datastructures/SearchQueue.h"
60 
61 namespace ompl
62 {
63  namespace geometric
64  {
81  {
82  public:
84  // Public functions:
86  Vertex(ompl::base::SpaceInformationPtr si, const CostHelper *const costHelpPtr, bool root = false);
87 
89  virtual ~Vertex();
90 
92  BITstar::VertexId getId() const;
93 
95  ompl::base::State const *stateConst() const;
96 
99 
101  // The vertex's graph properties:
103  bool isRoot() const;
104 
106  bool hasParent() const;
107 
110  bool isInTree() const;
111 
114  unsigned int getDepth() const;
115 
118 
121 
124  void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost, bool updateChildCosts);
125 
127  void removeParent(bool updateChildCosts);
128 
130  bool hasChildren() const;
131 
133  void getChildrenConst(VertexConstPtrVector *children) const;
134 
136  void getChildren(VertexPtrVector *children);
137 
139  void addChild(const VertexPtr &newChild);
140 
144  void removeChild(const VertexPtr &oldChild);
145 
147  ompl::base::Cost getCost() const;
148 
151 
153  bool isNew() const;
154 
156  void markNew();
157 
159  void markOld();
160 
162  bool hasBeenExpandedToSamples() const;
163 
165  void markExpandedToSamples();
166 
169 
171  bool hasBeenExpandedToVertices() const;
172 
174  void markExpandedToVertices();
175 
178 
180  bool isPruned() const;
181 
183  void markPruned();
184 
186  void markUnpruned();
188 
190  // Functions for the vertex's SearchQueue data
192  // Vertex queue info:
195 
198 
200  void clearVertexQueueIter();
201 
203  bool hasVertexQueueEntry() const;
205 
207  // Edge queue info (incoming edges):
209  void addIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &newInPtr, unsigned int vertexQueueResetNum);
210 
212  void rmIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum);
213 
215  void rmIncomingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &constIterToDelete, unsigned int vertexQueueResetNum);
216 
219 
221  BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator incomingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum);
222 
224  BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator incomingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum);
225 
227  unsigned int getNumIncomingEdgeQueuePtrs(unsigned int vertexQueueResetNum);
228 
230  bool hasIncomingEdgeQueueEntries(unsigned int vertexQueueResetNum);
232 
234  // Edge queue info (outgoing edges):
236  void addOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &newOutPtr, unsigned int vertexQueueResetNum);
237 
239  void rmOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum);
240 
242  void rmOutgoingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &constIterToDelete, unsigned int vertexQueueResetNum);
243 
246 
248  BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator outgoingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum);
249 
251  BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator outgoingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum);
252 
254  unsigned int getNumOutgoingEdgeQueuePtrs(unsigned int vertexQueueResetNum);
255 
257  bool hasOutgoingEdgeQueueEntries(unsigned int vertexQueueResetNum);
261  protected:
264  void updateCostAndDepth(bool cascadeUpdates = true);
265 
266  private:
268  BITstar::VertexId vId_;
269 
272 
274  const CostHelper *const costHelpPtr_;
275 
277  ompl::base::State *state_;
278 
280  bool isRoot_;
281 
283  bool isNew_{true};
284 
286  bool hasBeenExpandedToSamples_{false};
287 
289  bool hasBeenExpandedToVertices_{false};
290 
293  bool isPruned_{false};
294 
296  unsigned int depth_{0u};
297 
300  VertexPtr parentSPtr_;
301 
303  ompl::base::Cost edgeCost_;
304 
306  ompl::base::Cost cost_;
307 
310  std::vector<VertexWeakPtr> childWPtrs_;
311 
313  SearchQueue::VertexQueueIter vertexQueueIter_;
314 
316  bool isVertexQueueSet_{false};
317 
319  SearchQueue::EdgeQueueElemPtrVector edgeQueueInPtrs_;
320 
322  SearchQueue::EdgeQueueElemPtrVector edgeQueueOutPtrs_;
323 
326  unsigned int edgeLookupPass_{0u};
327 
329  void rmIncomingHelper(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete);
330 
332  void rmOutgoingHelper(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete);
333 
335  void clearOldLookups(unsigned int vertexQueueResetNum);
336 
338  void assertNotPruned() const;
339  }; // class: Vertex
340  } // geometric
341 } // ompl
342 #endif // OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_VERTEX_
bool hasBeenExpandedToVertices() const
Returns true if the vertex has been expanded towards vertices.
Definition: Vertex.cpp:472
bool hasBeenExpandedToSamples() const
Returns true if the vertex has been expanded towards samples.
Definition: Vertex.cpp:449
bool hasIncomingEdgeQueueEntries(unsigned int vertexQueueResetNum)
Return true if the vertex has links to incoming entries in the Edge Queue. Will clear existing in/out...
Definition: Vertex.cpp:699
void clearVertexQueueIter()
Clear the iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:554
unsigned int getDepth() const
Get the "depth" of the vertex from the root. A root vertex is at depth 0, a direct descendent of the ...
Definition: Vertex.cpp:151
void removeChild(const VertexPtr &oldChild)
Remove a child from this vertex. Does not change this vertex&#39;s cost or those of its descendants...
Definition: Vertex.cpp:339
virtual ~Vertex()
Destructor.
Definition: Vertex.cpp:98
void markNew()
Mark the vertex as new.
Definition: Vertex.cpp:433
bool hasParent() const
Get whether this vertex has a parent.
Definition: Vertex.cpp:137
void removeParent(bool updateChildCosts)
Remove the parent of this vertex. Will always update this vertex&#39;s cost, and can update the descenden...
Definition: Vertex.cpp:236
void markExpandedToSamples()
Mark the vertex as expanded towards samples.
Definition: Vertex.cpp:456
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator outgoingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
Get an iterator to the front of the outgoing edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:810
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:69
std::vector< EdgeQueueElemPtr > EdgeQueueElemPtrVector
A vector of edge queue pointers.
Definition: SearchQueue.h:122
void rmOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
Remove an outgoing edge queue entry by value.
Definition: Vertex.cpp:745
bool isPruned() const
Whether the vertex has been pruned.
Definition: Vertex.cpp:495
VertexPtr getParent()
Get the parent of a vertex as a mutable pointer.
Definition: Vertex.cpp:187
void updateCostAndDepth(bool cascadeUpdates=true)
Calculates the updated cost and depth of the current state, as well as calling all children&#39;s updateC...
Definition: Vertex.cpp:856
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:412
void addIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &newInPtr, unsigned int vertexQueueResetNum)
Add to the list of the edge queue entries that point in to this vertex. Will clear existing in/out lo...
Definition: Vertex.cpp:572
bool isNew() const
Returns true if the vertex is marked as new. Vertices are new until marked old.
Definition: Vertex.cpp:426
void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost, bool updateChildCosts)
Set the parent of this vertex, cannot be used to replace a previous parent. Will always update this v...
Definition: Vertex.cpp:208
void setVertexQueueIter(const SearchQueue::VertexQueueIter &newPtr)
Set the iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:535
void addChild(const VertexPtr &newChild)
Add a child to this vertex. Does not change this vertex&#39;s cost or those of its descendants. Child must already have this vertex listed as it&#39;s parent.
Definition: Vertex.cpp:312
Vertex(ompl::base::SpaceInformationPtr si, const CostHelper *const costHelpPtr, bool root=false)
Constructor.
Definition: Vertex.cpp:79
unsigned int getNumIncomingEdgeQueuePtrs(unsigned int vertexQueueResetNum)
Get the number of edge queue entries incoming to this vertex. Will clear existing in/out lookups if t...
Definition: Vertex.cpp:689
ompl::base::State * state()
The state of a vertex as a mutable pointer.
Definition: Vertex.cpp:120
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
void rmOutgoingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &constIterToDelete, unsigned int vertexQueueResetNum)
Remove an outgoing edge queue entry by iterator to the member vector.
Definition: Vertex.cpp:785
bool hasVertexQueueEntry() const
Return true if the vertex has a link to it&#39;s entry in the Vertex Queue.
Definition: Vertex.cpp:562
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
SearchQueue::VertexQueueIter getVertexQueueIter() const
Get an iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:520
void addOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &newOutPtr, unsigned int vertexQueueResetNum)
Add to the list of the edge queue entries that point out of this vertex. Will clear existing in/out l...
Definition: Vertex.cpp:713
ompl::base::State const * stateConst() const
The state of a vertex as a constant pointer.
Definition: Vertex.cpp:113
void rmIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
Remove an incoming edge queue entry by value to the member vector.
Definition: Vertex.cpp:604
void clearIncomingEdgeQueuePtrs()
Clear the pointers to all of the incoming edge queue entries.
Definition: Vertex.cpp:662
void markUnexpandedToVertices()
Mark the vertex as not expanded towards vertices.
Definition: Vertex.cpp:487
A shared pointer wrapper for ompl::base::SpaceInformation.
bool isRoot() const
Whether the vertex is root.
Definition: Vertex.cpp:130
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:405
void markOld()
Mark the vertex as old.
Definition: Vertex.cpp:441
Definition of an abstract state.
Definition: State.h:49
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator incomingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
Get an iterator to the front of the incoming edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:669
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition: Vertex.cpp:106
EdgeQueueAsPairBinHeap::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:120
void getChildrenConst(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition: Vertex.cpp:268
The vertex of the underlying graphs in BIT*.
Definition: Vertex.h:80
void getChildren(VertexPtrVector *children)
Get the children of a vertex as mutable pointers.
Definition: Vertex.cpp:290
void markUnexpandedToSamples()
Mark the vertex as not expanded towards samples.
Definition: Vertex.cpp:464
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers.
Definition: BITstar.h:130
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:132
void rmIncomingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &constIterToDelete, unsigned int vertexQueueResetNum)
Remove an incoming edge queue entry by iterator to the member vector.
Definition: Vertex.cpp:644
bool hasChildren() const
Get whether this vertex has any children.
Definition: Vertex.cpp:261
unsigned int getNumOutgoingEdgeQueuePtrs(unsigned int vertexQueueResetNum)
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition: Vertex.cpp:830
void markUnpruned()
Mark the vertex as unpruned.
Definition: Vertex.cpp:508
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator outgoingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
Get an iterator to the end of the outgoing edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:820
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator incomingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
Get an iterator to the end of the incoming edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:679
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
bool hasOutgoingEdgeQueueEntries(unsigned int vertexQueueResetNum)
Return true if the vertex has links to outgoing entries in the Edge Queue. Will clear existing in/out...
Definition: Vertex.cpp:840
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void markExpandedToVertices()
Mark the vertex as expanded towards vertices.
Definition: Vertex.cpp:479
VertexConstPtr getParentConst() const
Get the parent of a vertex as a constant pointer.
Definition: Vertex.cpp:166
void markPruned()
Mark the vertex as pruned.
Definition: Vertex.cpp:500
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:134
bool isInTree() const
Get whether a vertex is "in the graph" or not. This returns true if the vertex is the graph root or i...
Definition: Vertex.cpp:144
VertexQueueAsMMap::iterator VertexQueueIter
An iterator into the vertex queue multimap.
Definition: SearchQueue.h:109
void clearOutgoingEdgeQueuePtrs()
Clear the pointers to all of the outgoing edge queue entries.
Definition: Vertex.cpp:803