Vertex.cpp
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 #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, const std::size_t &batchId)
65  : spaceInformation_(spaceInformation)
66  , problemDefinition_(problemDefinition)
67  , objective_(problemDefinition->getOptimizationObjective())
68  , forwardChildren_()
69  , reverseChildren_()
70  , forwardParent_()
71  , reverseParent_()
72  , state_(spaceInformation->allocState()) // The memory allocated here is freed in the destructor.
73  , costToComeFromStart_(objective_->infiniteCost())
74  , edgeCostFromForwardParent_(objective_->infiniteCost())
75  , costToComeFromGoal_(objective_->infiniteCost())
76  , expandedCostToComeFromGoal_(objective_->infiniteCost())
77  , costToGoToGoal_(objective_->infiniteCost())
78  , vertexId_(generateId())
79  , batchId_(batchId)
80  {
81  }
82 
83  Vertex::Vertex(const std::shared_ptr<Vertex> &other)
84  : spaceInformation_(other->spaceInformation_)
85  , problemDefinition_(other->problemDefinition_)
86  , objective_(other->objective_)
87  , forwardChildren_(other->forwardChildren_)
88  , reverseChildren_(other->reverseChildren_)
89  , forwardParent_(other->forwardParent_)
90  , reverseParent_(other->reverseParent_)
91  , state_(spaceInformation_->allocState()) // The memory allocated here is freed in the destructor.
92  , costToComeFromStart_(other->costToComeFromStart_)
93  , edgeCostFromForwardParent_(other->edgeCostFromForwardParent_)
94  , costToComeFromGoal_(other->costToComeFromGoal_)
95  , expandedCostToComeFromGoal_(other->expandedCostToComeFromGoal_)
96  , costToGoToGoal_(other->costToGoToGoal_)
97  , vertexId_(other->vertexId_)
98  , batchId_(other->batchId_)
99  {
100  spaceInformation_->copyState(state_, other->getState());
101  }
102 
104  {
105  // The state has associated memory that needs to be freed manually.
106  spaceInformation_->freeState(state_);
107  };
108 
109  std::size_t Vertex::getId() const
110  {
111  return vertexId_;
112  }
113 
115  {
116  return state_;
117  }
118 
119  ompl::base::State const *Vertex::getState() const
120  {
121  return state_;
122  }
123 
125  {
126  return ompl::base::ScopedState<>(spaceInformation_->getStateSpace(), state_);
127  }
128 
130  {
131  return costToComeFromStart_;
132  }
133 
135  {
136  return costToComeFromGoal_;
137  }
138 
140  {
141  return expandedCostToComeFromGoal_;
142  }
143 
145  {
146  return getCostToComeFromGoal();
147  }
148 
150  {
151  return edgeCostFromForwardParent_;
152  }
153 
155  {
156  // See https://stackoverflow.com/questions/45507041/how-to-check-if-weak-ptr-is-empty-non-assigned.
157  // return parent_.owner_before(std::weak_ptr<Vertex>{}) &&
158  // std::weak_ptr<Vertex>{}.owner_before(parent_);
159  return static_cast<bool>(forwardParent_.lock());
160  }
161 
162  std::shared_ptr<Vertex> Vertex::getForwardParent() const
163  {
164  return forwardParent_.lock();
165  }
166 
168  {
169  return static_cast<bool>(reverseParent_.lock());
170  }
171 
172  std::shared_ptr<Vertex> Vertex::getReverseParent() const
173  {
174  return reverseParent_.lock();
175  }
176 
178  {
179  edgeCostFromForwardParent_ = cost;
180  }
181 
183  {
184  costToComeFromStart_ = cost;
185  }
186 
188  {
189  costToComeFromGoal_ = cost;
190  }
191 
193  {
194  costToComeFromGoal_ = objective_->infiniteCost();
195  }
196 
198  {
199  expandedCostToComeFromGoal_ = objective_->infiniteCost();
200  }
201 
203  {
204  expandedCostToComeFromGoal_ = cost;
205  }
206 
208  {
209  costToGoToGoal_ = cost;
210  }
211 
213  {
214  // Update the cost of all forward children.
215  for (const auto &child : getForwardChildren())
216  {
217  child->setCostToComeFromStart(
218  objective_->combineCosts(costToComeFromStart_, child->getEdgeCostFromForwardParent()));
219  child->updateCostOfForwardBranch();
220  }
221  }
222 
223  std::vector<std::weak_ptr<aitstar::Vertex>> Vertex::invalidateReverseBranch()
224  {
225  std::vector<std::weak_ptr<aitstar::Vertex>> accumulatedChildren = reverseChildren_;
226 
227  // Remove all children.
228  for (const auto &child : reverseChildren_)
229  {
230  child.lock()->setCostToComeFromGoal(objective_->infiniteCost());
231  child.lock()->setExpandedCostToComeFromGoal(objective_->infiniteCost());
232  child.lock()->resetReverseParent();
233  auto childsAccumulatedChildren = child.lock()->invalidateReverseBranch();
234  accumulatedChildren.insert(accumulatedChildren.end(), childsAccumulatedChildren.begin(),
235  childsAccumulatedChildren.end());
236  }
237  reverseChildren_.clear();
238 
239  return accumulatedChildren;
240  }
241 
242  std::vector<std::weak_ptr<aitstar::Vertex>> Vertex::invalidateForwardBranch()
243  {
244  std::vector<std::weak_ptr<aitstar::Vertex>> accumulatedChildren = forwardChildren_;
245 
246  // Remove all children.
247  for (const auto &child : forwardChildren_)
248  {
249  child.lock()->setCostToComeFromGoal(objective_->infiniteCost());
250  child.lock()->resetForwardParent();
251  auto childsAccumulatedChildren = child.lock()->invalidateForwardBranch();
252  accumulatedChildren.insert(accumulatedChildren.end(), childsAccumulatedChildren.begin(),
253  childsAccumulatedChildren.end());
254  }
255  forwardChildren_.clear();
256 
257  return accumulatedChildren;
258  }
259 
260  void Vertex::setForwardParent(const std::shared_ptr<Vertex> &vertex, const ompl::base::Cost &edgeCost)
261  {
262  // If this is a rewiring, remove from my parent's children.
263  if (static_cast<bool>(forwardParent_.lock()))
264  {
265  forwardParent_.lock()->removeFromForwardChildren(vertexId_);
266  }
267 
268  // Remember the edge cost.
269  edgeCostFromForwardParent_ = edgeCost;
270 
271  // Remember the corresponding parent.
272  forwardParent_ = std::weak_ptr<Vertex>(vertex);
273 
274  // Update the cost to come.
275  costToComeFromStart_ = objective_->combineCosts(vertex->getCostToComeFromStart(), edgeCost);
276  }
277 
279  {
280  forwardParent_.reset();
281  }
282 
283  void Vertex::setReverseParent(const std::shared_ptr<Vertex> &vertex)
284  {
285  // If this is a rewiring, remove from my parent's children.
286  if (static_cast<bool>(reverseParent_.lock()))
287  {
288  reverseParent_.lock()->removeFromReverseChildren(vertexId_);
289  }
290 
291  // Remember the parent.
292  reverseParent_ = std::weak_ptr<Vertex>(vertex);
293  }
294 
296  {
297  reverseParent_.reset();
298  }
299 
300  void Vertex::addToForwardChildren(const std::shared_ptr<Vertex> &vertex)
301  {
302  forwardChildren_.emplace_back(vertex);
303  }
304 
305  void Vertex::removeFromForwardChildren(std::size_t vertexId)
306  {
307  // Find the child.
308  auto it = std::find_if(
309  forwardChildren_.begin(), forwardChildren_.end(),
310  [vertexId](const std::weak_ptr<Vertex> &child) { return vertexId == child.lock()->getId(); });
311 
312  // Throw if it is not found.
313  if (it == forwardChildren_.end())
314  {
315  auto msg = "Asked to remove vertex from forward children that is currently not a child."s;
316  throw ompl::Exception(msg);
317  }
318 
319  // Swap and pop.
320  std::iter_swap(it, forwardChildren_.rbegin());
321  forwardChildren_.pop_back();
322  }
323 
324  void Vertex::addToReverseChildren(const std::shared_ptr<Vertex> &vertex)
325  {
326  reverseChildren_.push_back(vertex);
327  }
328 
329  void Vertex::removeFromReverseChildren(std::size_t vertexId)
330  {
331  // Find the child.
332  auto it = std::find_if(
333  reverseChildren_.begin(), reverseChildren_.end(),
334  [vertexId](const std::weak_ptr<Vertex> &child) { return vertexId == child.lock()->getId(); });
335 
336  // Throw if it is not found.
337  if (it == reverseChildren_.end())
338  {
339  auto msg = "Asked to remove vertex from reverse children that is currently not a child."s;
340  throw ompl::Exception(msg);
341  }
342 
343  // Swap and pop.
344  std::iter_swap(it, reverseChildren_.rbegin());
345  reverseChildren_.pop_back();
346  }
347 
348  void Vertex::whitelistAsChild(const std::shared_ptr<Vertex> &vertex) const
349  {
350  whitelistedChildren_.emplace_back(vertex);
351  }
352 
353  bool Vertex::isWhitelistedAsChild(const std::shared_ptr<Vertex> &vertex) const
354  {
355  // Check if the vertex is whitelisted by iterating over all whitelisted children.
356  // It this detects an invalid vertex, e.g., a vertex that was once whitelisted but
357  // has been pruned since, remove the vertex from the list of whitelisted children.
358  auto it = whitelistedChildren_.begin();
359  while (it != whitelistedChildren_.end())
360  {
361  // Check if the child is a valid vertex.
362  if (const auto child = it->lock())
363  {
364  // Check if the vertex is whitelisted.
365  if (child->getId() == vertex->getId())
366  {
367  return true;
368  }
369  ++it;
370  }
371  else
372  {
373  it = whitelistedChildren_.erase(it);
374  }
375  }
376  return false;
377  }
378 
379  void Vertex::blacklistAsChild(const std::shared_ptr<Vertex> &vertex) const
380  {
381  blacklistedChildren_.emplace_back(vertex);
382  }
383 
384  bool Vertex::isBlacklistedAsChild(const std::shared_ptr<Vertex> &vertex) const
385  {
386  auto it = blacklistedChildren_.begin();
387  while (it != blacklistedChildren_.end())
388  {
389  // Check if the child is a valid vertex.
390  if (const auto child = it->lock())
391  {
392  // Check if the vertex is whitelisted.
393  if (child->getId() == vertex->getId())
394  {
395  return true;
396  }
397  ++it;
398  }
399  else
400  {
401  it = blacklistedChildren_.erase(it);
402  }
403  }
404  return false;
405  }
406 
408  {
409  return neighborBatchId_ == batchId_;
410  }
411 
412  void Vertex::cacheNeighbors(const std::vector<std::shared_ptr<Vertex>> &neighbors) const
413  {
414  neighbors_.clear();
415  neighbors_.insert(neighbors_.begin(), neighbors.begin(), neighbors.end());
416  neighborBatchId_ = batchId_;
417  }
418 
419  const std::vector<std::shared_ptr<Vertex>> Vertex::getNeighbors() const
420  {
421  if (neighborBatchId_ != batchId_)
422  {
423  throw ompl::Exception("Requested neighbors from vertex of outdated approximation.");
424  }
425 
426  std::vector<std::shared_ptr<Vertex>> neighbors;
427  for (const auto &neighbor : neighbors_)
428  {
429  assert(neighbor.lock());
430  neighbors.emplace_back(neighbor.lock());
431  }
432 
433  return neighbors;
434  }
435 
436  std::vector<std::shared_ptr<Vertex>> Vertex::getForwardChildren() const
437  {
438  std::vector<std::shared_ptr<Vertex>> children;
439  for (const auto &child : forwardChildren_)
440  {
441  assert(!child.expired());
442  children.emplace_back(child.lock());
443  }
444  return children;
445  }
446 
447  std::vector<std::shared_ptr<Vertex>> Vertex::getReverseChildren() const
448  {
449  std::vector<std::shared_ptr<Vertex>> children;
450  children.reserve(reverseChildren_.size());
451  for (const auto &child : reverseChildren_)
452  {
453  assert(!child.expired());
454  children.emplace_back(child.lock());
455  }
456  return children;
457  }
458 
459  bool Vertex::isConsistent() const
460  {
461  // Return whether the cost to come is the same now as when this vertex was expanded.
462  return objective_->isCostEquivalentTo(costToComeFromGoal_, expandedCostToComeFromGoal_);
463  }
464 
466  typename ompl::BinaryHeap<
467  std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
468  std::function<bool(const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
469  const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
470  Element *pointer)
471  {
472  reverseQueuePointerId_ = batchId_;
473  reverseQueuePointer_ = pointer;
474  }
475 
476  typename ompl::BinaryHeap<
477  std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>,
478  std::function<bool(const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &,
479  const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> &)>>::
480  Element *
482  {
483  if (batchId_ != reverseQueuePointerId_)
484  {
485  reverseQueuePointer_ = nullptr;
486  }
487  return reverseQueuePointer_;
488  }
489 
491  {
492  reverseQueuePointer_ = nullptr;
493  }
494 
496  typename ompl::BinaryHeap<
497  aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *pointer)
498  {
499  forwardQueueIncomingLookup_.emplace_back(pointer);
500  }
501 
503  typename ompl::BinaryHeap<
504  aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *pointer)
505  {
506  forwardQueueOutgoingLookup_.emplace_back(pointer);
507  }
508 
509  typename std::vector<ompl::BinaryHeap<
510  aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *>
512  {
513  return forwardQueueIncomingLookup_;
514  }
515 
516  typename std::vector<ompl::BinaryHeap<
517  aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *>
519  {
520  return forwardQueueOutgoingLookup_;
521  }
522 
525  std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *element)
526  {
527  forwardQueueIncomingLookup_.erase(
528  std::remove(forwardQueueIncomingLookup_.begin(), forwardQueueIncomingLookup_.end(), element));
529  }
530 
533  std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>::Element *element)
534  {
535  forwardQueueOutgoingLookup_.erase(
536  std::remove(forwardQueueOutgoingLookup_.begin(), forwardQueueOutgoingLookup_.end(), element));
537  }
538 
540  {
541  forwardQueueIncomingLookup_.clear();
542  }
543 
545  {
546  forwardQueueOutgoingLookup_.clear();
547  }
548 
549  void Vertex::callOnForwardBranch(const std::function<void(const std::shared_ptr<Vertex> &)> &function)
550  {
551  // Call the function on this vertex.
552  function(shared_from_this());
553 
554  // Recursively call it on all forward children.
555  for (auto &child : forwardChildren_)
556  {
557  child.lock()->callOnForwardBranch(function);
558  }
559  }
560 
561  void Vertex::callOnReverseBranch(const std::function<void(const std::shared_ptr<Vertex> &)> &function)
562  {
563  // Call the function on this vertex.
564  function(shared_from_this());
565 
566  // Recursively call it on all reverse children.
567  for (auto &child : reverseChildren_)
568  {
569  child.lock()->callOnReverseBranch(function);
570  }
571  }
572 
573  } // namespace aitstar
574 
575  } // namespace geometric
576 
577 } // namespace ompl
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
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
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition: BinaryHeap.h:84
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
The exception type for ompl.
Definition: Exception.h:78
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