Loading...
Searching...
No Matches
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
53namespace 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
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
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_
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
A shared pointer wrapper for ompl::base::OptimizationObjective.
A shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
Definition ScopedState.h:57
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
std::vector< std::shared_ptr< Vertex > > getReverseChildren() const
Returns this vertex's children in the reverse search tree.
Definition Vertex.cpp:447
std::vector< std::weak_ptr< Vertex > > invalidateForwardBranch()
Recursively invalidates the branch of the forward tree rooted in this vertex.
Definition Vertex.cpp:242
ompl::base::State * getState()
Provides write access to the underlying state.
Definition Vertex.cpp:114
std::vector< EdgeQueue::Element * > getForwardQueueIncomingLookup() const
Returns the forward queue incoming lookup of this vertex.
Definition Vertex.cpp:511
void resetReverseParent()
Resets the reverse parent of the vertex.
Definition Vertex.cpp:295
bool isWhitelistedAsChild(const std::shared_ptr< Vertex > &vertex) const
Returns whether a child is whitelisted.
Definition Vertex.cpp:353
void removeFromForwardQueueOutgoingLookup(typename EdgeQueue::Element *element)
Remove an element from the outgoing queue lookup.
Definition Vertex.cpp:531
void blacklistAsChild(const std::shared_ptr< Vertex > &vertex) const
Blacklists a child.
Definition Vertex.cpp:379
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 resetForwardQueueOutgoingLookup()
Resets the forward queue outgoing lookup.
Definition Vertex.cpp:544
void addToForwardQueueOutgoingLookup(typename EdgeQueue::Element *pointer)
Adds an element to the forward queue outgoing lookup.
Definition Vertex.cpp:502
void removeFromForwardChildren(std::size_t vertexId)
Removes a vertex from this vertex's forward children.
Definition Vertex.cpp:305
bool hasCachedNeighbors() const
Returns whether the vertex knows its nearest neighbors on the current approximation.
Definition Vertex.cpp:407
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
virtual ~Vertex()
Destructs the vertex.
Definition Vertex.cpp:103
std::vector< EdgeQueue::Element * > getForwardQueueOutgoingLookup() const
Returns the forward queue outgoing lookup of this vertex.
Definition Vertex.cpp:518
void resetExpandedCostToComeFromGoal()
Resets the expanded cost to come to this vertex from the goal to infinity.
Definition Vertex.cpp:197
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 resetReverseQueuePointer()
Resets the reverse queue pointer.
Definition Vertex.cpp:490
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
void setCostToComeFromGoal(const ompl::base::Cost &cost)
Sets the cost to come to this vertex from the goal.
Definition Vertex.cpp:187
ompl::base::Cost getCostToComeFromStart() const
Returns the cost to come to this vertex from the start.
Definition Vertex.cpp:129
void addToReverseChildren(const std::shared_ptr< Vertex > &vertex)
Adds a vertex this vertex's children.
Definition Vertex.cpp:324
ompl::base::Cost getCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal.
Definition Vertex.cpp:134
ompl::base::ScopedState getScopedState() const
Returns a scoped copy of the underlying state.
Definition Vertex.cpp:124
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
bool hasForwardParent() const
Returns whether this vertex has a parent in the forward search.
Definition Vertex.cpp:154
bool isBlacklistedAsChild(const std::shared_ptr< Vertex > &vertex) const
Returns whether a child is blacklisted.
Definition Vertex.cpp:384
void resetForwardQueueIncomingLookup()
Resets the forward queue incoming lookup.
Definition Vertex.cpp:539
std::size_t getId() const
Get the unique id of this vertex.
Definition Vertex.cpp:109
void addToForwardChildren(const std::shared_ptr< Vertex > &vertex)
Adds a vertex to this vertex's forward children.
Definition Vertex.cpp:300
void updateCostOfForwardBranch() const
Updates the cost to the whole branch rooted at this vertex.
Definition Vertex.cpp:212
void setForwardEdgeCost(const ompl::base::Cost &cost)
Sets the cost to come to this vertex.
Definition Vertex.cpp:177
VertexQueue::Element * getReverseQueuePointer() const
Returns the reverse queue pointer of this vertex.
Definition Vertex.cpp:481
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 getCostToGoToGoal() const
Returns the cost to go heuristic from this vertex.
Definition Vertex.cpp:144
void setReverseParent(const std::shared_ptr< Vertex > &vertex)
Sets the parent vertex (in the reverse-search tree).
Definition Vertex.cpp:283
void removeFromReverseChildren(std::size_t vertexId)
Removes a vertex from this vertex's forward children.
Definition Vertex.cpp:329
void cacheNeighbors(const std::vector< std::shared_ptr< Vertex > > &neighbors) const
Caches the neighbors for the current approximation.
Definition Vertex.cpp:412
void whitelistAsChild(const std::shared_ptr< Vertex > &vertex) const
Whitelists a child.
Definition Vertex.cpp:348
bool hasReverseParent() const
Returns whether this vertex has a parent in the reverse search.
Definition Vertex.cpp:167
void addToForwardQueueIncomingLookup(typename EdgeQueue::Element *pointer)
Adds an element to the forward queue incoming lookup.
Definition Vertex.cpp:495
std::vector< std::weak_ptr< Vertex > > invalidateReverseBranch()
Recursively invalidates the branch of the reverse tree rooted in this vertex.
Definition Vertex.cpp:223
const std::vector< std::shared_ptr< Vertex > > getNeighbors() const
Returns the nearest neighbors, throws if not up to date.
Definition Vertex.cpp:419
void setCostToComeFromStart(const ompl::base::Cost &cost)
Sets the cost to come to this vertex.
Definition Vertex.cpp:182
void removeFromForwardQueueIncomingLookup(typename EdgeQueue::Element *element)
Remove an element from the incoming queue lookup.
Definition Vertex.cpp:523
void setCostToGoToGoal(const ompl::base::Cost &cost)
Sets the cost to go heuristic of this vertex.
Definition Vertex.cpp:207
std::shared_ptr< Vertex > getForwardParent() const
Returns the parent of the vertex (in the forward-search tree).
Definition Vertex.cpp:162
void setReverseQueuePointer(typename VertexQueue::Element *pointer)
Sets the reverse queue pointer of this vertex.
Definition Vertex.cpp:465
ompl::base::Cost getEdgeCostFromForwardParent() const
Returns the edge cost from the forward parent.
Definition Vertex.cpp:149
std::vector< std::shared_ptr< Vertex > > getForwardChildren() const
Returns this vertex's children in the forward search tree.
Definition Vertex.cpp:436
std::shared_ptr< Vertex > getReverseParent() const
Returns the parent of the vertex (in the reverse-search tree).
Definition Vertex.cpp:172
void resetCostToComeFromGoal()
Resets the cost to come to this vertex from the goal to infinity.
Definition Vertex.cpp:192
void resetForwardParent()
Resets the forward parent of the vertex.
Definition Vertex.cpp:278
Main namespace. Contains everything in this library.