Loading...
Searching...
No Matches
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
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
82 Vertex::~Vertex()
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
93 ompl::base::State *Vertex::getState()
94 {
95 return state_;
96 }
97
98 ompl::base::State const *Vertex::getState() const
99 {
100 return state_;
101 }
102
103 ompl::base::ScopedState<> Vertex::getScopedState() const
104 {
105 return ompl::base::ScopedState<>(spaceInformation_->getStateSpace(), state_);
106 }
107
108 ompl::base::Cost Vertex::getCostToComeFromStart() const
109 {
110 return costToComeFromStart_;
111 }
112
113 ompl::base::Cost Vertex::getCostToComeFromGoal() const
114 {
115 if (reverseSearchBatchId_ != batchId_)
116 {
117 costToComeFromGoal_ = objective_->infiniteCost();
118 }
119 return costToComeFromGoal_;
120 }
121
122 ompl::base::Cost Vertex::getExpandedCostToComeFromGoal() const
123 {
124 if (expandedReverseSearchId_ != batchId_)
125 {
126 expandedCostToComeFromGoal_ = objective_->infiniteCost();
127 }
128 return expandedCostToComeFromGoal_;
129 }
130
131 ompl::base::Cost Vertex::getCostToGoToGoal() const
132 {
133 return getCostToComeFromGoal();
134 }
135
136 ompl::base::Cost Vertex::getEdgeCostFromForwardParent() const
137 {
138 return edgeCostFromForwardParent_;
139 }
140
141 bool Vertex::hasForwardParent() const
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
154 bool Vertex::hasReverseParent() const
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
164 void Vertex::setForwardEdgeCost(const ompl::base::Cost &cost)
165 {
166 edgeCostFromForwardParent_ = cost;
167 }
168
169 void Vertex::setCostToComeFromStart(const ompl::base::Cost &cost)
170 {
171 costToComeFromStart_ = cost;
172 }
173
174 void Vertex::setCostToComeFromGoal(const ompl::base::Cost &cost)
175 {
176 reverseSearchBatchId_ = batchId_;
177 costToComeFromGoal_ = cost;
178 }
179
180 void Vertex::setExpandedCostToComeFromGoal(const ompl::base::Cost &cost)
181 {
182 expandedCostToComeFromGoal_ = cost;
183 }
184
185 void Vertex::setCostToGoToGoal(const ompl::base::Cost &cost)
186 {
187 costToGoToGoal_ = cost;
188 }
189
190 void Vertex::updateCostOfForwardBranch() const
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
257 void Vertex::resetForwardParent()
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
274 void Vertex::resetReverseParent()
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
361 bool Vertex::hasCachedNeighbors() const
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
405 void Vertex::registerPoppedOutgoingEdgeDuringForwardSearch()
406 {
407 poppedOutgoingEdgeId_ = batchId_;
408 }
409
410 void Vertex::registerExpansionDuringReverseSearch()
411 {
412 expandedCostToComeFromGoal_ = costToComeFromGoal_;
413 expandedReverseSearchId_ = batchId_;
414 }
415
416 void Vertex::unregisterExpansionDuringReverseSearch()
417 {
418 expandedReverseSearchId_ = 0u;
419 }
420
421 void Vertex::registerInsertionIntoQueueDuringReverseSearch()
422 {
423 insertedIntoQueueId_ = batchId_;
424 }
425
426 bool Vertex::hasHadOutgoingEdgePoppedDuringCurrentForwardSearch() const
427 {
428 return poppedOutgoingEdgeId_ == batchId_;
429 }
430
431 bool Vertex::hasBeenExpandedDuringCurrentReverseSearch() const
432 {
433 return expandedReverseSearchId_ == batchId_;
434 }
435
436 bool Vertex::hasBeenInsertedIntoQueueDuringCurrentReverseSearch() const
437 {
438 return insertedIntoQueueId_ == batchId_;
439 }
440
441 void Vertex::setReverseQueuePointer(
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 *
457 Vertex::getReverseQueuePointer() const
458 {
459 if (batchId_ != reverseQueuePointerId_)
460 {
461 reverseQueuePointer_ = nullptr;
462 }
463 return reverseQueuePointer_;
464 }
465
466 void Vertex::resetReverseQueuePointer()
467 {
468 reverseQueuePointer_ = nullptr;
469 }
470
471 void Vertex::addToForwardQueueIncomingLookup(
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
478 void Vertex::addToForwardQueueOutgoingLookup(
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 *>
487 Vertex::getForwardQueueIncomingLookup() const
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 *>
494 Vertex::getForwardQueueOutgoingLookup() const
495 {
496 return forwardQueueOutgoingLookup_;
497 }
498
499 void Vertex::removeFromForwardQueueIncomingLookup(
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
507 void Vertex::removeFromForwardQueueOutgoingLookup(
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
515 void Vertex::resetForwardQueueIncomingLookup()
516 {
517 forwardQueueIncomingLookup_.clear();
518 }
519
520 void Vertex::resetForwardQueueOutgoingLookup()
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
ompl::base::Cost getCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal.
Definition: Vertex.cpp:113
void unregisterExpansionDuringReverseSearch()
Unregisters the expansion of this vertex during the current reverse search, needed when a reverse bra...
Definition: Vertex.cpp:416
std::vector< std::shared_ptr< Vertex > > getForwardChildren() const
Returns this vertex's children in the forward search tree.
Definition: Vertex.cpp:382
Main namespace. Contains everything in this library.