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:
79 namespace
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 
101 namespace 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 
131  {
132  PRINT_VERTEX_CHANGE
133 
134  // Free the state on destruction
135  si_->freeState(state_);
136  }
137 
139  {
140  ASSERT_NOT_PRUNED
141 
142  return id_;
143  }
144 
146  {
147  ASSERT_NOT_PRUNED
148 
149  return state_;
150  }
151 
153  {
154  PRINT_VERTEX_CHANGE
155  ASSERT_NOT_PRUNED
156 
157  return state_;
158  }
159 
161  // The vertex's graph properties:
163  {
164  ASSERT_NOT_PRUNED
165 
166  return isRoot_;
167  }
168 
170  {
171  ASSERT_NOT_PRUNED
172 
173  return static_cast<bool>(parentPtr_);
174  }
175 
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 
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 
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 
293  {
294  ASSERT_NOT_PRUNED
295 
296  return !children_.empty();
297  }
298 
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 
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 
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 
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 
437  {
438  childIdBlacklist_.emplace(vertex->getId());
439  }
440 
442  {
443  childIdWhitelist_.emplace(vertex->getId());
444  }
445 
447  {
448  return childIdBlacklist_.find(vertex->getId()) != childIdBlacklist_.end();
449  }
450 
452  {
453  return childIdWhitelist_.find(vertex->getId()) != childIdWhitelist_.end();
454  }
455 
457  {
458  childIdBlacklist_.clear();
459  }
460 
462  {
463  childIdWhitelist_.clear();
464  }
465 
467  {
468  ASSERT_NOT_PRUNED
469 
470  return cost_;
471  }
472 
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 
488  {
489  return cost_.value() == costAtExpansion_.value();
490  }
491 
493  {
494  return isPruned_;
495  }
496 
498  {
499  return expansionApproximationId_ == *currentApproximationId_;
500  }
501 
503  {
504  return expansionSearchId_ == *currentSearchId_;
505  }
506 
508  {
509  return hasEverBeenExpandedAsRewiring_;
510  }
511 
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 
523  {
524  hasEverBeenExpandedAsRewiring_ = true;
525  }
526 
528  {
529  PRINT_VERTEX_CHANGE
530  ASSERT_NOT_PRUNED
531 
532  isPruned_ = true;
533  }
534 
536  {
537  PRINT_VERTEX_CHANGE
538 
539  isPruned_ = false;
540  }
541 
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 
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 
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 
663  {
664  ASSERT_NOT_PRUNED
665 
666  // Conditionally clear any existing lookups
667  this->clearLookupsIfOutdated();
668 
669  return edgeQueueInLookup_.size();
670  }
671 
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 
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 
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 
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
void clearEdgeQueueOutLookup()
Clear the pointers to all of the outgoing edge queue entries.
Definition: Vertex.cpp:765
A shared pointer wrapper for ompl::base::SpaceInformation.
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:215
A queue of edges, sorted according to a sort key.
Definition: SearchQueue.h:128
bool isExpandedOnCurrentApproximation() const
Returns whether the vertex is expanded on current approximation.
Definition: Vertex.cpp:497
Definition of an abstract state.
Definition: State.h:113
void blacklistChild(const VertexConstPtr &vertex)
Put the vertex on the blacklist of children.
Definition: Vertex.cpp:436
bool isPruned() const
Whether the vertex has been pruned.
Definition: Vertex.cpp:492
void registerExpansion()
Mark the vertex as expanded.
Definition: Vertex.cpp:512
bool isRoot() const
Returns whether the vertex is the root of the search tree.
Definition: Vertex.cpp:162
bool isInTree() const
Get whether a vertex is in the search tree or a sample (i.e., a vertex of the RRG).
Definition: Vertex.cpp:176
bool hasParent() const
Returns whether this vertex has a parent.
Definition: Vertex.cpp:169
unsigned int getDepth() const
Get the depth of the vertex from the root.
Definition: Vertex.cpp:183
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:466
bool isBlacklistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition: Vertex.cpp:446
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:180
bool isExpandedOnCurrentSearch() const
Returns whether the vertex is expaned on current search.
Definition: Vertex.cpp:502
void markPruned()
Mark the vertex as pruned.
Definition: Vertex.cpp:527
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:133
bool hasEverBeenExpandedAsRewiring() const
Returns whether the vertex has ever been expanded as a rewiring.
Definition: Vertex.cpp:507
void addChild(const VertexPtr &newChild)
Add a child to this vertex. Does not change this vertex's cost or those of its descendants....
Definition: Vertex.cpp:343
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstEnd()
Get an iterator to the end of the incoming edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:652
An ID generator class for vertex IDs.
Definition: IdGenerator.h:122
void whitelistChild(const VertexConstPtr &vertex)
Put the vertex on the whitelist of children.
Definition: Vertex.cpp:441
void insertInEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Add to the list of the edge queue entries that point in to this vertex. Will clear existing in/out lo...
Definition: Vertex.cpp:542
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
Definition: BITstar.h:224
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:221
void clearEdgeQueueInLookup()
Clear the pointers to all of the incoming edge queue entries.
Definition: Vertex.cpp:635
bool isWhitelistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition: Vertex.cpp:451
void getChildren(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition: Vertex.cpp:299
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:212
void removeChild(const VertexPtr &oldChild)
Remove a child from this vertex. Does not change this vertex's cost or those of its descendants....
Definition: Vertex.cpp:370
Vertex(ompl::base::SpaceInformationPtr spaceInformation, const CostHelper *const costHelpPtr, SearchQueue *const queuePtr, const std::shared_ptr< const unsigned int > &approximationId, bool root=false)
Construct a vertex using space information, and helpers to compute various costs.
Definition: Vertex.cpp:107
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstBegin()
Get an iterator to the front of the outgoing edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:772
void removeParent(bool updateChildCosts)
Remove the parent of this vertex. Will always update this vertex's cost, and can update the descenden...
Definition: Vertex.cpp:267
void registerRewiringExpansion()
Mark expansion to vertices.
Definition: Vertex.cpp:522
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstBegin()
Get an iterator to the front of the incoming edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:642
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition: Vertex.cpp:138
void markUnpruned()
Mark the vertex as unpruned.
Definition: Vertex.cpp:535
unsigned int edgeQueueInLookupSize()
Get the number of edge queue entries incoming to this vertex. Will clear existing in/out lookups if t...
Definition: Vertex.cpp:662
bool hasChildren() const
Get whether this vertex has any children.
Definition: Vertex.cpp:292
virtual ~Vertex()
Destruct a vertex.
Definition: Vertex.cpp:130
bool isConsistent() const
Whether the vertex is consistent.
Definition: Vertex.cpp:487
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Remove an incoming edge queue entry by value to the member vector.
Definition: Vertex.cpp:574
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstEnd()
Get an iterator to the end of the outgoing edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:782
void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost)
Set the parent of this vertex, cannot be used to replace a previous parent. Will always update this v...
Definition: Vertex.cpp:240
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:227
void removeFromEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge)
Remove an outgoing edge queue entry by value.
Definition: Vertex.cpp:704
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:473
void clearWhitelist()
Clears the whitelist.
Definition: Vertex.cpp:461
The exception type for ompl.
Definition: Exception.h:78
unsigned int edgeQueueOutLookupSize()
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition: Vertex.cpp:792
void insertInEdgeQueueOutLookup(const SearchQueue::EdgeQueueElemPtr &outEdge)
Add to the list of the edge queue entries that point out of this vertex. Will clear existing in/out l...
Definition: Vertex.cpp:672
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition: Vertex.cpp:198
Main namespace. Contains everything in this library.
void clearBlacklist()
Clears the blacklist.
Definition: Vertex.cpp:456
ompl::base::State * state()
The state of a vertex as a pointer.
Definition: Vertex.cpp:145