Vertex.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Oxford
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 names of the copyright holders 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: Marlin Strub
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_VERTEX_
38 #define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_VERTEX_
39 
40 #include <memory>
41 #include <vector>
42 
43 #include "ompl/base/OptimizationObjective.h"
44 #include "ompl/base/ProblemDefinition.h"
45 #include "ompl/base/ScopedState.h"
46 #include "ompl/base/SpaceInformation.h"
47 #include "ompl/base/State.h"
48 #include "ompl/datastructures/BinaryHeap.h"
49 
50 #include "ompl/geometric/planners/informedtrees/aitstar/Edge.h"
51 #include "ompl/geometric/planners/informedtrees/aitstar/Queuetypes.h"
52 
53 namespace ompl
54 {
55  namespace geometric
56  {
57  namespace aitstar
58  {
59  class Vertex : public std::enable_shared_from_this<Vertex>
60  {
61  public:
63  Vertex(const ompl::base::SpaceInformationPtr &spaceInformation,
64  const ompl::base::ProblemDefinitionPtr &problemDefinition, const std::size_t &batchId);
65 
67  explicit Vertex(const std::shared_ptr<Vertex> &other);
68 
70  virtual ~Vertex();
71 
73  std::size_t getId() const;
74 
77 
79  ompl::base::State const *getState() const;
80 
83 
86 
89 
92 
95 
98 
100  bool hasForwardParent() const;
101 
103  void setForwardParent(const std::shared_ptr<Vertex> &vertex, const ompl::base::Cost &edgeCost);
104 
106  void resetForwardParent();
107 
109  bool hasReverseParent() const;
110 
112  void setReverseParent(const std::shared_ptr<Vertex> &vertex);
113 
115  void resetReverseParent();
116 
118  std::shared_ptr<Vertex> getForwardParent() const;
119 
121  std::shared_ptr<Vertex> getReverseParent() const;
122 
124  void setForwardEdgeCost(const ompl::base::Cost &cost);
125 
127  void setCostToComeFromStart(const ompl::base::Cost &cost);
128 
130  void setCostToComeFromGoal(const ompl::base::Cost &cost);
131 
134 
137 
140 
142  void setCostToGoToGoal(const ompl::base::Cost &cost);
143 
145  void updateCostOfForwardBranch() const;
146 
148  std::vector<std::weak_ptr<Vertex>> invalidateReverseBranch();
149 
151  std::vector<std::weak_ptr<Vertex>> invalidateForwardBranch();
152 
154  void addToForwardChildren(const std::shared_ptr<Vertex> &vertex);
155 
157  void removeFromForwardChildren(std::size_t vertexId);
158 
160  std::vector<std::shared_ptr<Vertex>> getForwardChildren() const;
161 
163  void addToReverseChildren(const std::shared_ptr<Vertex> &vertex);
164 
166  void removeFromReverseChildren(std::size_t vertexId);
167 
169  std::vector<std::shared_ptr<Vertex>> getReverseChildren() const;
170 
172  void whitelistAsChild(const std::shared_ptr<Vertex> &vertex) const;
173 
175  bool isWhitelistedAsChild(const std::shared_ptr<Vertex> &vertex) const;
176 
178  void blacklistAsChild(const std::shared_ptr<Vertex> &vertex) const;
179 
181  bool isBlacklistedAsChild(const std::shared_ptr<Vertex> &vertex) const;
182 
184  bool hasCachedNeighbors() const;
185 
187  void cacheNeighbors(const std::vector<std::shared_ptr<Vertex>> &neighbors) const;
188 
190  const std::vector<std::shared_ptr<Vertex>> getNeighbors() const;
191 
194  bool isConsistent() const;
195 
197  void setReverseQueuePointer(typename VertexQueue::Element *pointer);
198 
200  typename VertexQueue::Element *getReverseQueuePointer() const;
201 
204 
206  void addToForwardQueueIncomingLookup(typename EdgeQueue::Element *pointer);
207 
209  void addToForwardQueueOutgoingLookup(typename EdgeQueue::Element *pointer);
210 
212  std::vector<EdgeQueue::Element *> getForwardQueueIncomingLookup() const;
213 
215  std::vector<EdgeQueue::Element *> getForwardQueueOutgoingLookup() const;
216 
218  void removeFromForwardQueueIncomingLookup(typename EdgeQueue::Element *element);
219 
221  void removeFromForwardQueueOutgoingLookup(typename EdgeQueue::Element *element);
222 
225 
228 
230  void callOnForwardBranch(const std::function<void(const std::shared_ptr<Vertex> &)> &function);
231 
233  void callOnReverseBranch(const std::function<void(const std::shared_ptr<Vertex> &)> &function);
234 
235  private:
237  const ompl::base::SpaceInformationPtr spaceInformation_;
238 
240  const ompl::base::ProblemDefinitionPtr problemDefinition_;
241 
243  const ompl::base::OptimizationObjectivePtr objective_;
244 
246  std::vector<std::weak_ptr<Vertex>> forwardChildren_{};
247 
249  std::vector<std::weak_ptr<Vertex>> reverseChildren_{};
250 
252  mutable std::vector<std::weak_ptr<Vertex>> neighbors_{};
253 
255  mutable std::vector<std::weak_ptr<Vertex>> whitelistedChildren_{};
256 
258  mutable std::vector<std::weak_ptr<Vertex>> blacklistedChildren_{};
259 
261  std::weak_ptr<Vertex> forwardParent_;
262 
264  std::weak_ptr<Vertex> reverseParent_;
265 
267  ompl::base::State *state_;
268 
270  ompl::base::Cost costToComeFromStart_;
271 
273  ompl::base::Cost edgeCostFromForwardParent_;
274 
276  mutable ompl::base::Cost costToComeFromGoal_;
277 
279  mutable ompl::base::Cost expandedCostToComeFromGoal_;
280 
282  mutable ompl::base::Cost costToGoToGoal_;
283 
285  const std::size_t vertexId_;
286 
288  const std::size_t &batchId_;
289 
291  mutable std::size_t neighborBatchId_{0u};
292 
294  mutable std::size_t reverseQueuePointerId_{0u};
295 
297  mutable typename VertexQueue::Element *reverseQueuePointer_{nullptr};
298 
300  mutable std::vector<EdgeQueue::Element *> forwardQueueIncomingLookup_;
301 
303  mutable std::vector<EdgeQueue::Element *> forwardQueueOutgoingLookup_;
304  };
305 
306  } // namespace aitstar
307 
308  } // namespace geometric
309 
310 } // namespace ompl
311 
312 #endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_VERTEX_
void removeFromForwardQueueIncomingLookup(typename EdgeQueue::Element *element)
Remove an element from the incoming queue lookup.
Definition: Vertex.cpp:523
void addToForwardQueueOutgoingLookup(typename EdgeQueue::Element *pointer)
Adds an element to the forward queue outgoing lookup.
Definition: Vertex.cpp:502
bool isWhitelistedAsChild(const std::shared_ptr< Vertex > &vertex) const
Returns whether a child is whitelisted.
Definition: Vertex.cpp:353
A shared pointer wrapper for ompl::base::SpaceInformation.
ompl::base::Cost getCostToGoToGoal() const
Returns the cost to go heuristic from this vertex.
Definition: Vertex.cpp:144
void setExpandedCostToComeFromGoal(const ompl::base::Cost &cost)
Sets the cost to come to this vertex from the goal when it was expanded.
Definition: Vertex.cpp:202
void setReverseQueuePointer(typename VertexQueue::Element *pointer)
Sets the reverse queue pointer of this vertex.
Definition: Vertex.cpp:465
std::vector< EdgeQueue::Element * > getForwardQueueIncomingLookup() const
Returns the forward queue incoming lookup of this vertex.
Definition: Vertex.cpp:511
virtual ~Vertex()
Destructs the vertex.
Definition: Vertex.cpp:103
Definition of an abstract state.
Definition: State.h:113
void addToReverseChildren(const std::shared_ptr< Vertex > &vertex)
Adds a vertex this vertex's children.
Definition: Vertex.cpp:324
void setReverseParent(const std::shared_ptr< Vertex > &vertex)
Sets the parent vertex (in the reverse-search tree).
Definition: Vertex.cpp:283
bool isBlacklistedAsChild(const std::shared_ptr< Vertex > &vertex) const
Returns whether a child is blacklisted.
Definition: Vertex.cpp:384
void addToForwardQueueIncomingLookup(typename EdgeQueue::Element *pointer)
Adds an element to the forward queue incoming lookup.
Definition: Vertex.cpp:495
void resetForwardQueueOutgoingLookup()
Resets the forward queue outgoing lookup.
Definition: Vertex.cpp:544
ompl::base::State * getState()
Provides write access to the underlying state.
Definition: Vertex.cpp:114
void resetCostToComeFromGoal()
Resets the cost to come to this vertex from the goal to infinity.
Definition: Vertex.cpp:192
void removeFromReverseChildren(std::size_t vertexId)
Removes a vertex from this vertex's forward children.
Definition: Vertex.cpp:329
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void setForwardParent(const std::shared_ptr< Vertex > &vertex, const ompl::base::Cost &edgeCost)
Sets the parent vertex (in the forward-search tree).
Definition: Vertex.cpp:260
void removeFromForwardQueueOutgoingLookup(typename EdgeQueue::Element *element)
Remove an element from the outgoing queue lookup.
Definition: Vertex.cpp:531
A shared pointer wrapper for ompl::base::OptimizationObjective.
std::vector< EdgeQueue::Element * > getForwardQueueOutgoingLookup() const
Returns the forward queue outgoing lookup of this vertex.
Definition: Vertex.cpp:518
std::size_t getId() const
Get the unique id of this vertex.
Definition: Vertex.cpp:109
std::vector< std::weak_ptr< Vertex > > invalidateForwardBranch()
Recursively invalidates the branch of the forward tree rooted in this vertex.
Definition: Vertex.cpp:242
void resetForwardQueueIncomingLookup()
Resets the forward queue incoming lookup.
Definition: Vertex.cpp:539
ompl::base::Cost getExpandedCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal when it was expanded.
Definition: Vertex.cpp:139
void resetReverseParent()
Resets the reverse parent of the vertex.
Definition: Vertex.cpp:295
std::vector< std::weak_ptr< Vertex > > invalidateReverseBranch()
Recursively invalidates the branch of the reverse tree rooted in this vertex.
Definition: Vertex.cpp:223
void setCostToComeFromGoal(const ompl::base::Cost &cost)
Sets the cost to come to this vertex from the goal.
Definition: Vertex.cpp:187
std::shared_ptr< Vertex > getForwardParent() const
Returns the parent of the vertex (in the forward-search tree).
Definition: Vertex.cpp:162
void callOnForwardBranch(const std::function< void(const std::shared_ptr< Vertex > &)> &function)
Calls the given function on this vertex and all of its children.
Definition: Vertex.cpp:549
ompl::base::Cost getCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal.
Definition: Vertex.cpp:134
A shared pointer wrapper for ompl::base::ProblemDefinition.
void resetReverseQueuePointer()
Resets the reverse queue pointer.
Definition: Vertex.cpp:490
void setCostToComeFromStart(const ompl::base::Cost &cost)
Sets the cost to come to this vertex.
Definition: Vertex.cpp:182
void callOnReverseBranch(const std::function< void(const std::shared_ptr< Vertex > &)> &function)
Calls the given function on this vertex and all of its children.
Definition: Vertex.cpp:561
void setForwardEdgeCost(const ompl::base::Cost &cost)
Sets the cost to come to this vertex.
Definition: Vertex.cpp:177
void updateCostOfForwardBranch() const
Updates the cost to the whole branch rooted at this vertex.
Definition: Vertex.cpp:212
std::vector< std::shared_ptr< Vertex > > getForwardChildren() const
Returns this vertex's children in the forward search tree.
Definition: Vertex.cpp:436
VertexQueue::Element * getReverseQueuePointer() const
Returns the reverse queue pointer of this vertex.
bool hasCachedNeighbors() const
Returns whether the vertex knows its nearest neighbors on the current approximation.
Definition: Vertex.cpp:407
bool hasReverseParent() const
Returns whether this vertex has a parent in the reverse search.
Definition: Vertex.cpp:167
void setCostToGoToGoal(const ompl::base::Cost &cost)
Sets the cost to go heuristic of this vertex.
Definition: Vertex.cpp:207
Vertex(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, const std::size_t &batchId)
Constructs a vertex by sampling a state.
Definition: Vertex.cpp:63
std::vector< std::shared_ptr< Vertex > > getReverseChildren() const
Returns this vertex's children in the reverse search tree.
Definition: Vertex.cpp:447
void cacheNeighbors(const std::vector< std::shared_ptr< Vertex >> &neighbors) const
Caches the neighbors for the current approximation.
Definition: Vertex.cpp:412
const std::vector< std::shared_ptr< Vertex > > getNeighbors() const
Returns the nearest neighbors, throws if not up to date.
Definition: Vertex.cpp:419
ompl::base::Cost getEdgeCostFromForwardParent() const
Returns the edge cost from the forward parent.
Definition: Vertex.cpp:149
ompl::base::ScopedState getScopedState() const
Returns a scoped copy of the underlying state.
Definition: Vertex.cpp:124
void blacklistAsChild(const std::shared_ptr< Vertex > &vertex) const
Blacklists a child.
Definition: Vertex.cpp:379
std::shared_ptr< Vertex > getReverseParent() const
Returns the parent of the vertex (in the reverse-search tree).
Definition: Vertex.cpp:172
void removeFromForwardChildren(std::size_t vertexId)
Removes a vertex from this vertex's forward children.
Definition: Vertex.cpp:305
void resetForwardParent()
Resets the forward parent of the vertex.
Definition: Vertex.cpp:278
void addToForwardChildren(const std::shared_ptr< Vertex > &vertex)
Adds a vertex to this vertex's forward children.
Definition: Vertex.cpp:300
Definition of a scoped state.
Definition: ScopedState.h:120
bool hasForwardParent() const
Returns whether this vertex has a parent in the forward search.
Definition: Vertex.cpp:154
void whitelistAsChild(const std::shared_ptr< Vertex > &vertex) const
Whitelists a child.
Definition: Vertex.cpp:348
ompl::base::Cost getCostToComeFromStart() const
Returns the cost to come to this vertex from the start.
Definition: Vertex.cpp:129
Main namespace. Contains everything in this library.
void resetExpandedCostToComeFromGoal()
Resets the expanded cost to come to this vertex from the goal to infinity.
Definition: Vertex.cpp:197
bool isConsistent() const
Returns whether the vertex is consistent, i.e., whether its cost-to-come is equal to the cost-to-come...
Definition: Vertex.cpp:459