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 #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 
434  {
435  childIdBlacklist_.emplace(vertex->getId());
436  }
437 
439  {
440  childIdWhitelist_.emplace(vertex->getId());
441  }
442 
444  {
445  return childIdBlacklist_.find(vertex->getId()) != childIdBlacklist_.end();
446  }
447 
449  {
450  return childIdWhitelist_.find(vertex->getId()) != childIdWhitelist_.end();
451  }
452 
454  {
455  childIdBlacklist_.clear();
456  }
457 
459  {
460  childIdWhitelist_.clear();
461  }
462 
464  {
465  ASSERT_NOT_PRUNED
466 
467  return cost_;
468  }
469 
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 
485  {
486  return cost_.value() == costAtExpansion_.value();
487  }
488 
490  {
491  return isPruned_;
492  }
493 
495  {
496  return expansionApproximationId_ == *currentApproximationId_;
497  }
498 
500  {
501  return expansionSearchId_ == *currentSearchId_;
502  }
503 
505  {
506  return hasEverBeenExpandedAsRewiring_;
507  }
508 
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 
520  {
521  hasEverBeenExpandedAsRewiring_ = true;
522  }
523 
525  {
526  PRINT_VERTEX_CHANGE
527  ASSERT_NOT_PRUNED
528 
529  isPruned_ = true;
530  }
531 
533  {
534  PRINT_VERTEX_CHANGE
535 
536  isPruned_ = false;
537  }
538 
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 
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 
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 
657  {
658  ASSERT_NOT_PRUNED
659 
660  // Conditionally clear any existing lookups
661  this->clearLookupsIfOutdated();
662 
663  return edgeQueueInLookup_.size();
664  }
665 
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 
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 
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 
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
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition: Vertex.cpp:198
void clearEdgeQueueOutLookup()
Clear the pointers to all of the outgoing edge queue entries.
Definition: Vertex.cpp:756
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:129
bool isExpandedOnCurrentApproximation() const
Returns whether the vertex is expanded on current approximation.
Definition: Vertex.cpp:494
Definition of an abstract state.
Definition: State.h:114
void blacklistChild(const VertexConstPtr &vertex)
Put the vertex on the blacklist of children.
Definition: Vertex.cpp:433
bool isPruned() const
Whether the vertex has been pruned.
Definition: Vertex.cpp:489
void registerExpansion()
Mark the vertex as expanded.
Definition: Vertex.cpp:509
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:112
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:463
bool isBlacklistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition: Vertex.cpp:443
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:499
void markPruned()
Mark the vertex as pruned.
Definition: Vertex.cpp:524
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:134
bool hasEverBeenExpandedAsRewiring() const
Returns whether the vertex has ever been expanded as a rewiring.
Definition: Vertex.cpp:504
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:646
An ID generator class for vertex IDs.
Definition: IdGenerator.h:123
void whitelistChild(const VertexConstPtr &vertex)
Put the vertex on the whitelist of children.
Definition: Vertex.cpp:438
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:539
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:629
bool isWhitelistedAsChild(const VertexConstPtr &vertex) const
Returns true if the vertex is blacklisted as a child of this vertex.
Definition: Vertex.cpp:448
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:763
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:519
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:636
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition: Vertex.cpp:138
void markUnpruned()
Mark the vertex as unpruned.
Definition: Vertex.cpp:532
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:656
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:484
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:481
void removeFromEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Remove an incoming edge queue entry by value to the member vector.
Definition: Vertex.cpp:571
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:773
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:698
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:470
void clearWhitelist()
Clears the whitelist.
Definition: Vertex.cpp:458
The exception type for ompl.
Definition: Exception.h:79
unsigned int edgeQueueOutLookupSize()
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition: Vertex.cpp:783
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:666
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
void clearBlacklist()
Clears the blacklist.
Definition: Vertex.cpp:453
ompl::base::State * state()
The state of a vertex as a pointer.
Definition: Vertex.cpp:152