Vertex.cpp
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 #include "ompl/geometric/planners/informedtrees/aitstar/Vertex.h"
37 
38 #include <algorithm>
39 #include <atomic>
40 #include <cmath>
41 #include <string>
42 
43 #include "ompl/base/goals/GoalState.h"
44 #include "ompl/base/goals/GoalStates.h"
45 
46 using namespace std::string_literals;
47 
48 namespace ompl
49 {
50  namespace geometric
51  {
52  namespace aitstar
53  {
54  namespace
55  {
56  std::size_t generateId()
57  {
58  static std::atomic<std::size_t> id{0u};
59  return id++;
60  }
61  } // namespace
62 
63  Vertex::Vertex(const ompl::base::SpaceInformationPtr &spaceInformation,
64  const ompl::base::ProblemDefinitionPtr &problemDefinition,
65  const std::size_t &batchId)
66  : spaceInformation_(spaceInformation)
67  , problemDefinition_(problemDefinition)
68  , objective_(problemDefinition->getOptimizationObjective())
69  , forwardChildren_()
70  , forwardParent_()
71  , state_(spaceInformation->allocState()) // The memory allocated here is freed in the destructor.
72  , costToComeFromStart_(objective_->infiniteCost())
73  , edgeCostFromForwardParent_(objective_->infiniteCost())
74  , costToComeFromGoal_(objective_->infiniteCost())
75  , expandedCostToComeFromGoal_(objective_->infiniteCost())
76  , costToGoToGoal_(objective_->infiniteCost())
77  , vertexId_(generateId())
78  , batchId_(batchId)
79  {
80  }
81 
83  {
84  // The state has associated memory that needs to be freed manually.
85  spaceInformation_->freeState(state_);
86  };
87 
88  std::size_t Vertex::getId() const
89  {
90  return vertexId_;
91  }
92 
94  {
95  return state_;
96  }
97 
99  {
100  return state_;
101  }
102 
104  {
105  return ompl::base::ScopedState<>(spaceInformation_->getStateSpace(), state_);
106  }
107 
109  {
110  return costToComeFromStart_;
111  }
112 
114  {
115  if (reverseSearchBatchId_ != batchId_)
116  {
117  costToComeFromGoal_ = objective_->infiniteCost();
118  }
119  return costToComeFromGoal_;
120  }
121 
123  {
124  if (expandedReverseSearchId_ != batchId_)
125  {
126  expandedCostToComeFromGoal_ = objective_->infiniteCost();
127  }
128  return expandedCostToComeFromGoal_;
129  }
130 
132  {
133  return getCostToComeFromGoal();
134  }
135 
137  {
138  return edgeCostFromForwardParent_;
139  }
140 
142  {
143  // See https://stackoverflow.com/questions/45507041/how-to-check-if-weak-ptr-is-empty-non-assigned.
144  // return parent_.owner_before(std::weak_ptr<Vertex>{}) &&
145  // std::weak_ptr<Vertex>{}.owner_before(parent_);
146  return static_cast<bool>(forwardParent_.lock());
147  }
148 
149  std::shared_ptr<Vertex> Vertex::getForwardParent() const
150  {
151  return forwardParent_.lock();
152  }
153 
155  {
156  return static_cast<bool>(reverseParent_.lock());
157  }
158 
159  std::shared_ptr<Vertex> Vertex::getReverseParent() const
160  {
161  return reverseParent_.lock();
162  }
163 
165  {
166  edgeCostFromForwardParent_ = cost;
167  }
168 
170  {
171  costToComeFromStart_ = cost;
172  }
173 
175  {
176  reverseSearchBatchId_ = batchId_;
177  costToComeFromGoal_ = cost;
178  }
179 
181  {
182  expandedCostToComeFromGoal_ = cost;
183  }
184 
186  {
187  costToGoToGoal_ = cost;
188  }
189 
191  {
192  // Update the cost of all forward children.
193  for (const auto &child : getForwardChildren())
194  {
195  child->setCostToComeFromStart(objective_->combineCosts(
196  costToComeFromStart_, child->getEdgeCostFromForwardParent()));
197  child->updateCostOfForwardBranch();
198  }
199  }
200 
201  std::vector<std::weak_ptr<aitstar::Vertex>> Vertex::invalidateReverseBranch()
202  {
203  std::vector<std::weak_ptr<aitstar::Vertex>> accumulatedChildren = reverseChildren_;
204 
205  // Remove all children.
206  for (const auto &child : reverseChildren_)
207  {
208  child.lock()->setCostToComeFromGoal(objective_->infiniteCost());
209  child.lock()->setExpandedCostToComeFromGoal(objective_->infiniteCost());
210  child.lock()->resetReverseParent();
211  auto childsAccumulatedChildren = child.lock()->invalidateReverseBranch();
212  accumulatedChildren.insert(accumulatedChildren.end(), childsAccumulatedChildren.begin(),
213  childsAccumulatedChildren.end());
214  }
215  reverseChildren_.clear();
216 
218  return accumulatedChildren;
219  }
220 
221  std::vector<std::weak_ptr<aitstar::Vertex>> Vertex::invalidateForwardBranch()
222  {
223  std::vector<std::weak_ptr<aitstar::Vertex>> accumulatedChildren = forwardChildren_;
224 
225  // Remove all children.
226  for (const auto &child : forwardChildren_)
227  {
228  child.lock()->setCostToComeFromGoal(objective_->infiniteCost());
229  child.lock()->resetForwardParent();
230  auto childsAccumulatedChildren = child.lock()->invalidateForwardBranch();
231  accumulatedChildren.insert(accumulatedChildren.end(), childsAccumulatedChildren.begin(),
232  childsAccumulatedChildren.end());
233  }
234  forwardChildren_.clear();
235 
236  return accumulatedChildren;
237  }
238 
239  void Vertex::setForwardParent(const std::shared_ptr<Vertex> &vertex, const ompl::base::Cost &edgeCost)
240  {
241  // If this is a rewiring, remove from my parent's children.
242  if (static_cast<bool>(forwardParent_.lock()))
243  {
244  forwardParent_.lock()->removeFromForwardChildren(vertexId_);
245  }
246 
247  // Remember the edge cost.
248  edgeCostFromForwardParent_ = edgeCost;
249 
250  // Remember the corresponding parent.
251  forwardParent_ = std::weak_ptr<Vertex>(vertex);
252 
253  // Update the cost to come.
254  costToComeFromStart_ = objective_->combineCosts(vertex->getCostToComeFromStart(), edgeCost);
255  }
256 
258  {
259  forwardParent_.reset();
260  }
261 
262  void Vertex::setReverseParent(const std::shared_ptr<Vertex> &vertex)
263  {
264  // If this is a rewiring, remove from my parent's children.
265  if (static_cast<bool>(reverseParent_.lock()))
266  {
267  reverseParent_.lock()->removeFromReverseChildren(vertexId_);
268  }
269 
270  // Remember the parent.
271  reverseParent_ = std::weak_ptr<Vertex>(vertex);
272  }
273 
275  {
276  reverseParent_.reset();
277  }
278 
279  void Vertex::addToForwardChildren(const std::shared_ptr<Vertex> &vertex)
280  {
281  forwardChildren_.emplace_back(vertex);
282  }
283 
284  void Vertex::removeFromForwardChildren(std::size_t vertexId)
285  {
286  // Find the child.
287  auto it = std::find_if(
288  forwardChildren_.begin(), forwardChildren_.end(),
289  [vertexId](const std::weak_ptr<Vertex> &child) { return vertexId == child.lock()->getId(); });
290 
291  // Throw if it is not found.
292  if (it == forwardChildren_.end())
293  {
294  auto msg = "Asked to remove vertex from forward children that is currently not a child."s;
295  throw ompl::Exception(msg);
296  }
297 
298  // Swap and pop.
299  std::iter_swap(it, forwardChildren_.rbegin());
300  forwardChildren_.pop_back();
301  }
302 
303  void Vertex::addToReverseChildren(const std::shared_ptr<Vertex> &vertex)
304  {
305  reverseChildren_.push_back(vertex);
306  }
307 
308  void Vertex::removeFromReverseChildren(std::size_t vertexId)
309  {
310  // Find the child.
311  auto it = std::find_if(
312  reverseChildren_.begin(), reverseChildren_.end(),
313  [vertexId](const std::weak_ptr<Vertex> &child) { return vertexId == child.lock()->getId(); });
314 
315  // Throw if it is not found.
316  if (it == reverseChildren_.end())
317  {
318  auto msg = "Asked to remove vertex from reverse children that is currently not a child."s;
319  throw ompl::Exception(msg);
320  }
321 
322  // Swap and pop.
323  std::iter_swap(it, reverseChildren_.rbegin());
324  reverseChildren_.pop_back();
325  }
326 
327  void Vertex::whitelistAsChild(const std::shared_ptr<Vertex> &vertex) const
328  {
329  whitelistedChildren_.emplace_back(vertex);
330  }
331 
332  bool Vertex::isWhitelistedAsChild(const std::shared_ptr<Vertex> &vertex) const
333  {
334  for (const auto &whitelistedChild : whitelistedChildren_)
335  {
336  if (whitelistedChild.lock()->getId() == vertex->getId())
337  {
338  return true;
339  }
340  }
341  return false;
342  }
343 
344  void Vertex::blacklistAsChild(const std::shared_ptr<Vertex> &vertex) const
345  {
346  blacklistedChildren_.emplace_back(vertex);
347  }
348 
349  bool Vertex::isBlacklistedAsChild(const std::shared_ptr<Vertex> &vertex) const
350  {
351  for (const auto &blacklistedChild : blacklistedChildren_)
352  {
353  if (blacklistedChild.lock()->getId() == vertex->getId())
354  {
355  return true;
356  }
357  }
358  return false;
359  }
360 
362  {
363  return neighborBatchId_ == batchId_;
364  }
365 
366  void Vertex::cacheNeighbors(const std::vector<std::shared_ptr<Vertex>> &neighbors) const
367  {
368  neighbors_ = neighbors;
369  neighborBatchId_ = batchId_;
370  }
371 
372  const std::vector<std::shared_ptr<Vertex>> &Vertex::getNeighbors() const
373  {
374  if (neighborBatchId_ != batchId_)
375  {
376  throw ompl::Exception("Requested neighbors from vertex of outdated approximation.");
377  }
378 
379  return neighbors_;
380  }
381 
382  std::vector<std::shared_ptr<Vertex>> Vertex::getForwardChildren() const
383  {
384  std::vector<std::shared_ptr<Vertex>> children;
385  for (const auto &child : forwardChildren_)
386  {
387  assert(!child.expired());
388  children.emplace_back(child.lock());
389  }
390  return children;
391  }
392 
393  std::vector<std::shared_ptr<Vertex>> Vertex::getReverseChildren() const
394  {
395  std::vector<std::shared_ptr<Vertex>> children;
396  children.reserve(reverseChildren_.size());
397  for (const auto &child : reverseChildren_)
398  {
399  assert(!child.expired());
400  children.emplace_back(child.lock());
401  }
402  return children;
403  }
404 
406  {
407  poppedOutgoingEdgeId_ = batchId_;
408  }
409 
411  {
412  expandedCostToComeFromGoal_ = costToComeFromGoal_;
413  expandedReverseSearchId_ = batchId_;
414  }
415 
417  {
418  expandedReverseSearchId_ = 0u;
419  }
420 
422  {
423  insertedIntoQueueId_ = batchId_;
424  }
425 
427  {
428  return poppedOutgoingEdgeId_ == batchId_;
429  }
430 
432  {
433  return expandedReverseSearchId_ == batchId_;
434  }
435 
437  {
438  return insertedIntoQueueId_ == batchId_;
439  }
440 
442  typename ompl::BinaryHeap<
443  std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
444  std::function<bool(const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
445  const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
446  Element *pointer)
447  {
448  reverseQueuePointerId_ = batchId_;
449  reverseQueuePointer_ = pointer;
450  }
451 
452  typename ompl::BinaryHeap<
453  std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
454  std::function<bool(const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
455  const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
456  Element *
458  {
459  if (batchId_ != reverseQueuePointerId_)
460  {
461  reverseQueuePointer_ = nullptr;
462  }
463  return reverseQueuePointer_;
464  }
465 
467  {
468  reverseQueuePointer_ = nullptr;
469  }
470 
472  typename ompl::BinaryHeap<
473  aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *pointer)
474  {
475  forwardQueueIncomingLookup_.emplace_back(pointer);
476  }
477 
479  typename ompl::BinaryHeap<
480  aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *pointer)
481  {
482  forwardQueueOutgoingLookup_.emplace_back(pointer);
483  }
484 
485  typename std::vector<ompl::BinaryHeap<
486  aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *>
488  {
489  return forwardQueueIncomingLookup_;
490  }
491 
492  typename std::vector<ompl::BinaryHeap<
493  aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *>
495  {
496  return forwardQueueOutgoingLookup_;
497  }
498 
501  std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *element)
502  {
503  forwardQueueIncomingLookup_.erase(
504  std::remove(forwardQueueIncomingLookup_.begin(), forwardQueueIncomingLookup_.end(), element));
505  }
506 
509  std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *element)
510  {
511  forwardQueueOutgoingLookup_.erase(
512  std::remove(forwardQueueOutgoingLookup_.begin(), forwardQueueOutgoingLookup_.end(), element));
513  }
514 
516  {
517  forwardQueueIncomingLookup_.clear();
518  }
519 
521  {
522  forwardQueueOutgoingLookup_.clear();
523  }
524 
525  } // namespace aitstar
526 
527  } // namespace geometric
528 
529 } // namespace ompl
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
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
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
The exception type for ompl.
Definition: Exception.h:79
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