Loading...
Searching...
No Matches
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
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, 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
103 Vertex::~Vertex()
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
114 ompl::base::State *Vertex::getState()
115 {
116 return state_;
117 }
118
119 ompl::base::State const *Vertex::getState() const
120 {
121 return state_;
122 }
123
124 ompl::base::ScopedState<> Vertex::getScopedState() const
125 {
126 return ompl::base::ScopedState<>(spaceInformation_->getStateSpace(), state_);
127 }
128
129 ompl::base::Cost Vertex::getCostToComeFromStart() const
130 {
131 return costToComeFromStart_;
132 }
133
134 ompl::base::Cost Vertex::getCostToComeFromGoal() const
135 {
136 return costToComeFromGoal_;
137 }
138
139 ompl::base::Cost Vertex::getExpandedCostToComeFromGoal() const
140 {
141 return expandedCostToComeFromGoal_;
142 }
143
144 ompl::base::Cost Vertex::getCostToGoToGoal() const
145 {
146 return getCostToComeFromGoal();
147 }
148
149 ompl::base::Cost Vertex::getEdgeCostFromForwardParent() const
150 {
151 return edgeCostFromForwardParent_;
152 }
153
154 bool Vertex::hasForwardParent() const
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
167 bool Vertex::hasReverseParent() const
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
177 void Vertex::setForwardEdgeCost(const ompl::base::Cost &cost)
178 {
179 edgeCostFromForwardParent_ = cost;
180 }
181
182 void Vertex::setCostToComeFromStart(const ompl::base::Cost &cost)
183 {
184 costToComeFromStart_ = cost;
185 }
186
187 void Vertex::setCostToComeFromGoal(const ompl::base::Cost &cost)
188 {
189 costToComeFromGoal_ = cost;
190 }
191
192 void Vertex::resetCostToComeFromGoal()
193 {
194 costToComeFromGoal_ = objective_->infiniteCost();
195 }
196
197 void Vertex::resetExpandedCostToComeFromGoal()
198 {
199 expandedCostToComeFromGoal_ = objective_->infiniteCost();
200 }
201
202 void Vertex::setExpandedCostToComeFromGoal(const ompl::base::Cost &cost)
203 {
204 expandedCostToComeFromGoal_ = cost;
205 }
206
207 void Vertex::setCostToGoToGoal(const ompl::base::Cost &cost)
208 {
209 costToGoToGoal_ = cost;
210 }
211
212 void Vertex::updateCostOfForwardBranch() const
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
278 void Vertex::resetForwardParent()
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
295 void Vertex::resetReverseParent()
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
407 bool Vertex::hasCachedNeighbors() const
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
465 void Vertex::setReverseQueuePointer(
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 *
481 Vertex::getReverseQueuePointer() const
482 {
483 if (batchId_ != reverseQueuePointerId_)
484 {
485 reverseQueuePointer_ = nullptr;
486 }
487 return reverseQueuePointer_;
488 }
489
490 void Vertex::resetReverseQueuePointer()
491 {
492 reverseQueuePointer_ = nullptr;
493 }
494
495 void Vertex::addToForwardQueueIncomingLookup(
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
502 void Vertex::addToForwardQueueOutgoingLookup(
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 *>
511 Vertex::getForwardQueueIncomingLookup() const
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 *>
518 Vertex::getForwardQueueOutgoingLookup() const
519 {
520 return forwardQueueOutgoingLookup_;
521 }
522
523 void Vertex::removeFromForwardQueueIncomingLookup(
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
531 void Vertex::removeFromForwardQueueOutgoingLookup(
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
539 void Vertex::resetForwardQueueIncomingLookup()
540 {
541 forwardQueueIncomingLookup_.clear();
542 }
543
544 void Vertex::resetForwardQueueOutgoingLookup()
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
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
Definition of a scoped state.
Definition ScopedState.h:57
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:134
std::vector< std::shared_ptr< Vertex > > getForwardChildren() const
Returns this vertex's children in the forward search tree.
Definition Vertex.cpp:436
This namespace contains code that is specific to planning under geometric constraints.
Message namespace. This contains classes needed to output error messages (or logging) from within the...
Definition Console.h:82
Main namespace. Contains everything in this library.