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 
52 namespace 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 
124  void setCostToComeFromStart(const ompl::base::Cost &cost);
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 *
222  getReverseQueuePointer() const;
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 
275  const ompl::base::OptimizationObjectivePtr objective_;
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_
bool isWhitelistedAsChild(const std::shared_ptr< Vertex > &vertex) const
Returns whether a child is whitelisted.
Definition: Vertex.cpp:332
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:131
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
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.
Definition: Vertex.cpp:487
virtual ~Vertex()
Destructs the vertex.
Definition: Vertex.cpp:82
Definition of an abstract state.
Definition: State.h:114
void addToReverseChildren(const std::shared_ptr< Vertex > &vertex)
Adds a vertex this vertex's children.
Definition: Vertex.cpp:303
void setReverseParent(const std::shared_ptr< Vertex > &vertex)
Sets the parent vertex (in the reverse-search tree).
Definition: Vertex.cpp:262
bool isBlacklistedAsChild(const std::shared_ptr< Vertex > &vertex) const
Returns whether a child is blacklisted.
Definition: Vertex.cpp:349
ompl::base::State * getState()
Provides write access to the underlying state.
Definition: Vertex.cpp:93
void resetForwardQueueOutgoingLookup()
Resets the forward queue outgoing lookup.
Definition: Vertex.cpp:520
void removeFromReverseChildren(std::size_t vertexId)
Removes a vertex from this vertex's forward children.
Definition: Vertex.cpp:308
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
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 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
A shared pointer wrapper for ompl::base::OptimizationObjective.
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.
Definition: Vertex.cpp:494
std::size_t getId() const
Get the unique id of this vertex.
Definition: Vertex.cpp:88
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 resetForwardQueueIncomingLookup()
Resets the forward queue incoming lookup.
Definition: Vertex.cpp:515
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
ompl::base::Cost getExpandedCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal when it was expanded.
Definition: Vertex.cpp:122
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition: BinaryHeap.h:85
void registerExpansionDuringReverseSearch()
Registers the expansion of this vertex during the current reverse search.
Definition: Vertex.cpp:410
void resetReverseParent()
Resets the reverse parent of the vertex.
Definition: Vertex.cpp:274
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 setCostToComeFromGoal(const ompl::base::Cost &cost)
Sets the cost to come to this vertex from the goal.
Definition: Vertex.cpp:174
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
std::shared_ptr< Vertex > getForwardParent() const
Returns the parent of the vertex (in the forward-search tree).
Definition: Vertex.cpp:149
void registerPoppedOutgoingEdgeDuringForwardSearch()
Registers that a child has been added to this vertex during the current forward search.
Definition: Vertex.cpp:405
void unregisterExpansionDuringReverseSearch()
Unregisters the expansion of this vertex during the current reverse search, needed when a reverse bra...
Definition: Vertex.cpp:416
ompl::base::Cost getCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal.
Definition: Vertex.cpp:113
A shared pointer wrapper for ompl::base::ProblemDefinition.
void resetReverseQueuePointer()
Resets the reverse queue pointer.
Definition: Vertex.cpp:466
void setCostToComeFromStart(const ompl::base::Cost &cost)
Sets the cost to come to this vertex.
Definition: Vertex.cpp:169
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 setForwardEdgeCost(const ompl::base::Cost &cost)
Sets the cost to come to this vertex.
Definition: Vertex.cpp:164
bool hasBeenInsertedIntoQueueDuringCurrentReverseSearch() const
Returns whether the vertex has been inserted into the queue during the current reverse search.
Definition: Vertex.cpp:436
void updateCostOfForwardBranch() const
Updates the cost to the whole branch rooted at this vertex.
Definition: Vertex.cpp:190
std::vector< std::shared_ptr< Vertex > > getForwardChildren() const
Returns this vertex's children in the forward search tree.
Definition: Vertex.cpp:382
bool hasCachedNeighbors() const
Returns whether the vertex knows its nearest neighbors on the current approximation.
Definition: Vertex.cpp:361
bool hasReverseParent() const
Returns whether this vertex has a parent in the reverse search.
Definition: Vertex.cpp:154
void setCostToGoToGoal(const ompl::base::Cost &cost)
Sets the cost to go heuristic of this vertex.
Definition: Vertex.cpp:185
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:393
void cacheNeighbors(const std::vector< std::shared_ptr< Vertex >> &neighbors) const
Caches the neighbors for the current approximation.
Definition: Vertex.cpp:366
ompl::base::Cost getEdgeCostFromForwardParent() const
Returns the edge cost from the forward parent.
Definition: Vertex.cpp:136
bool hasHadOutgoingEdgePoppedDuringCurrentForwardSearch() const
Returns whether the vertex has had an outgoing edge popped during the current forward search.
Definition: Vertex.cpp:426
ompl::base::ScopedState getScopedState() const
Returns a scoped copy of the underlying state.
Definition: Vertex.cpp:103
bool hasBeenExpandedDuringCurrentReverseSearch() const
Returns whether the vertex has been expanded during the current reverse search.
Definition: Vertex.cpp:431
void blacklistAsChild(const std::shared_ptr< Vertex > &vertex) const
Blacklists a child.
Definition: Vertex.cpp:344
std::shared_ptr< Vertex > getReverseParent() const
Returns the parent of the vertex (in the reverse-search tree).
Definition: Vertex.cpp:159
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 * getReverseQueuePointer() const
Returns the reverse queue pointer of this vertex.
void removeFromForwardChildren(std::size_t vertexId)
Removes a vertex from this vertex's forward children.
Definition: Vertex.cpp:284
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
void resetForwardParent()
Resets the forward parent of the vertex.
Definition: Vertex.cpp:257
void addToForwardChildren(const std::shared_ptr< Vertex > &vertex)
Adds a vertex to this vertex's forward children.
Definition: Vertex.cpp:279
Definition of a scoped state.
Definition: ScopedState.h:121
bool hasForwardParent() const
Returns whether this vertex has a parent in the forward search.
Definition: Vertex.cpp:141
void whitelistAsChild(const std::shared_ptr< Vertex > &vertex) const
Whitelists a child.
Definition: Vertex.cpp:327
const std::vector< std::shared_ptr< Vertex > > & getNeighbors() const
Returns the nearest neighbors, throws if not up to date.
Definition: Vertex.cpp:372
void registerInsertionIntoQueueDuringReverseSearch()
Registers the insertion of this vertex into the open queue during the current reverse search.
Definition: Vertex.cpp:421
ompl::base::Cost getCostToComeFromStart() const
Returns the cost to come to this vertex from the start.
Definition: Vertex.cpp:108
Main namespace. Contains everything in this library.
Definition: AppBase.h:22