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
390 // Variables
391 // Whether the child has been found (and then deleted);
392 bool foundChild;
393
394 // Iterate over the vector of children pointers until the child is found. Iterators make erase easier
395 foundChild = false;
396#endif // BITSTAR_DEBUG
397 for (auto it = children_.begin(); it != children_.end(); ++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#ifdef BITSTAR_DEBUG
412 // It is, mark as found
413 foundChild = true;
414#endif // BITSTAR_DEBUG
415
416 // First, clear the entry in the vector
417 it->reset();
418
419 // Then remove that entry from the vector efficiently
420 swapPopBack(it, &children_);
421 break;
422 }
423 // No else.
424 }
425
426 // Leave the costs of the child out of date.
427#ifdef BITSTAR_DEBUG
428 if (!foundChild)
429 {
430 throw ompl::Exception("Attempting to remove a child vertex not present in the vector of children "
431 "stored in the (supposed) parent vertex.");
432 }
433#endif // BITSTAR_DEBUG
434 }
435
436 void BITstar::Vertex::blacklistChild(const VertexConstPtr &vertex)
437 {
438 childIdBlacklist_.emplace(vertex->getId());
439 }
440
441 void BITstar::Vertex::whitelistChild(const VertexConstPtr &vertex)
442 {
443 childIdWhitelist_.emplace(vertex->getId());
444 }
445
446 bool BITstar::Vertex::isBlacklistedAsChild(const VertexConstPtr &vertex) const
447 {
448 return childIdBlacklist_.find(vertex->getId()) != childIdBlacklist_.end();
449 }
450
451 bool BITstar::Vertex::isWhitelistedAsChild(const VertexConstPtr &vertex) const
452 {
453 return childIdWhitelist_.find(vertex->getId()) != childIdWhitelist_.end();
454 }
455
456 void BITstar::Vertex::clearBlacklist()
457 {
458 childIdBlacklist_.clear();
459 }
460
461 void BITstar::Vertex::clearWhitelist()
462 {
463 childIdWhitelist_.clear();
464 }
465
466 ompl::base::Cost BITstar::Vertex::getCost() const
467 {
468 ASSERT_NOT_PRUNED
469
470 return cost_;
471 }
472
473 ompl::base::Cost BITstar::Vertex::getEdgeInCost() const
474 {
475 ASSERT_NOT_PRUNED
476
477#ifdef BITSTAR_DEBUG
478 if (!this->hasParent())
479 {
480 throw ompl::Exception("Attempting to access the incoming-edge cost of a vertex without a parent.");
481 }
482#endif // BITSTAR_DEBUG
483
484 return edgeCost_;
485 }
486
487 bool BITstar::Vertex::isConsistent() const
488 {
489 return cost_.value() == costAtExpansion_.value();
490 }
491
492 bool BITstar::Vertex::isPruned() const
493 {
494 return isPruned_;
495 }
496
497 bool BITstar::Vertex::isExpandedOnCurrentApproximation() const
498 {
499 return expansionApproximationId_ == *currentApproximationId_;
500 }
501
502 bool BITstar::Vertex::isExpandedOnCurrentSearch() const
503 {
504 return expansionSearchId_ == *currentSearchId_;
505 }
506
507 bool BITstar::Vertex::hasEverBeenExpandedAsRewiring() const
508 {
509 return hasEverBeenExpandedAsRewiring_;
510 }
511
512 void BITstar::Vertex::registerExpansion()
513 {
514 // Store the cost-to-come at expansion.
515 costAtExpansion_ = cost_;
516
517 // Remember the search and approximation ids.
518 expansionApproximationId_ = *currentApproximationId_;
519 expansionSearchId_ = *currentSearchId_;
520 }
521
522 void BITstar::Vertex::registerRewiringExpansion()
523 {
524 hasEverBeenExpandedAsRewiring_ = true;
525 }
526
527 void BITstar::Vertex::markPruned()
528 {
529 PRINT_VERTEX_CHANGE
530 ASSERT_NOT_PRUNED
531
532 isPruned_ = true;
533 }
534
535 void BITstar::Vertex::markUnpruned()
536 {
537 PRINT_VERTEX_CHANGE
538
539 isPruned_ = false;
540 }
541
542 void BITstar::Vertex::insertInEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &element)
543 {
544 ASSERT_NOT_PRUNED
545
546 // Conditionally clear any existing lookups.
547 this->clearLookupsIfOutdated();
548
549#ifdef BITSTAR_DEBUG
550 // Assert that this edge is NOT _from_ this vertex.
551 if (element->data.second.first->getId() == id_)
552 {
553 throw ompl::Exception("Attempted to add a cyclic incoming queue edge.");
554 }
555 // Assert that this edge is _to_ this vertex.
556 if (element->data.second.second->getId() != id_)
557 {
558 throw ompl::Exception("Attempted to add an incoming queue edge to the wrong vertex.");
559 }
560 // Assert that an edge from this source does not already exist.
561 for (const auto &inEdge : edgeQueueInLookup_)
562 {
563 if (element->data.second.first->getId() == inEdge->data.second.first->getId())
564 {
565 throw ompl::Exception("Attempted to add a second edge to the queue from a single source vertex.");
566 }
567 }
568#endif // BITSTAR_DEBUG
569
570 // Insert it into the lookup.
571 edgeQueueInLookup_.push_back(element);
572 }
573
574 void BITstar::Vertex::removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &element)
575 {
576 ASSERT_NOT_PRUNED
577
578#ifdef BITSTAR_DEBUG
579 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
580 // 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.
581 if (*currentApproximationId_ != lookupApproximationId_)
582 {
583 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
584 }
585
586 // Variable
587 // Element found
588 bool found = false;
589#endif // BITSTAR_DEBUG
590
591 // Iterate through the list and find the address of the element to delete
592 for (auto it = edgeQueueInLookup_.begin(); it != edgeQueueInLookup_.end(); ++it)
593 {
594 // Is it the element we're looking for? Source id
595 if ((*it)->data.second.first->getId() == element->data.second.first->getId())
596 {
597 // Remove by iterator
598 this->removeFromEdgeQueueInLookup(it);
599
600#ifdef BITSTAR_DEBUG
601 // Mark as found
602 found = true;
603#endif // BITSTAR_DEBUG
604 break;
605 }
606 // No else, try the next
607 }
608
609#ifdef BITSTAR_DEBUG
610 if (!found)
611 {
612 throw ompl::Exception("Attempted to remove an edge not in the incoming lookup.");
613 }
614#endif // BITSTAR_DEBUG
615 }
616
617 void BITstar::Vertex::removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
618 {
619 ASSERT_NOT_PRUNED
620
621#ifdef BITSTAR_DEBUG
622 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
623 // 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.
624 if (*currentApproximationId_ != lookupApproximationId_)
625 {
626 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
627 }
628#endif // BITSTAR_DEBUG
629
630 // Remove a non-const version of the given iterator.
631 // (trick from https://stackoverflow.com/a/10669041/1442500)
632 this->removeFromEdgeQueueInLookup(edgeQueueInLookup_.erase(element, element));
633 }
634
635 void BITstar::Vertex::clearEdgeQueueInLookup()
636 {
637 ASSERT_NOT_PRUNED
638
639 edgeQueueInLookup_.clear();
640 }
641
642 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstBegin()
643 {
644 ASSERT_NOT_PRUNED
645
646 // Conditionally clear any existing lookups
647 this->clearLookupsIfOutdated();
648
649 return edgeQueueInLookup_.cbegin();
650 }
651
652 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstEnd()
653 {
654 ASSERT_NOT_PRUNED
655
656 // Conditionally clear any existing lookups
657 this->clearLookupsIfOutdated();
658
659 return edgeQueueInLookup_.cend();
660 }
661
662 unsigned int BITstar::Vertex::edgeQueueInLookupSize()
663 {
664 ASSERT_NOT_PRUNED
665
666 // Conditionally clear any existing lookups
667 this->clearLookupsIfOutdated();
668
669 return edgeQueueInLookup_.size();
670 }
671
672 void BITstar::Vertex::insertInEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr& element)
673 {
674 ASSERT_NOT_PRUNED
675
676 // Conditionally clear any existing lookups
677 this->clearLookupsIfOutdated();
678
679#ifdef BITSTAR_DEBUG
680 // Assert that this edge is _from_ this vertex
681 if (element->data.second.first->getId() != id_)
682 {
683 throw ompl::Exception("Attempted to add an outgoing queue edge to the wrong vertex.");
684 }
685 // Assert that this edge is NOT _to_ this vertex
686 if (element->data.second.second->getId() == id_)
687 {
688 throw ompl::Exception("Attempted to add a cyclic outgoing queue edge.");
689 }
690 // Assert that an edge to this target does not already exist
691 for (const auto &outEdge : edgeQueueOutLookup_)
692 {
693 if (element->data.second.second->getId() == outEdge->data.second.second->getId())
694 {
695 throw ompl::Exception("Attempted to add a second edge to the queue to a single target vertex.");
696 }
697 }
698#endif // BITSTAR_DEBUG
699
700 // Push back
701 edgeQueueOutLookup_.push_back(element);
702 }
703
704 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &element)
705 {
706 ASSERT_NOT_PRUNED
707
708#ifdef BITSTAR_DEBUG
709 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
710 // 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.
711 if (*currentApproximationId_ != lookupApproximationId_)
712 {
713 throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
714 }
715
716 // Variable
717 // Element found
718 bool found = false;
719#endif // BITSTAR_DEBUG
720
721 // Iterate through the list and find the address of the element to delete
722 for (auto it = edgeQueueOutLookup_.begin(); it != edgeQueueOutLookup_.end(); ++it)
723 {
724 // Is it the element we're looking for? Source id
725 if ((*it)->data.second.second->getId() == element->data.second.second->getId())
726 {
727 // Remove by iterator
728 this->removeFromEdgeQueueOutLookup(it);
729
730#ifdef BITSTAR_DEBUG
731 // Mark as found
732 found = true;
733#endif // BITSTAR_DEBUG
734 break;
735 }
736 // No else, try the next
737 }
738
739#ifdef BITSTAR_DEBUG
740 if (!found)
741 {
742 throw ompl::Exception("Attempted to remove an edge not in the outgoing lookup.");
743 }
744#endif // BITSTAR_DEBUG
745 }
746
747 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
748 {
749 ASSERT_NOT_PRUNED
750
751#ifdef BITSTAR_DEBUG
752 // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
753 // 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.
754 if (*currentApproximationId_ != lookupApproximationId_)
755 {
756 throw ompl::Exception("Attempted to remove an outgoing queue edge added under a different expansion id.");
757 }
758#endif // BITSTAR_DEBUG
759
760 // Remove a non-const version of the given iterator
761 // (trick from https://stackoverflow.com/a/10669041/1442500)
762 this->removeFromEdgeQueueOutLookup(edgeQueueOutLookup_.erase(element, element));
763 }
764
765 void BITstar::Vertex::clearEdgeQueueOutLookup()
766 {
767 ASSERT_NOT_PRUNED
768
769 edgeQueueOutLookup_.clear();
770 }
771
772 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstBegin()
773 {
774 ASSERT_NOT_PRUNED
775
776 // Make sure the lookups aren't out of date.
777 this->clearLookupsIfOutdated();
778
779 return edgeQueueOutLookup_.cbegin();
780 }
781
782 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstEnd()
783 {
784 ASSERT_NOT_PRUNED
785
786 // Make sure the lookups aren't out of date.
787 this->clearLookupsIfOutdated();
788
789 return edgeQueueOutLookup_.cend();
790 }
791
792 unsigned int BITstar::Vertex::edgeQueueOutLookupSize()
793 {
794 ASSERT_NOT_PRUNED
795
796 // Make sure the lookups aren't out of date.
797 this->clearLookupsIfOutdated();
798
799 return edgeQueueOutLookup_.size();
800 }
801
802 void BITstar::Vertex::updateCostAndDepth(bool cascadeUpdates /*= true*/)
803 {
804 PRINT_VERTEX_CHANGE
805 ASSERT_NOT_PRUNED
806
807 if (this->isRoot())
808 {
809 // Am I root? -- I don't really know how this would ever be called, but ok.
810 cost_ = costHelpPtr_->identityCost();
811 depth_ = 0u;
812 }
813 else if (!this->hasParent())
814 {
815 // Am I disconnected?
816 cost_ = costHelpPtr_->infiniteCost();
817
818 // Set the depth to 0u, getDepth will throw in this condition
819 depth_ = 0u;
820
821#ifdef BITSTAR_DEBUG
822 // Assert that I have not been asked to cascade this bad data to my children:
823 if (this->hasChildren() && cascadeUpdates)
824 {
825 throw ompl::Exception("Attempting to update descendants' costs and depths of a vertex that does "
826 "not have a parent and is not root. This information would therefore be "
827 "gibberish.");
828 }
829#endif // BITSTAR_DEBUG
830 }
831 else
832 {
833 // I have a parent, so my cost is my parent cost + my edge cost to the parent
834 cost_ = costHelpPtr_->combineCosts(parentPtr_->getCost(), edgeCost_);
835
836 // If I have outgoing edges in the search queue, they need to be updated.
837 for (const auto& edge : edgeQueueOutLookup_) {
838 if (lookupApproximationId_ == *currentApproximationId_) {
839 queuePtr_->update(edge);
840 }
841 }
842
843 // I am one more than my parent's depth:
844 depth_ = (parentPtr_->getDepth() + 1u);
845 }
846
847 // Am I updating my children?
848 if (cascadeUpdates)
849 {
850 // Now, iterate over my vector of children and tell each one to update its own damn cost:
851 for (auto &child : children_)
852 {
853#ifdef BITSTAR_DEBUG
854 // Check that it hasn't expired
855 if (child.expired())
856 {
857 throw ompl::Exception("A (weak) pointer to a child has was found to have expired while "
858 "updating the costs and depths of descendant vertices.");
859 }
860#endif // BITSTAR_DEBUG
861
862 // Get a lock and tell the child to update:
863 child.lock()->updateCostAndDepth(true);
864 }
865 }
866 // No else, do not update the children. Let's hope the caller knows what they're doing.
867 }
868
869 void BITstar::Vertex::removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
870 {
871#ifdef BITSTAR_DEBUG
872 // Store the parent id of the edge we're removing.
873 VertexId parentId = (*element)->data.second.first->getId();
874
875 // Assert that this edge is not from this vertex.
876 if (parentId == id_)
877 {
878 throw ompl::Exception("Attempted to remove a cyclic incoming queue edge.");
879 }
880
881 // Assert that this edge is to this vertex.
882 if ((*element)->data.second.second->getId() != id_)
883 {
884 throw ompl::Exception("Attempted to remove an incoming queue edge from the wrong vertex.");
885 }
886
887 // Assert that it could exist.
888 if (edgeQueueInLookup_.empty())
889 {
890 throw ompl::Exception("Attempted to remove an incoming queue edge from a vertex with an empty list.");
891 }
892
893 // Assert that this edge actually exists.
894 bool found = false;
895 for (auto it = edgeQueueInLookup_.begin(); it != edgeQueueInLookup_.end() && !found; ++it)
896 {
897 found = ((*it)->data.second.first->getId() == parentId);
898 }
899 if (!found)
900 {
901 throw ompl::Exception("Attempted to remove an edge not in the incoming lookup.");
902 }
903#endif // BITSTAR_DEBUG
904
905 // Clear our entry in the list
906 *element = nullptr;
907
908 // Remove it efficiently
909 swapPopBack(element, &edgeQueueInLookup_);
910
911#ifdef BITSTAR_DEBUG
912 // Assert that it's now gone.
913 for (const auto &edgePtr : edgeQueueInLookup_)
914 {
915 if (edgePtr->data.second.first->getId() == parentId)
916 {
917 throw ompl::Exception("Failed to remove the designated edge in the incoming lookup.");
918 }
919 }
920#endif // BITSTAR_DEBUG
921 }
922
923 void BITstar::Vertex::removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
924 {
925#ifdef BITSTAR_DEBUG
926 // Store the child id of the edge we're removing.
927 VertexId childId = (*element)->data.second.second->getId();
928
929 // Assert that this edge is this vertex.
930 if ((*element)->data.second.first->getId() != id_)
931 {
932 throw ompl::Exception("Attempted to remove an outgoing queue edge from the wrong vertex.");
933 }
934 // Assert that this edge is not to this vertex.
935 if (childId == id_)
936 {
937 throw ompl::Exception("Attempted to remove a cyclic outgoing queue edge.");
938 }
939 // Assert that it could exist
940 if (edgeQueueOutLookup_.empty())
941 {
942 throw ompl::Exception("Attempted to remove an outgoing queue edge from a vertex with an empty list.");
943 }
944 // Assert that this edge actually exists
945 bool found = false;
946 for (auto it = edgeQueueOutLookup_.begin(); it != edgeQueueOutLookup_.end() && !found; ++it)
947 {
948 found = ((*it)->data.second.second->getId() == childId);
949 }
950 if (!found)
951 {
952 throw ompl::Exception("Attempted to remove an edge not in the outgoing lookup.");
953 }
954#endif // BITSTAR_DEBUG
955
956 // Clear our entry in the list.
957 *element = nullptr;
958
959 // Remove it efficiently.
960 swapPopBack(element, &edgeQueueOutLookup_);
961
962#ifdef BITSTAR_DEBUG
963 // Assert that it's now gone.
964 for (const auto &edgePtr : edgeQueueOutLookup_)
965 {
966 if (edgePtr->data.second.second->getId() == childId)
967 {
968 throw ompl::Exception("Failed to remove the designated edge in the outgoing lookup.");
969 }
970 }
971#endif // BITSTAR_DEBUG
972 }
973
974 void BITstar::Vertex::clearLookupsIfOutdated()
975 {
976 // Clean up any old lookups.
977 if (lookupApproximationId_ != *currentApproximationId_)
978 {
979 // Clear the existing entries.
980 this->clearEdgeQueueInLookup();
981 this->clearEdgeQueueOutLookup();
982
983 // Update the counter.
984 lookupApproximationId_ = *currentApproximationId_;
985 }
986 // No else, this is the same pass through the vertex queue.
987 }
988 } // geometric
989} // 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:410
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 hasParent() const
Returns whether this vertex has a parent.
Definition Vertex.cpp:169
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition Vertex.cpp:198
bool isRoot() const
Returns whether the vertex is the root of the search tree.
Definition Vertex.cpp:162
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition Vertex.cpp:138
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.