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
46using namespace std::string_literals;
47
48namespace 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
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
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition: BinaryHeap.h:53
The exception type for ompl.
Definition: Exception.h:47
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::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
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
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
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
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
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
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
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.
Definition: Vertex.cpp:457
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
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.