38#include "ompl/geometric/planners/informedtrees/bitstar/Vertex.h"
44#include "ompl/util/Exception.h"
48#include "ompl/geometric/planners/informedtrees/bitstar/HelperFunctions.h"
50#include "ompl/geometric/planners/informedtrees/bitstar/IdGenerator.h"
52#include "ompl/geometric/planners/informedtrees/bitstar/CostHelper.h"
57 #define TRACK_VERTEX_ID 0
60 #define PRINT_VERTEX_CHANGE \
61 if (id_ == TRACK_VERTEX_ID) \
63 std::cout << "Vertex " << id_ << ": " << __func__ << "" << std::endl; \
67 #define ASSERT_NOT_PRUNED \
68 if (this->isPruned_) \
70 std::cout << "Vertex " << id_ << ": " << __func__ << std::endl; \
71 throw ompl::Exception("Attempting to access a pruned vertex."); \
74 #define PRINT_VERTEX_CHANGE
75 #define ASSERT_NOT_PRUNED
83 std::once_flag g_IdInited;
85 boost::scoped_ptr<ompl::geometric::BITstar::IdGenerator> g_IdGenerator;
88 void initIdGenerator()
96 std::call_once(g_IdInited, &initIdGenerator);
97 return *g_IdGenerator;
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())
115 , edgeCost_(costHelpPtr_->infiniteCost())
116 , cost_(costHelpPtr_->infiniteCost())
117 , costAtExpansion_(costHelpPtr_->infiniteCost())
118 , currentSearchId_(queuePtr->getSearchId())
119 , currentApproximationId_(approximationId)
125 cost_ = costHelpPtr_->identityCost();
130 BITstar::Vertex::~Vertex()
135 si_->freeState(state_);
162 bool BITstar::Vertex::isRoot()
const
169 bool BITstar::Vertex::hasParent()
const
173 return static_cast<bool>(parentPtr_);
176 bool BITstar::Vertex::isInTree()
const
180 return this->isRoot() || this->hasParent();
183 unsigned int BITstar::Vertex::getDepth()
const
188 if (!this->isRoot() && !this->hasParent())
190 throw ompl::Exception(
"Attempting to get the depth of a vertex that does not have a parent yet is not "
203 if (!this->hasParent())
207 throw ompl::Exception(
"Attempting to access the parent of the root vertex.");
211 throw ompl::Exception(
"Attempting to access the parent of a vertex that does not have one.");
224 if (!this->hasParent())
228 throw ompl::Exception(
"Attempting to access the parent of the root vertex.");
232 throw ompl::Exception(
"Attempting to access the parent of a vertex that does not have one.");
249 throw ompl::Exception(
"Attempting to add a parent to the root vertex, which cannot have a parent.");
251 if (this->hasParent())
253 throw ompl::Exception(
"Attempting to add a parent to a vertex that already has one.");
258 parentPtr_ = newParent;
261 edgeCost_ = edgeInCost;
264 this->updateCostAndDepth(
true);
267 void BITstar::Vertex::removeParent(
bool updateChildCosts)
276 throw ompl::Exception(
"Attempting to remove the parent of the root vertex, which cannot have a "
279 if (!this->hasParent())
281 throw ompl::Exception(
"Attempting to remove the parent of a vertex that does not have a parent.");
289 this->updateCostAndDepth(updateChildCosts);
292 bool BITstar::Vertex::hasChildren()
const
296 return !children_.empty();
305 for (
const auto &child : children_)
311 throw ompl::Exception(
"A (weak) pointer to a child was found to have expired while collecting the "
312 "children of a vertex.");
317 children->push_back(child.lock());
327 for (
const auto &child : children_)
333 throw ompl::Exception(
"A (weak) pointer to a child was found to have expired while collecting the "
334 "children of a vertex.");
339 children->push_back(child.lock());
354 if (!child->hasParent())
356 throw ompl::Exception(
"Attempted to add child that does not have a listed parent.");
358 if (child->getParent()->getId() != id_)
360 throw ompl::Exception(
"Attempted to add someone else's child as mine.");
365 children_.push_back(child);
370 void BITstar::Vertex::removeChild(
const VertexPtr &child)
379 throw ompl::Exception(
"Attempted to remove a root vertex as a child.");
381 if (!child->hasParent())
383 throw ompl::Exception(
"Attempted to remove a child that does not have a listed parent.");
385 if (child->getParent()->getId() != id_)
387 throw ompl::Exception(
"Attempted to remove a child vertex from the wrong parent.");
397 for (
auto it = children_.begin(); it != children_.end() && !foundChild; ++it)
403 throw ompl::Exception(
"A (weak) pointer to a child was found to have expired while removing a "
404 "child from a vertex.");
409 if (it->lock()->getId() == child->getId())
418 swapPopBack(it, &children_);
427 throw ompl::Exception(
"Attempting to remove a child vertex not present in the vector of children "
428 "stored in the (supposed) parent vertex.");
435 childIdBlacklist_.emplace(vertex->getId());
440 childIdWhitelist_.emplace(vertex->getId());
445 return childIdBlacklist_.find(vertex->getId()) != childIdBlacklist_.end();
450 return childIdWhitelist_.find(vertex->getId()) != childIdWhitelist_.end();
453 void BITstar::Vertex::clearBlacklist()
455 childIdBlacklist_.clear();
458 void BITstar::Vertex::clearWhitelist()
460 childIdWhitelist_.clear();
475 if (!this->hasParent())
477 throw ompl::Exception(
"Attempting to access the incoming-edge cost of a vertex without a parent.");
484 bool BITstar::Vertex::isConsistent()
const
486 return cost_.value() == costAtExpansion_.value();
489 bool BITstar::Vertex::isPruned()
const
494 bool BITstar::Vertex::isExpandedOnCurrentApproximation()
const
496 return expansionApproximationId_ == *currentApproximationId_;
499 bool BITstar::Vertex::isExpandedOnCurrentSearch()
const
501 return expansionSearchId_ == *currentSearchId_;
504 bool BITstar::Vertex::hasEverBeenExpandedAsRewiring()
const
506 return hasEverBeenExpandedAsRewiring_;
509 void BITstar::Vertex::registerExpansion()
512 costAtExpansion_ = cost_;
515 expansionApproximationId_ = *currentApproximationId_;
516 expansionSearchId_ = *currentSearchId_;
519 void BITstar::Vertex::registerRewiringExpansion()
521 hasEverBeenExpandedAsRewiring_ =
true;
524 void BITstar::Vertex::markPruned()
532 void BITstar::Vertex::markUnpruned()
544 this->clearLookupsIfOutdated();
548 if (element->data.second.first->getId() == id_)
550 throw ompl::Exception(
"Attempted to add a cyclic incoming queue edge.");
553 if (element->data.second.second->getId() != id_)
555 throw ompl::Exception(
"Attempted to add an incoming queue edge to the wrong vertex.");
558 for (
const auto &inEdge : edgeQueueInLookup_)
560 if (element->data.second.first->getId() == inEdge->data.second.first->getId())
562 throw ompl::Exception(
"Attempted to add a second edge to the queue from a single source vertex.");
568 edgeQueueInLookup_.push_back(element);
578 if (*currentApproximationId_ != lookupApproximationId_)
580 throw ompl::Exception(
"Attempted to remove an incoming queue edge added under a different expansion id.");
589 for (
auto it = edgeQueueInLookup_.begin(); it != edgeQueueInLookup_.end() && !found; ++it)
592 if ((*it)->data.second.first->getId() == element->data.second.first->getId())
595 this->removeFromEdgeQueueInLookup(it);
606 throw ompl::Exception(
"Attempted to remove an edge not in the incoming lookup.");
611 void BITstar::Vertex::removeFromEdgeQueueInLookup(
const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
618 if (*currentApproximationId_ != lookupApproximationId_)
620 throw ompl::Exception(
"Attempted to remove an incoming queue edge added under a different expansion id.");
626 this->removeFromEdgeQueueInLookup(edgeQueueInLookup_.erase(element, element));
629 void BITstar::Vertex::clearEdgeQueueInLookup()
633 edgeQueueInLookup_.clear();
636 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstBegin()
641 this->clearLookupsIfOutdated();
643 return edgeQueueInLookup_.cbegin();
646 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueInLookupConstEnd()
651 this->clearLookupsIfOutdated();
653 return edgeQueueInLookup_.cend();
656 unsigned int BITstar::Vertex::edgeQueueInLookupSize()
661 this->clearLookupsIfOutdated();
663 return edgeQueueInLookup_.size();
671 this->clearLookupsIfOutdated();
675 if (element->data.second.first->getId() != id_)
677 throw ompl::Exception(
"Attempted to add an outgoing queue edge to the wrong vertex.");
680 if (element->data.second.second->getId() == id_)
682 throw ompl::Exception(
"Attempted to add a cyclic outgoing queue edge.");
685 for (
const auto &outEdge : edgeQueueOutLookup_)
687 if (element->data.second.second->getId() == outEdge->data.second.second->getId())
689 throw ompl::Exception(
"Attempted to add a second edge to the queue to a single target vertex.");
695 edgeQueueOutLookup_.push_back(element);
705 if (*currentApproximationId_ != lookupApproximationId_)
707 throw ompl::Exception(
"Attempted to remove an incoming queue edge added under a different expansion id.");
716 for (
auto it = edgeQueueOutLookup_.begin(); it != edgeQueueOutLookup_.end() && !found; ++it)
719 if ((*it)->data.second.second->getId() == element->data.second.second->getId())
722 this->removeFromEdgeQueueOutLookup(it);
733 throw ompl::Exception(
"Attempted to remove an edge not in the outgoing lookup.");
738 void BITstar::Vertex::removeFromEdgeQueueOutLookup(
const SearchQueue::EdgeQueueElemPtrVector::const_iterator& element)
745 if (*currentApproximationId_ != lookupApproximationId_)
747 throw ompl::Exception(
"Attempted to remove an outgoing queue edge added under a different expansion id.");
753 this->removeFromEdgeQueueOutLookup(edgeQueueOutLookup_.erase(element, element));
756 void BITstar::Vertex::clearEdgeQueueOutLookup()
760 edgeQueueOutLookup_.clear();
763 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstBegin()
768 this->clearLookupsIfOutdated();
770 return edgeQueueOutLookup_.cbegin();
773 BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::edgeQueueOutLookupConstEnd()
778 this->clearLookupsIfOutdated();
780 return edgeQueueOutLookup_.cend();
783 unsigned int BITstar::Vertex::edgeQueueOutLookupSize()
788 this->clearLookupsIfOutdated();
790 return edgeQueueOutLookup_.size();
793 void BITstar::Vertex::updateCostAndDepth(
bool cascadeUpdates )
801 cost_ = costHelpPtr_->identityCost();
804 else if (!this->hasParent())
807 cost_ = costHelpPtr_->infiniteCost();
814 if (this->hasChildren() && cascadeUpdates)
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 "
825 cost_ = costHelpPtr_->combineCosts(parentPtr_->getCost(), edgeCost_);
828 for (
const auto& edge : edgeQueueOutLookup_) {
829 if (lookupApproximationId_ == *currentApproximationId_) {
830 queuePtr_->update(edge);
835 depth_ = (parentPtr_->getDepth() + 1u);
842 for (
auto &child : children_)
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.");
854 child.lock()->updateCostAndDepth(
true);
860 void BITstar::Vertex::removeFromEdgeQueueInLookup(
const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
864 VertexId parentId = (*element)->data.second.first->getId();
869 throw ompl::Exception(
"Attempted to remove a cyclic incoming queue edge.");
873 if ((*element)->data.second.second->getId() != id_)
875 throw ompl::Exception(
"Attempted to remove an incoming queue edge from the wrong vertex.");
879 if (edgeQueueInLookup_.empty())
881 throw ompl::Exception(
"Attempted to remove an incoming queue edge from a vertex with an empty list.");
886 for (
auto it = edgeQueueInLookup_.begin(); it != edgeQueueInLookup_.end() && !found; ++it)
888 found = ((*it)->data.second.first->getId() == parentId);
892 throw ompl::Exception(
"Attempted to remove an edge not in the incoming lookup.");
900 swapPopBack(element, &edgeQueueInLookup_);
904 for (
const auto &edgePtr : edgeQueueInLookup_)
906 if (edgePtr->data.second.first->getId() == parentId)
908 throw ompl::Exception(
"Failed to remove the designated edge in the incoming lookup.");
914 void BITstar::Vertex::removeFromEdgeQueueOutLookup(
const SearchQueue::EdgeQueueElemPtrVector::iterator &element)
918 VertexId childId = (*element)->data.second.second->getId();
921 if ((*element)->data.second.first->getId() != id_)
923 throw ompl::Exception(
"Attempted to remove an outgoing queue edge from the wrong vertex.");
928 throw ompl::Exception(
"Attempted to remove a cyclic outgoing queue edge.");
931 if (edgeQueueOutLookup_.empty())
933 throw ompl::Exception(
"Attempted to remove an outgoing queue edge from a vertex with an empty list.");
937 for (
auto it = edgeQueueOutLookup_.begin(); it != edgeQueueOutLookup_.end() && !found; ++it)
939 found = ((*it)->data.second.second->getId() == childId);
943 throw ompl::Exception(
"Attempted to remove an edge not in the outgoing lookup.");
951 swapPopBack(element, &edgeQueueOutLookup_);
955 for (
const auto &edgePtr : edgeQueueOutLookup_)
957 if (edgePtr->data.second.second->getId() == childId)
959 throw ompl::Exception(
"Failed to remove the designated edge in the outgoing lookup.");
965 void BITstar::Vertex::clearLookupsIfOutdated()
968 if (lookupApproximationId_ != *currentApproximationId_)
971 this->clearEdgeQueueInLookup();
972 this->clearEdgeQueueOutLookup();
975 lookupApproximationId_ = *currentApproximationId_;
The exception type for ompl.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
SpaceInformationPtr si_
The space information for which planning is done.
Definition of an abstract state.
A helper class to handle the various heuristic functions in one place.
An ID generator class for vertex IDs.
A queue of edges, sorted according to a sort key.
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
bool isRoot() const
Returns whether the vertex is the root of the search tree.
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
unsigned int VertexId
The vertex id type.
Main namespace. Contains everything in this library.