Loading...
Searching...
No Matches
Vertex.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2014, University of Toronto
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 name of the University of Toronto 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: Jonathan Gammell, Marlin Strub */
36
37// My definition:
38#include "ompl/geometric/planners/informedtrees/bitstar/Vertex.h"
39
40// For std::move
41#include <utility>
42
43// For exceptions:
44#include "ompl/util/Exception.h"
45
46// BIT*:
47// A collection of common helper functions
48#include "ompl/geometric/planners/informedtrees/bitstar/HelperFunctions.h"
49// The ID generator class
50#include "ompl/geometric/planners/informedtrees/bitstar/IdGenerator.h"
51// The cost-helper class:
52#include "ompl/geometric/planners/informedtrees/bitstar/CostHelper.h"
53
54// Debug macros.
55#ifdef BITSTAR_DEBUG
56 // Debug setting. The id number of a vertex to track. Requires BITSTAR_DEBUG to be defined in BITstar.h
57 #define TRACK_VERTEX_ID 0
58
60 #define PRINT_VERTEX_CHANGE \
61 if (id_ == TRACK_VERTEX_ID) \
62 { \
63 std::cout << "Vertex " << id_ << ": " << __func__ << "" << std::endl; \
64 }
65
67 #define ASSERT_NOT_PRUNED \
68 if (this->isPruned_) \
69 { \
70 std::cout << "Vertex " << id_ << ": " << __func__ << std::endl; \
71 throw ompl::Exception("Attempting to access a pruned vertex."); \
72 }
73#else
74 #define PRINT_VERTEX_CHANGE
75 #define ASSERT_NOT_PRUNED
76#endif // BITSTAR_DEBUG
77
78// An anonymous namespace to hide the instance:
79namespace
80{
81 // Global variables:
82 // The initialization flag stating that the ID generator has been created:
83 std::once_flag g_IdInited;
84 // A pointer to the actual ID generator
85 boost::scoped_ptr<ompl::geometric::BITstar::IdGenerator> g_IdGenerator;
86
87 // A function to initialize the ID generator pointer:
88 void initIdGenerator()
89 {
90 g_IdGenerator.reset(new ompl::geometric::BITstar::IdGenerator());
91 }
92
93 // A function to get the current ID generator:
95 {
96 std::call_once(g_IdInited, &initIdGenerator);
97 return *g_IdGenerator;
98 }
99}
100
101namespace ompl
102{
103 namespace geometric
104 {
106 // Public functions:
107 BITstar::Vertex::Vertex(ompl::base::SpaceInformationPtr spaceInformation, const CostHelper *const costHelpPtr, SearchQueue *const queuePtr,
108 const std::shared_ptr<const unsigned int> &approximationId, bool root)
109 : id_(getIdGenerator().getNewId())
110 , si_(std::move(spaceInformation))
111 , costHelpPtr_(std::move(costHelpPtr))
112 , queuePtr_(queuePtr)
113 , state_(si_->allocState())
114 , isRoot_(root)
115 , edgeCost_(costHelpPtr_->infiniteCost())
116 , cost_(costHelpPtr_->infiniteCost())
117 , costAtExpansion_(costHelpPtr_->infiniteCost())
118 , currentSearchId_(queuePtr->getSearchId())
119 , currentApproximationId_(approximationId)
120 {
121 PRINT_VERTEX_CHANGE
122
123 if (this->isRoot())
124 {
125 cost_ = costHelpPtr_->identityCost();
126 }
127 // No else, infinite by default
128 }
129
130 BITstar::Vertex::~Vertex()
131 {
132 PRINT_VERTEX_CHANGE
133
134 // Free the state on destruction
135 si_->freeState(state_);
136 }
137
138 BITstar::VertexId BITstar::Vertex::getId() const
139 {
140 ASSERT_NOT_PRUNED
141
142 return id_;
143 }
144
145 ompl::base::State const *BITstar::Vertex::state() const
146 {
147 ASSERT_NOT_PRUNED
148
149 return state_;
150 }
151
152 ompl::base::State *BITstar::Vertex::state()
153 {
154 PRINT_VERTEX_CHANGE
155 ASSERT_NOT_PRUNED
156
157 return state_;
158 }
159
161 // The vertex's graph properties:
162 bool BITstar::Vertex::isRoot() const
163 {
164 ASSERT_NOT_PRUNED
165
166 return isRoot_;
167 }
168
169 bool BITstar::Vertex::hasParent() const
170 {
171 ASSERT_NOT_PRUNED
172
173 return static_cast<bool>(parentPtr_);
174 }
175
176 bool BITstar::Vertex::isInTree() const
177 {
178 ASSERT_NOT_PRUNED
179
180 return this->isRoot() || this->hasParent();
181 }
182
183 unsigned int BITstar::Vertex::getDepth() const
184 {
185 ASSERT_NOT_PRUNED
186
187#ifdef BITSTAR_DEBUG
188 if (!this->isRoot() && !this->hasParent())
189 {
190 throw ompl::Exception("Attempting to get the depth of a vertex that does not have a parent yet is not "
191 "root.");
192 }
193#endif // BITSTAR_DEBUG
194
195 return depth_;
196 }
197
198 BITstar::VertexConstPtr BITstar::Vertex::getParent() const
199 {
200 ASSERT_NOT_PRUNED
201
202#ifdef BITSTAR_DEBUG
203 if (!this->hasParent())
204 {
205 if (this->isRoot())
206 {
207 throw ompl::Exception("Attempting to access the parent of the root vertex.");
208 }
209 else
210 {
211 throw ompl::Exception("Attempting to access the parent of a vertex that does not have one.");
212 }
213 }
214#endif // BITSTAR_DEBUG
215
216 return parentPtr_;
217 }
218
219 BITstar::VertexPtr BITstar::Vertex::getParent()
220 {
221 ASSERT_NOT_PRUNED
222
223#ifdef BITSTAR_DEBUG
224 if (!this->hasParent())
225 {
226 if (this->isRoot())
227 {
228 throw ompl::Exception("Attempting to access the parent of the root vertex.");
229 }
230 else
231 {
232 throw ompl::Exception("Attempting to access the parent of a vertex that does not have one.");
233 }
234 }
235#endif // BITSTAR_DEBUG
236
237 return parentPtr_;
238 }
239
240 void BITstar::Vertex::addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost)
241 {
242 PRINT_VERTEX_CHANGE
243 ASSERT_NOT_PRUNED
244
245#ifdef BITSTAR_DEBUG
246 // Assert I can take a parent
247 if (this->isRoot())
248 {
249 throw ompl::Exception("Attempting to add a parent to the root vertex, which cannot have a parent.");
250 }
251 if (this->hasParent())
252 {
253 throw ompl::Exception("Attempting to add a parent to a vertex that already has one.");
254 }
255#endif // BITSTAR_DEBUG
256
257 // Store the parent.
258 parentPtr_ = newParent;
259
260 // Store the edge cost.
261 edgeCost_ = edgeInCost;
262
263 // Update my cost and that of my children.
264 this->updateCostAndDepth(true);
265 }
266
267 void BITstar::Vertex::removeParent(bool updateChildCosts)
268 {
269 PRINT_VERTEX_CHANGE
270 ASSERT_NOT_PRUNED
271
272#ifdef BITSTAR_DEBUG
273 // Assert I have a parent
274 if (this->isRoot())
275 {
276 throw ompl::Exception("Attempting to remove the parent of the root vertex, which cannot have a "
277 "parent.");
278 }
279 if (!this->hasParent())
280 {
281 throw ompl::Exception("Attempting to remove the parent of a vertex that does not have a parent.");
282 }
283#endif // BITSTAR_DEBUG
284
285 // Clear my parent
286 parentPtr_.reset();
287
288 // Update my cost and possibly the cost of my descendants:
289 this->updateCostAndDepth(updateChildCosts);
290 }
291
292 bool BITstar::Vertex::hasChildren() const
293 {
294 ASSERT_NOT_PRUNED
295
296 return !children_.empty();
297 }
298
299 void BITstar::Vertex::getChildren(VertexConstPtrVector *children) const
300 {
301 ASSERT_NOT_PRUNED
302
303 children->clear();
304
305 for (const auto &child : children_)
306 {
307#ifdef BITSTAR_DEBUG
308 // Check that the weak pointer hasn't expired
309 if (child.expired())
310 {
311 throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
312 "children of a vertex.");
313 }
314#endif // BITSTAR_DEBUG
315
316 // Lock and push back
317 children->push_back(child.lock());
318 }
319 }
320
321 void BITstar::Vertex::getChildren(VertexPtrVector *children)
322 {
323 ASSERT_NOT_PRUNED
324
325 children->clear();
326
327 for (const auto &child : children_)
328 {
329#ifdef BITSTAR_DEBUG
330 // Check that the weak pointer hasn't expired
331 if (child.expired())
332 {
333 throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
334 "children of a vertex.");
335 }
336#endif // BITSTAR_DEBUG
337
338 // Lock and push back
339 children->push_back(child.lock());
340 }
341 }
342
343 void BITstar::Vertex::addChild(const VertexPtr &child)
344 {
345 PRINT_VERTEX_CHANGE
346 ASSERT_NOT_PRUNED
347
348#ifdef BITSTAR_DEBUG
349 // Assert that I am this child's parent
350 if (child->isRoot())
351 {
352 throw ompl::Exception("Attempted to add a root vertex as a child.");
353 }
354 if (!child->hasParent())
355 {
356 throw ompl::Exception("Attempted to add child that does not have a listed parent.");
357 }
358 if (child->getParent()->getId() != id_)
359 {
360 throw ompl::Exception("Attempted to add someone else's child as mine.");
361 }
362#endif // BITSTAR_DEBUG
363
364 // Push back the shared_ptr into the vector of weak_ptrs, this makes a weak_ptr copy
365 children_.push_back(child);
366
367 // Leave the costs of the child out of date.
368 }
369
370 void BITstar::Vertex::removeChild(const VertexPtr &child)
371 {
372 PRINT_VERTEX_CHANGE
373 ASSERT_NOT_PRUNED
374
375#ifdef BITSTAR_DEBUG
376 // Assert that I am this child's parent
377 if (child->isRoot())
378 {
379 throw ompl::Exception("Attempted to remove a root vertex as a child.");
380 }
381 if (!child->hasParent())
382 {
383 throw ompl::Exception("Attempted to remove a child that does not have a listed parent.");
384 }
385 if (child->getParent()->getId() != id_)
386 {
387 throw ompl::Exception("Attempted to remove a child vertex from the wrong parent.");
388 }
389#endif // BITSTAR_DEBUG
390
391 // Variables
392 // Whether the child has been found (and then deleted);
393 bool foundChild;
394
395 // Iterate over the vector of children pointers until the child is found. Iterators make erase easier
396 foundChild = false;
397 for (auto it = children_.begin(); it != children_.end() && !foundChild; ++it)
398 {
399#ifdef BITSTAR_DEBUG
400 // Check that the weak pointer hasn't expired
401 if (it->expired())
402 {
403 throw ompl::Exception("A (weak) pointer to a child was found to have expired while removing a "
404 "child from a vertex.");
405 }
406#endif // BITSTAR_DEBUG
407
408 // Check if this is the child we're looking for
409 if (it->lock()->getId() == child->getId())
410 {
411 // It is, mark as found
412 foundChild = true;
413
414 // First, clear the entry in the vector
415 it->reset();
416
417 // Then remove that entry from the vector efficiently
418 swapPopBack(it, &children_);
419 }
420 // No else.
421 }
422
423 // Leave the costs of the child out of date.
424#ifdef BITSTAR_DEBUG
425 if (!foundChild)
426 {
427 throw ompl::Exception("Attempting to remove a child vertex not present in the vector of children "
428 "stored in the (supposed) parent vertex.");
429 }
430#endif // BITSTAR_DEBUG
431 }
432
433 void BITstar::Vertex::blacklistChild(const VertexConstPtr &vertex)
434 {
435 childIdBlacklist_.emplace(vertex->getId());
436 }
437
438 void BITstar::Vertex::whitelistChild(const VertexConstPtr &vertex)
439 {
440 childIdWhitelist_.emplace(vertex->getId());
441 }
442
443 bool BITstar::Vertex::isBlacklistedAsChild(const VertexConstPtr &vertex) const
444 {
445 return childIdBlacklist_.find(vertex->getId()) != childIdBlacklist_.end();
446 }
447
448 bool BITstar::Vertex::isWhitelistedAsChild(const VertexConstPtr &vertex) const
449 {
450 return childIdWhitelist_.find(vertex->getId()) != childIdWhitelist_.end();
451 }
452
453 void BITstar::Vertex::clearBlacklist()
454 {
455 childIdBlacklist_.clear();
456 }
457
458 void BITstar::Vertex::clearWhitelist()
459 {
460 childIdWhitelist_.clear();
461 }
462
463 ompl::base::Cost BITstar::Vertex::getCost() const
464 {
465 ASSERT_NOT_PRUNED
466
467 return cost_;
468 }
469
470 ompl::base::Cost BITstar::Vertex::getEdgeInCost() const
471 {
472 ASSERT_NOT_PRUNED
473
474#ifdef BITSTAR_DEBUG
475 if (!this->hasParent())
476 {
477 throw ompl::Exception("Attempting to access the incoming-edge cost of a vertex without a parent.");
478 }
479#endif // BITSTAR_DEBUG
480
481 return edgeCost_;
482 }
483
484 bool BITstar::Vertex::isConsistent() const
485 {
486 return cost_.value() == costAtExpansion_.value();
487 }
488
489 bool BITstar::Vertex::isPruned() const
490 {
491 return isPruned_;
492 }
493
494 bool BITstar::Vertex::isExpandedOnCurrentApproximation() const
495 {
496 return expansionApproximationId_ == *currentApproximationId_;
497 }
498
499 bool BITstar::Vertex::isExpandedOnCurrentSearch() const
500 {
501 return expansionSearchId_ == *currentSearchId_;
502 }
503
504 bool BITstar::Vertex::hasEverBeenExpandedAsRewiring() const
505 {
506 return hasEverBeenExpandedAsRewiring_;
507 }
508
509 void BITstar::Vertex::registerExpansion()
510 {
511 // Store the cost-to-come at expansion.
512 costAtExpansion_ = cost_;
513
514 // Remember the search and approximation ids.
515 expansionApproximationId_ = *currentApproximationId_;
516 expansionSearchId_ = *currentSearchId_;
517 }
518
519 void BITstar::Vertex::registerRewiringExpansion()
520 {
521 hasEverBeenExpandedAsRewiring_ = true;
522 }
523
524 void BITstar::Vertex::markPruned()
525 {
526 PRINT_VERTEX_CHANGE
527 ASSERT_NOT_PRUNED
528
529 isPruned_ = true;
530 }
531
532 void BITstar::Vertex::markUnpruned()
533 {
534 PRINT_VERTEX_CHANGE
535
536 isPruned_ = false;
537 }
538
539 void BITstar::Vertex::insertInEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &element)
540 {
541 ASSERT_NOT_PRUNED
542
543 // Conditionally clear any existing lookups.
544 this->clearLookupsIfOutdated();
545
546#ifdef BITSTAR_DEBUG
547 // Assert that this edge is NOT _from_ this vertex.
548 if (element->data.second.first->getId() == id_)
549 {
550 throw ompl::Exception("Attempted to add a cyclic incoming queue edge.");
551 }
552 // Assert that this edge is _to_ this vertex.
553 if (element->data.second.second->getId() != id_)
554 {
555 throw ompl::Exception("Attempted to add an incoming queue edge to the wrong vertex.");
556 }
557 // Assert that an edge from this source does not already exist.
558 for (const auto &inEdge : edgeQueueInLookup_)
559 {
560 if (element->data.second.first->getId() == inEdge->data.second.first->getId())
561 {
562 throw ompl::Exception("Attempted to add a second edge to the queue from a single source vertex.");
563 }
564 }
565#endif // BITSTAR_DEBUG
566
567 // Insert it into the lookup.
568 edgeQueueInLookup_.push_back(element);
569 }
570
571 void BITstar::Vertex::removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &element)
572 {
573 ASSERT_NOT_PRUNED
574
575#ifdef BITSTAR_DEBUG
576 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
577 // If so, there's no point clearing them, as then we'd be trying to remove an edge that doesn't exist which would be an error.
578 if (*currentApproximationId_ != lookupApproximationId_)
579 {
580 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
581 }
582#endif // BITSTAR_DEBUG
583
584 // Variable
585 // Element found
586 bool found = false;
587
588 // Iterate through the list and find the address of the element to delete
589 for (auto it = edgeQueueInLookup_.begin(); it != edgeQueueInLookup_.end() && !found; ++it)
590 {
591 // Is it the element we're looking for? Source id
592 if ((*it)->data.second.first->getId() == element->data.second.first->getId())
593 {
594 // Remove by iterator
595 this->removeFromEdgeQueueInLookup(it);
596
597 // Mark as found
598 found = true;
599 }
600 // No else, try the next
601 }
602
603#ifdef BITSTAR_DEBUG
604 if (!found)
605 {
606 throw ompl::Exception("Attempted to remove an edge not in the incoming lookup.");
607 }
608#endif // BITSTAR_DEBUG
609 }
610
611 void BITstar::Vertex::removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
612 {
613 ASSERT_NOT_PRUNED
614
615#ifdef BITSTAR_DEBUG
616 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
617 // If so, there's no point clearing them, as then we'd be trying to remove an edge that doesn't exist which would be an error.
618 if (*currentApproximationId_ != lookupApproximationId_)
619 {
620 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
621 }
622#endif // BITSTAR_DEBUG
623
624 // Remove a non-const version of the given iterator.
625 // (trick from https://stackoverflow.com/a/10669041/1442500)
626 this->removeFromEdgeQueueInLookup(edgeQueueInLookup_.erase(element, element));
627 }
628
629 void BITstar::Vertex::clearEdgeQueueInLookup()
630 {
631 ASSERT_NOT_PRUNED
632
633 edgeQueueInLookup_.clear();
634 }
635
636 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstBegin()
637 {
638 ASSERT_NOT_PRUNED
639
640 // Conditionally clear any existing lookups
641 this->clearLookupsIfOutdated();
642
643 return edgeQueueInLookup_.cbegin();
644 }
645
646 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstEnd()
647 {
648 ASSERT_NOT_PRUNED
649
650 // Conditionally clear any existing lookups
651 this->clearLookupsIfOutdated();
652
653 return edgeQueueInLookup_.cend();
654 }
655
656 unsigned int BITstar::Vertex::edgeQueueInLookupSize()
657 {
658 ASSERT_NOT_PRUNED
659
660 // Conditionally clear any existing lookups
661 this->clearLookupsIfOutdated();
662
663 return edgeQueueInLookup_.size();
664 }
665
666 void BITstar::Vertex::insertInEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr& element)
667 {
668 ASSERT_NOT_PRUNED
669
670 // Conditionally clear any existing lookups
671 this->clearLookupsIfOutdated();
672
673#ifdef BITSTAR_DEBUG
674 // Assert that this edge is _from_ this vertex
675 if (element->data.second.first->getId() != id_)
676 {
677 throw ompl::Exception("Attempted to add an outgoing queue edge to the wrong vertex.");
678 }
679 // Assert that this edge is NOT _to_ this vertex
680 if (element->data.second.second->getId() == id_)
681 {
682 throw ompl::Exception("Attempted to add a cyclic outgoing queue edge.");
683 }
684 // Assert that an edge to this target does not already exist
685 for (const auto &outEdge : edgeQueueOutLookup_)
686 {
687 if (element->data.second.second->getId() == outEdge->data.second.second->getId())
688 {
689 throw ompl::Exception("Attempted to add a second edge to the queue to a single target vertex.");
690 }
691 }
692#endif // BITSTAR_DEBUG
693
694 // Push back
695 edgeQueueOutLookup_.push_back(element);
696 }
697
698 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &element)
699 {
700 ASSERT_NOT_PRUNED
701
702#ifdef BITSTAR_DEBUG
703 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
704 // If so, there's no point clearing them, as then we'd be trying to remove an edge that doesn't exist which would be an error.
705 if (*currentApproximationId_ != lookupApproximationId_)
706 {
707 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
708 }
709#endif // BITSTAR_DEBUG
710
711 // Variable
712 // Element found
713 bool found = false;
714
715 // Iterate through the list and find the address of the element to delete
716 for (auto it = edgeQueueOutLookup_.begin(); it != edgeQueueOutLookup_.end() && !found; ++it)
717 {
718 // Is it the element we're looking for? Source id
719 if ((*it)->data.second.second->getId() == element->data.second.second->getId())
720 {
721 // Remove by iterator
722 this->removeFromEdgeQueueOutLookup(it);
723
724 // Mark as found
725 found = true;
726 }
727 // No else, try the next
728 }
729
730#ifdef BITSTAR_DEBUG
731 if (!found)
732 {
733 throw ompl::Exception("Attempted to remove an edge not in the outgoing lookup.");
734 }
735#endif // BITSTAR_DEBUG
736 }
737
738 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
739 {
740 ASSERT_NOT_PRUNED
741
742#ifdef BITSTAR_DEBUG
743 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
744 // If so, there's no point clearing them, as then we'd be trying to remove an edge that doesn't exist which would be an error.
745 if (*currentApproximationId_ != lookupApproximationId_)
746 {
747 throw ompl::Exception("Attempted to remove an outgoing queue edge added under a different expansion id.");
748 }
749#endif // BITSTAR_DEBUG
750
751 // Remove a non-const version of the given iterator
752 // (trick from https://stackoverflow.com/a/10669041/1442500)
753 this->removeFromEdgeQueueOutLookup(edgeQueueOutLookup_.erase(element, element));
754 }
755
756 void BITstar::Vertex::clearEdgeQueueOutLookup()
757 {
758 ASSERT_NOT_PRUNED
759
760 edgeQueueOutLookup_.clear();
761 }
762
763 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstBegin()
764 {
765 ASSERT_NOT_PRUNED
766
767 // Make sure the lookups aren't out of date.
768 this->clearLookupsIfOutdated();
769
770 return edgeQueueOutLookup_.cbegin();
771 }
772
773 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstEnd()
774 {
775 ASSERT_NOT_PRUNED
776
777 // Make sure the lookups aren't out of date.
778 this->clearLookupsIfOutdated();
779
780 return edgeQueueOutLookup_.cend();
781 }
782
783 unsigned int BITstar::Vertex::edgeQueueOutLookupSize()
784 {
785 ASSERT_NOT_PRUNED
786
787 // Make sure the lookups aren't out of date.
788 this->clearLookupsIfOutdated();
789
790 return edgeQueueOutLookup_.size();
791 }
792
793 void BITstar::Vertex::updateCostAndDepth(bool cascadeUpdates /*= true*/)
794 {
795 PRINT_VERTEX_CHANGE
796 ASSERT_NOT_PRUNED
797
798 if (this->isRoot())
799 {
800 // Am I root? -- I don't really know how this would ever be called, but ok.
801 cost_ = costHelpPtr_->identityCost();
802 depth_ = 0u;
803 }
804 else if (!this->hasParent())
805 {
806 // Am I disconnected?
807 cost_ = costHelpPtr_->infiniteCost();
808
809 // Set the depth to 0u, getDepth will throw in this condition
810 depth_ = 0u;
811
812#ifdef BITSTAR_DEBUG
813 // Assert that I have not been asked to cascade this bad data to my children:
814 if (this->hasChildren() && cascadeUpdates)
815 {
816 throw ompl::Exception("Attempting to update descendants' costs and depths of a vertex that does "
817 "not have a parent and is not root. This information would therefore be "
818 "gibberish.");
819 }
820#endif // BITSTAR_DEBUG
821 }
822 else
823 {
824 // I have a parent, so my cost is my parent cost + my edge cost to the parent
825 cost_ = costHelpPtr_->combineCosts(parentPtr_->getCost(), edgeCost_);
826
827 // If I have outgoing edges in the search queue, they need to be updated.
828 for (const auto& edge : edgeQueueOutLookup_) {
829 if (lookupApproximationId_ == *currentApproximationId_) {
830 queuePtr_->update(edge);
831 }
832 }
833
834 // I am one more than my parent's depth:
835 depth_ = (parentPtr_->getDepth() + 1u);
836 }
837
838 // Am I updating my children?
839 if (cascadeUpdates)
840 {
841 // Now, iterate over my vector of children and tell each one to update its own damn cost:
842 for (auto &child : children_)
843 {
844#ifdef BITSTAR_DEBUG
845 // Check that it hasn't expired
846 if (child.expired())
847 {
848 throw ompl::Exception("A (weak) pointer to a child has was found to have expired while "
849 "updating the costs and depths of descendant vertices.");
850 }
851#endif // BITSTAR_DEBUG
852
853 // Get a lock and tell the child to update:
854 child.lock()->updateCostAndDepth(true);
855 }
856 }
857 // No else, do not update the children. Let's hope the caller knows what they're doing.
858 }
859
860 void BITstar::Vertex::removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
861 {
862#ifdef BITSTAR_DEBUG
863 // Store the parent id of the edge we're removing.
864 VertexId parentId = (*element)->data.second.first->getId();
865
866 // Assert that this edge is not from this vertex.
867 if (parentId == id_)
868 {
869 throw ompl::Exception("Attempted to remove a cyclic incoming queue edge.");
870 }
871
872 // Assert that this edge is to this vertex.
873 if ((*element)->data.second.second->getId() != id_)
874 {
875 throw ompl::Exception("Attempted to remove an incoming queue edge from the wrong vertex.");
876 }
877
878 // Assert that it could exist.
879 if (edgeQueueInLookup_.empty())
880 {
881 throw ompl::Exception("Attempted to remove an incoming queue edge from a vertex with an empty list.");
882 }
883
884 // Assert that this edge actually exists.
885 bool found = false;
886 for (auto it = edgeQueueInLookup_.begin(); it != edgeQueueInLookup_.end() && !found; ++it)
887 {
888 found = ((*it)->data.second.first->getId() == parentId);
889 }
890 if (!found)
891 {
892 throw ompl::Exception("Attempted to remove an edge not in the incoming lookup.");
893 }
894#endif // BITSTAR_DEBUG
895
896 // Clear our entry in the list
897 *element = nullptr;
898
899 // Remove it efficiently
900 swapPopBack(element, &edgeQueueInLookup_);
901
902#ifdef BITSTAR_DEBUG
903 // Assert that it's now gone.
904 for (const auto &edgePtr : edgeQueueInLookup_)
905 {
906 if (edgePtr->data.second.first->getId() == parentId)
907 {
908 throw ompl::Exception("Failed to remove the designated edge in the incoming lookup.");
909 }
910 }
911#endif // BITSTAR_DEBUG
912 }
913
914 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
915 {
916#ifdef BITSTAR_DEBUG
917 // Store the child id of the edge we're removing.
918 VertexId childId = (*element)->data.second.second->getId();
919
920 // Assert that this edge is this vertex.
921 if ((*element)->data.second.first->getId() != id_)
922 {
923 throw ompl::Exception("Attempted to remove an outgoing queue edge from the wrong vertex.");
924 }
925 // Assert that this edge is not to this vertex.
926 if (childId == id_)
927 {
928 throw ompl::Exception("Attempted to remove a cyclic outgoing queue edge.");
929 }
930 // Assert that it could exist
931 if (edgeQueueOutLookup_.empty())
932 {
933 throw ompl::Exception("Attempted to remove an outgoing queue edge from a vertex with an empty list.");
934 }
935 // Assert that this edge actually exists
936 bool found = false;
937 for (auto it = edgeQueueOutLookup_.begin(); it != edgeQueueOutLookup_.end() && !found; ++it)
938 {
939 found = ((*it)->data.second.second->getId() == childId);
940 }
941 if (!found)
942 {
943 throw ompl::Exception("Attempted to remove an edge not in the outgoing lookup.");
944 }
945#endif // BITSTAR_DEBUG
946
947 // Clear our entry in the list.
948 *element = nullptr;
949
950 // Remove it efficiently.
951 swapPopBack(element, &edgeQueueOutLookup_);
952
953#ifdef BITSTAR_DEBUG
954 // Assert that it's now gone.
955 for (const auto &edgePtr : edgeQueueOutLookup_)
956 {
957 if (edgePtr->data.second.second->getId() == childId)
958 {
959 throw ompl::Exception("Failed to remove the designated edge in the outgoing lookup.");
960 }
961 }
962#endif // BITSTAR_DEBUG
963 }
964
965 void BITstar::Vertex::clearLookupsIfOutdated()
966 {
967 // Clean up any old lookups.
968 if (lookupApproximationId_ != *currentApproximationId_)
969 {
970 // Clear the existing entries.
971 this->clearEdgeQueueInLookup();
972 this->clearEdgeQueueOutLookup();
973
974 // Update the counter.
975 lookupApproximationId_ = *currentApproximationId_;
976 }
977 // No else, this is the same pass through the vertex queue.
978 }
979 } // geometric
980} // ompl
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
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:70
An ID generator class for vertex IDs.
Definition: IdGenerator.h:59
A queue of edges, sorted according to a sort key.
Definition: SearchQueue.h:65
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:84
bool isRoot() const
Returns whether the vertex is the root of the search tree.
Definition: Vertex.cpp:162
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
Definition: BITstar.h:128
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:125
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:119
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:116
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:131
Main namespace. Contains everything in this library.
STL namespace.