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 but I will need to
57 // include any I use in my .cpp (to avoid dependency loops).
58 #include "ompl/geometric/planners/bitstar/BITstar.h"
59 
60 namespace ompl
61 {
62  namespace geometric
63  {
80  {
81  public:
84 
86  ~Vertex();
87 
89  BITstar::VertexId getId() const;
90 
92  ompl::base::State const *stateConst() const;
93 
96 
98  bool isRoot() const;
99 
101  bool hasParent() const;
102 
105  bool isInTree() const;
106 
109  unsigned int getDepth() const;
110 
113 
116 
119  void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost,
120  bool updateChildCosts = true);
121 
123  void removeParent(bool updateChildCosts = true);
124 
126  bool hasChildren() const;
127 
129  void getChildrenConst(VertexConstPtrVector *children) const;
130 
132  void getChildren(VertexPtrVector *children);
133 
136  void addChild(const VertexPtr &newChild, bool updateChildCosts = true);
137 
140  void removeChild(const VertexPtr &oldChild, bool updateChildCosts = true);
141 
143  ompl::base::Cost getCost() const;
144 
147 
149  bool isNew() const;
150 
152  void markNew();
153 
155  void markOld();
156 
158  bool hasBeenExpandedToSamples() const;
159 
161  void markExpandedToSamples();
162 
165 
167  bool hasBeenExpandedToVertices() const;
168 
170  void markExpandedToVertices();
171 
174 
176  bool isPruned() const;
177 
179  void markPruned();
180 
182  void markUnpruned();
183 
184  protected:
187  void updateCostAndDepth(bool cascadeUpdates = true);
188 
189  private:
191  BITstar::VertexId vId_;
192 
195 
198 
200  ompl::base::State *state_;
201 
203  bool isRoot_;
204 
206  bool isNew_;
207 
209  bool hasBeenExpandedToSamples_;
210 
212  bool hasBeenExpandedToVertices_;
213 
216  bool isPruned_;
217 
219  unsigned int depth_;
220 
223  VertexPtr parentSPtr_;
224 
226  ompl::base::Cost edgeCost_;
227 
229  ompl::base::Cost cost_;
230 
233  std::vector<VertexWeakPtr> childWPtrs_;
234 
236  void assertNotPruned() const;
237  }; // class: Vertex
238  } // geometric
239 } // ompl
240 #endif // OMPL_GEOMETRIC_PLANNERS_BITSTAR_DATASTRUCTURES_VERTEX_
bool hasBeenExpandedToVertices() const
Returns true if the vertex has been expanded towards vertices.
Definition: Vertex.cpp:419
bool hasBeenExpandedToSamples() const
Returns true if the vertex has been expanded towards samples.
Definition: Vertex.cpp:398
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:125
void markNew()
Mark the vertex as new.
Definition: Vertex.cpp:384
bool hasParent() const
Get whether this vertex has a parent.
Definition: Vertex.cpp:111
void markExpandedToSamples()
Mark the vertex as expanded towards samples.
Definition: Vertex.cpp:405
bool isPruned() const
Whether the vertex has been pruned.
Definition: Vertex.cpp:440
VertexPtr getParent()
Get the parent of a vertex as a mutable pointer.
Definition: Vertex.cpp:161
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:460
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:363
bool isNew() const
Returns true if the vertex is marked as new. Vertices are new until marked old.
Definition: Vertex.cpp:377
ompl::base::State * state()
The state of a vertex as a mutable pointer.
Definition: Vertex.cpp:97
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
Vertex(ompl::base::SpaceInformationPtr si, ompl::base::OptimizationObjectivePtr opt, bool root=false)
Constructor.
Definition: Vertex.cpp:54
void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost, bool updateChildCosts=true)
Set the parent of a vertex, cannot be used to replace a previous parent. Will update this vertex&#39;s co...
Definition: Vertex.cpp:182
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
ompl::base::State const * stateConst() const
The state of a vertex as a constant pointer.
Definition: Vertex.cpp:90
void removeChild(const VertexPtr &oldChild, bool updateChildCosts=true)
Remove a child vertex. Does not change this vertex&#39;s cost, and can update the child and its descenden...
Definition: Vertex.cpp:296
void removeParent(bool updateChildCosts=true)
Remove the parent edge. Will update this vertex&#39;s cost, and can update the descendent costs...
Definition: Vertex.cpp:208
void markUnexpandedToVertices()
Mark the vertex as not expanded towards vertices.
Definition: Vertex.cpp:433
A shared pointer wrapper for ompl::base::SpaceInformation.
bool isRoot() const
Whether the vertex is root.
Definition: Vertex.cpp:104
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:356
void markOld()
Mark the vertex as old.
Definition: Vertex.cpp:391
Definition of an abstract state.
Definition: State.h:49
void addChild(const VertexPtr &newChild, bool updateChildCosts=true)
Add a child vertex. Does not change this vertex&#39;s cost, and can update the child and its descendent c...
Definition: Vertex.cpp:282
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition: Vertex.cpp:84
void getChildrenConst(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition: Vertex.cpp:238
The vertex of the underlying graphs in BIT*.
Definition: Vertex.h:79
A shared pointer wrapper for ompl::base::OptimizationObjective.
void getChildren(VertexPtrVector *children)
Get the children of a vertex as mutable pointers.
Definition: Vertex.cpp:260
void markUnexpandedToSamples()
Mark the vertex as not expanded towards samples.
Definition: Vertex.cpp:412
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
bool hasChildren() const
Get whether this vertex has any children.
Definition: Vertex.cpp:231
void markUnpruned()
Mark the vertex as unpruned.
Definition: Vertex.cpp:452
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
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:426
VertexConstPtr getParentConst() const
Get the parent of a vertex as a constant pointer.
Definition: Vertex.cpp:140
void markPruned()
Mark the vertex as pruned.
Definition: Vertex.cpp:445
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:118