Loading...
Searching...
No Matches
Vertex.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019-present 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
52namespace ompl
53{
54 namespace geometric
55 {
56 namespace aitstar
57 {
58 class Vertex
59 {
60 public:
62 Vertex(const ompl::base::SpaceInformationPtr &spaceInformation,
63 const ompl::base::ProblemDefinitionPtr &problemDefinition,
64 const std::size_t &batchId);
65
67 virtual ~Vertex();
68
70 std::size_t getId() const;
71
74
76 ompl::base::State const *getState() const;
77
80
83
86
89
92
95
97 bool hasForwardParent() const;
98
100 void setForwardParent(const std::shared_ptr<Vertex> &vertex, const ompl::base::Cost &edgeCost);
101
103 void resetForwardParent();
104
106 bool hasReverseParent() const;
107
109 void setReverseParent(const std::shared_ptr<Vertex> &vertex);
110
112 void resetReverseParent();
113
115 std::shared_ptr<Vertex> getForwardParent() const;
116
118 std::shared_ptr<Vertex> getReverseParent() const;
119
121 void setForwardEdgeCost(const ompl::base::Cost &cost);
122
125
127 void setCostToComeFromGoal(const ompl::base::Cost &cost);
128
131
133 void setCostToGoToGoal(const ompl::base::Cost &cost);
134
136 void updateCostOfForwardBranch() const;
137
139 std::vector<std::weak_ptr<aitstar::Vertex>> invalidateReverseBranch();
140
142 std::vector<std::weak_ptr<aitstar::Vertex>> invalidateForwardBranch();
143
145 void addToForwardChildren(const std::shared_ptr<Vertex> &vertex);
146
148 void removeFromForwardChildren(std::size_t vertexId);
149
151 std::vector<std::shared_ptr<Vertex>> getForwardChildren() const;
152
154 void addToReverseChildren(const std::shared_ptr<Vertex> &vertex);
155
157 void removeFromReverseChildren(std::size_t vertexId);
158
160 std::vector<std::shared_ptr<Vertex>> getReverseChildren() const;
161
163 void whitelistAsChild(const std::shared_ptr<Vertex> &vertex) const;
164
166 bool isWhitelistedAsChild(const std::shared_ptr<Vertex> &vertex) const;
167
169 void blacklistAsChild(const std::shared_ptr<Vertex> &vertex) const;
170
172 bool isBlacklistedAsChild(const std::shared_ptr<Vertex> &vertex) const;
173
175 bool hasCachedNeighbors() const;
176
178 void cacheNeighbors(const std::vector<std::shared_ptr<Vertex>> &neighbors) const;
179
181 const std::vector<std::shared_ptr<Vertex>> &getNeighbors() const;
182
185
188
192
196
200
203
207
210 typename ompl::BinaryHeap<
211 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
212 std::function<bool(const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
213 const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>
214 &)>>::Element *pointer);
215
217 typename ompl::BinaryHeap<
218 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
219 std::function<bool(const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
220 const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
221 Element *
223
226
229 typename ompl::BinaryHeap<
230 aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element
231 *pointer);
232
235 typename ompl::BinaryHeap<
236 aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element
237 *pointer);
238
240 std::vector<ompl::BinaryHeap<
241 aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *>
243
245 std::vector<ompl::BinaryHeap<
246 aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *>
248
252 std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element
253 *element);
254
258 std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element
259 *element);
260
263
266
267 private:
269 const ompl::base::SpaceInformationPtr spaceInformation_;
270
272 const ompl::base::ProblemDefinitionPtr problemDefinition_;
273
276
278 std::vector<std::weak_ptr<Vertex>> forwardChildren_{};
279
281 std::vector<std::weak_ptr<Vertex>> reverseChildren_{};
282
284 mutable std::vector<std::shared_ptr<Vertex>> neighbors_{};
285
287 mutable std::vector<std::weak_ptr<Vertex>> whitelistedChildren_{};
288
290 mutable std::vector<std::weak_ptr<Vertex>> blacklistedChildren_{};
291
293 std::weak_ptr<Vertex> forwardParent_;
294
296 std::weak_ptr<Vertex> reverseParent_;
297
299 ompl::base::State *state_;
300
302 ompl::base::Cost costToComeFromStart_;
303
305 ompl::base::Cost edgeCostFromForwardParent_;
306
308 mutable ompl::base::Cost costToComeFromGoal_;
309
311 mutable ompl::base::Cost expandedCostToComeFromGoal_;
312
314 mutable ompl::base::Cost costToGoToGoal_;
315
317 const std::size_t vertexId_;
318
320 const std::size_t& batchId_;
321
323 mutable std::size_t neighborBatchId_{0u};
324
326 mutable std::size_t reverseSearchBatchId_{0u};
327
330 mutable std::size_t poppedOutgoingEdgeId_{0u};
331
333 mutable std::size_t expandedReverseSearchId_{0u};
334
336 mutable std::size_t insertedIntoQueueId_{0u};
337
339 mutable std::size_t reverseQueuePointerId_{0u};
340
342 using ReverseQueueElement = typename ompl::BinaryHeap<
343 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
344 std::function<bool(const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
345 const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
346 Element;
347
349 mutable ReverseQueueElement *reverseQueuePointer_{nullptr};
350
352 using ForwardQueueElement = typename ompl::BinaryHeap<
353 aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element;
354
356 mutable std::vector<ForwardQueueElement *> forwardQueueIncomingLookup_;
357
359 mutable std::vector<ForwardQueueElement *> forwardQueueOutgoingLookup_;
360 };
361
362 } // namespace aitstar
363
364 } // namespace geometric
365
366} // namespace ompl
367
368#endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_VERTEX_
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition BinaryHeap.h:53
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:393
std::vector< std::weak_ptr< aitstar::Vertex > > invalidateForwardBranch()
Recursively invalidates the branch of the forward tree rooted in this vertex.
Definition Vertex.cpp:221
void addToForwardQueueOutgoingLookup(typename ompl::BinaryHeap< aitstar::Edge, std::function< bool(const aitstar::Edge &, const aitstar::Edge &)> >::Element *pointer)
Adds an element to the forward queue outgoing lookup.
Definition Vertex.cpp:478
ompl::base::State * getState()
Provides write access to the underlying state.
Definition Vertex.cpp:93
void resetReverseParent()
Resets the reverse parent of the vertex.
Definition Vertex.cpp:274
bool isWhitelistedAsChild(const std::shared_ptr< Vertex > &vertex) const
Returns whether a child is whitelisted.
Definition Vertex.cpp:332
void blacklistAsChild(const std::shared_ptr< Vertex > &vertex) const
Blacklists a child.
Definition Vertex.cpp:344
ompl::base::Cost getExpandedCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal when it was expanded.
Definition Vertex.cpp:122
std::vector< ompl::BinaryHeap< aitstar::Edge, std::function< bool(const aitstar::Edge &, const aitstar::Edge &)> >::Element * getForwardQueueOutgoingLookup)() const
Returns the forward queue outgoing lookup of this vertex.
void resetForwardQueueOutgoingLookup()
Resets the forward queue outgoing lookup.
Definition Vertex.cpp:520
void registerPoppedOutgoingEdgeDuringForwardSearch()
Registers that a child has been added to this vertex during the current forward search.
Definition Vertex.cpp:405
bool hasHadOutgoingEdgePoppedDuringCurrentForwardSearch() const
Returns whether the vertex has had an outgoing edge popped during the current forward search.
Definition Vertex.cpp:426
const std::vector< std::shared_ptr< Vertex > > & getNeighbors() const
Returns the nearest neighbors, throws if not up to date.
Definition Vertex.cpp:372
void removeFromForwardChildren(std::size_t vertexId)
Removes a vertex from this vertex's forward children.
Definition Vertex.cpp:284
bool hasCachedNeighbors() const
Returns whether the vertex knows its nearest neighbors on the current approximation.
Definition Vertex.cpp:361
bool hasBeenExpandedDuringCurrentReverseSearch() const
Returns whether the vertex has been expanded during the current reverse search.
Definition Vertex.cpp:431
virtual ~Vertex()
Destructs the vertex.
Definition Vertex.cpp:82
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:239
void resetReverseQueuePointer()
Resets the reverse queue pointer.
Definition Vertex.cpp:466
void setCostToComeFromGoal(const ompl::base::Cost &cost)
Sets the cost to come to this vertex from the goal.
Definition Vertex.cpp:174
ompl::base::Cost getCostToComeFromStart() const
Returns the cost to come to this vertex from the start.
Definition Vertex.cpp:108
void addToReverseChildren(const std::shared_ptr< Vertex > &vertex)
Adds a vertex this vertex's children.
Definition Vertex.cpp:303
ompl::base::Cost getCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal.
Definition Vertex.cpp:113
ompl::base::ScopedState getScopedState() const
Returns a scoped copy of the underlying state.
Definition Vertex.cpp:103
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:180
bool hasForwardParent() const
Returns whether this vertex has a parent in the forward search.
Definition Vertex.cpp:141
bool isBlacklistedAsChild(const std::shared_ptr< Vertex > &vertex) const
Returns whether a child is blacklisted.
Definition Vertex.cpp:349
void resetForwardQueueIncomingLookup()
Resets the forward queue incoming lookup.
Definition Vertex.cpp:515
std::size_t getId() const
Get the unique id of this vertex.
Definition Vertex.cpp:88
void addToForwardChildren(const std::shared_ptr< Vertex > &vertex)
Adds a vertex to this vertex's forward children.
Definition Vertex.cpp:279
void updateCostOfForwardBranch() const
Updates the cost to the whole branch rooted at this vertex.
Definition Vertex.cpp:190
void removeFromForwardQueueOutgoingLookup(ompl::BinaryHeap< aitstar::Edge, std::function< bool(const aitstar::Edge &, const aitstar::Edge &)> >::Element *element)
Remove an element from the outgoing queue lookup.
Definition Vertex.cpp:507
void addToForwardQueueIncomingLookup(typename ompl::BinaryHeap< aitstar::Edge, std::function< bool(const aitstar::Edge &, const aitstar::Edge &)> >::Element *pointer)
Adds an element to the forward queue incoming lookup.
Definition Vertex.cpp:471
void setForwardEdgeCost(const ompl::base::Cost &cost)
Sets the cost to come to this vertex.
Definition Vertex.cpp:164
ompl::BinaryHeap< std::pair< std::array< ompl::base::Cost, 2u >, std::shared_ptr< Vertex > >, std::function< bool(conststd::pair< std::array< ompl::base::Cost, 2u >, std::shared_ptr< Vertex > > &, conststd::pair< std::array< ompl::base::Cost, 2u >, std::shared_ptr< Vertex > > &)> >::Element getReverseQueuePointer)() const
Returns the reverse queue pointer of this vertex.
ompl::base::Cost getCostToGoToGoal() const
Returns the cost to go heuristic from this vertex.
Definition Vertex.cpp:131
void setReverseParent(const std::shared_ptr< Vertex > &vertex)
Sets the parent vertex (in the reverse-search tree).
Definition Vertex.cpp:262
void removeFromForwardQueueIncomingLookup(ompl::BinaryHeap< aitstar::Edge, std::function< bool(const aitstar::Edge &, const aitstar::Edge &)> >::Element *element)
Remove an element from the incoming queue lookup.
Definition Vertex.cpp:499
void unregisterExpansionDuringReverseSearch()
Unregisters the expansion of this vertex during the current reverse search, needed when a reverse bra...
Definition Vertex.cpp:416
void removeFromReverseChildren(std::size_t vertexId)
Removes a vertex from this vertex's forward children.
Definition Vertex.cpp:308
void cacheNeighbors(const std::vector< std::shared_ptr< Vertex > > &neighbors) const
Caches the neighbors for the current approximation.
Definition Vertex.cpp:366
std::vector< ompl::BinaryHeap< aitstar::Edge, std::function< bool(const aitstar::Edge &, const aitstar::Edge &)> >::Element * getForwardQueueIncomingLookup)() const
Returns the forward queue incoming lookup of this vertex.
void whitelistAsChild(const std::shared_ptr< Vertex > &vertex) const
Whitelists a child.
Definition Vertex.cpp:327
bool hasReverseParent() const
Returns whether this vertex has a parent in the reverse search.
Definition Vertex.cpp:154
void registerExpansionDuringReverseSearch()
Registers the expansion of this vertex during the current reverse search.
Definition Vertex.cpp:410
std::vector< std::weak_ptr< aitstar::Vertex > > invalidateReverseBranch()
Recursively invalidates the branch of the reverse tree rooted in this vertex.
Definition Vertex.cpp:201
void setCostToComeFromStart(const ompl::base::Cost &cost)
Sets the cost to come to this vertex.
Definition Vertex.cpp:169
void setCostToGoToGoal(const ompl::base::Cost &cost)
Sets the cost to go heuristic of this vertex.
Definition Vertex.cpp:185
std::shared_ptr< Vertex > getForwardParent() const
Returns the parent of the vertex (in the forward-search tree).
Definition Vertex.cpp:149
ompl::base::Cost getEdgeCostFromForwardParent() const
Returns the edge cost from the forward parent.
Definition Vertex.cpp:136
void registerInsertionIntoQueueDuringReverseSearch()
Registers the insertion of this vertex into the open queue during the current reverse search.
Definition Vertex.cpp:421
std::vector< std::shared_ptr< Vertex > > getForwardChildren() const
Returns this vertex's children in the forward search tree.
Definition Vertex.cpp:382
std::shared_ptr< Vertex > getReverseParent() const
Returns the parent of the vertex (in the reverse-search tree).
Definition Vertex.cpp:159
bool hasBeenInsertedIntoQueueDuringCurrentReverseSearch() const
Returns whether the vertex has been inserted into the queue during the current reverse search.
Definition Vertex.cpp:436
void setReverseQueuePointer(typename ompl::BinaryHeap< std::pair< std::array< ompl::base::Cost, 2u >, std::shared_ptr< Vertex > >, std::function< bool(const std::pair< std::array< ompl::base::Cost, 2u >, std::shared_ptr< Vertex > > &, const std::pair< std::array< ompl::base::Cost, 2u >, std::shared_ptr< Vertex > > &)> >::Element *pointer)
Sets the reverse queue pointer of this vertex.
Definition Vertex.cpp:441
void resetForwardParent()
Resets the forward parent of the vertex.
Definition Vertex.cpp:257
Main namespace. Contains everything in this library.