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 */
36 
37 // My definition:
38 #include "ompl/geometric/planners/bitstar/datastructures/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/bitstar/datastructures/HelperFunctions.h"
49 // The ID generator class
50 #include "ompl/geometric/planners/bitstar/datastructures/IdGenerator.h"
51 // The cost-helper class:
52 #include "ompl/geometric/planners/bitstar/datastructures/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 (vId_ == TRACK_VERTEX_ID) \
62  { \
63  std::cout << "vId " << vId_ << ": " << __func__ << "()" << std::endl; \
64  }
65 
67  #define ASSERT_NOT_PRUNED this->assertNotPruned();
68 #else
69  #define PRINT_VERTEX_CHANGE
70  #define ASSERT_NOT_PRUNED
71 #endif // BITSTAR_DEBUG
72 
73 namespace ompl
74 {
75  namespace geometric
76  {
78  // Public functions:
80  bool root /*= false*/)
81  : vId_(getIdGenerator().getNewId())
82  , si_(std::move(si))
83  , costHelpPtr_(std::move(costHelpPtr))
84  , state_(si_->allocState())
85  , isRoot_(root)
86  , edgeCost_(costHelpPtr_->infiniteCost())
87  , cost_(costHelpPtr_->infiniteCost())
88  {
89  PRINT_VERTEX_CHANGE
90 
91  if (this->isRoot())
92  {
93  cost_ = costHelpPtr_->identityCost();
94  }
95  // No else, infinite by default
96  }
97 
99  {
100  PRINT_VERTEX_CHANGE
101 
102  // Free the state on destruction
103  si_->freeState(state_);
104  }
105 
107  {
108  ASSERT_NOT_PRUNED
109 
110  return vId_;
111  }
112 
114  {
115  ASSERT_NOT_PRUNED
116 
117  return state_;
118  }
119 
121  {
122  PRINT_VERTEX_CHANGE
123  ASSERT_NOT_PRUNED
124 
125  return state_;
126  }
127 
129  // The vertex's graph properties:
131  {
132  ASSERT_NOT_PRUNED
133 
134  return isRoot_;
135  }
136 
138  {
139  ASSERT_NOT_PRUNED
140 
141  return static_cast<bool>(parentSPtr_);
142  }
143 
145  {
146  ASSERT_NOT_PRUNED
147 
148  return this->isRoot() || this->hasParent();
149  }
150 
151  unsigned int BITstar::Vertex::getDepth() const
152  {
153  ASSERT_NOT_PRUNED
154 
155 #ifdef BITSTAR_DEBUG
156  if (this->isRoot() == false && this->hasParent() == false)
157  {
158  throw ompl::Exception("Attempting to get the depth of a vertex that does not have a parent yet is not "
159  "root.");
160  }
161 #endif // BITSTAR_DEBUG
162 
163  return depth_;
164  }
165 
167  {
168  ASSERT_NOT_PRUNED
169 
170 #ifdef BITSTAR_DEBUG
171  if (this->hasParent() == false)
172  {
173  if (this->isRoot() == true)
174  {
175  throw ompl::Exception("Attempting to access the parent of the root vertex.");
176  }
177  else
178  {
179  throw ompl::Exception("Attempting to access the parent of a vertex that does not have one.");
180  }
181  }
182 #endif // BITSTAR_DEBUG
183 
184  return parentSPtr_;
185  }
186 
188  {
189  ASSERT_NOT_PRUNED
190 
191 #ifdef BITSTAR_DEBUG
192  if (this->hasParent() == false)
193  {
194  if (this->isRoot() == true)
195  {
196  throw ompl::Exception("Attempting to access the parent of the root vertex.");
197  }
198  else
199  {
200  throw ompl::Exception("Attempting to access the parent of a vertex that does not have one.");
201  }
202  }
203 #endif // BITSTAR_DEBUG
204 
205  return parentSPtr_;
206  }
207 
208  void BITstar::Vertex::addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost,
209  bool updateChildCosts)
210  {
211  PRINT_VERTEX_CHANGE
212  ASSERT_NOT_PRUNED
213 
214 #ifdef BITSTAR_DEBUG
215  // Assert I can take a parent
216  if (this->isRoot() == true)
217  {
218  throw ompl::Exception("Attempting to add a parent to the root vertex, which cannot have a parent.");
219  }
220  if (this->hasParent() == true)
221  {
222  throw ompl::Exception("Attempting to add a parent to a vertex that already has one.");
223  }
224 #endif // BITSTAR_DEBUG
225 
226  // Store the parent
227  parentSPtr_ = newParent;
228 
229  // Store the edge cost
230  edgeCost_ = edgeInCost;
231 
232  // Update my cost and possibly the cost of my descendants:
233  this->updateCostAndDepth(updateChildCosts);
234  }
235 
236  void BITstar::Vertex::removeParent(bool updateChildCosts)
237  {
238  PRINT_VERTEX_CHANGE
239  ASSERT_NOT_PRUNED
240 
241 #ifdef BITSTAR_DEBUG
242  // Assert I have a parent
243  if (this->isRoot() == true)
244  {
245  throw ompl::Exception("Attempting to remove the parent of the root vertex, which cannot have a "
246  "parent.");
247  }
248  if (this->hasParent() == false)
249  {
250  throw ompl::Exception("Attempting to remove the parent of a vertex that does not have a parent.");
251  }
252 #endif // BITSTAR_DEBUG
253 
254  // Clear my parent
255  parentSPtr_.reset();
256 
257  // Update my cost and possibly the cost of my descendants:
258  this->updateCostAndDepth(updateChildCosts);
259  }
260 
262  {
263  ASSERT_NOT_PRUNED
264 
265  return !childWPtrs_.empty();
266  }
267 
269  {
270  ASSERT_NOT_PRUNED
271 
272  children->clear();
273 
274  for (const auto &childWPtr : childWPtrs_)
275  {
276 #ifdef BITSTAR_DEBUG
277  // Check that the weak pointer hasn't expired
278  if (childWPtr.expired() == true)
279  {
280  throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
281  "children of a vertex.");
282  }
283 #endif // BITSTAR_DEBUG
284 
285  // Lock and push back
286  children->push_back(childWPtr.lock());
287  }
288  }
289 
291  {
292  ASSERT_NOT_PRUNED
293 
294  children->clear();
295 
296  for (const auto &childWPtr : childWPtrs_)
297  {
298 #ifdef BITSTAR_DEBUG
299  // Check that the weak pointer hasn't expired
300  if (childWPtr.expired() == true)
301  {
302  throw ompl::Exception("A (weak) pointer to a child was found to have expired while collecting the "
303  "children of a vertex.");
304  }
305 #endif // BITSTAR_DEBUG
306 
307  // Lock and push back
308  children->push_back(childWPtr.lock());
309  }
310  }
311 
312  void BITstar::Vertex::addChild(const VertexPtr &newChild)
313  {
314  PRINT_VERTEX_CHANGE
315  ASSERT_NOT_PRUNED
316 
317 #ifdef BITSTAR_DEBUG
318  // Assert that I am this child's parent
319  if (newChild->isRoot())
320  {
321  throw ompl::Exception("Attempted to add a root vertex as a child.");
322  }
323  if (!newChild->hasParent())
324  {
325  throw ompl::Exception("Attempted to add child that does not have a listed parent.");
326  }
327  if (newChild->getParent()->getId() != vId_)
328  {
329  throw ompl::Exception("Attempted to add someone else's child as mine.");
330  }
331 #endif // BITSTAR_DEBUG
332 
333  // Push back the shared_ptr into the vector of weak_ptrs, this makes a weak_ptr copy
334  childWPtrs_.push_back(newChild);
335 
336  // Leave the costs of the child out of date.
337  }
338 
340  {
341  PRINT_VERTEX_CHANGE
342  ASSERT_NOT_PRUNED
343 
344 #ifdef BITSTAR_DEBUG
345  // Assert that I am this child's parent
346  if (oldChild->isRoot())
347  {
348  throw ompl::Exception("Attempted to remove a root vertex as a child.");
349  }
350  if (!oldChild->hasParent())
351  {
352  throw ompl::Exception("Attempted to remove a child that does not have a listed parent.");
353  }
354  if (oldChild->getParent()->getId() != vId_)
355  {
356  throw ompl::Exception("Attempted to remove a child vertex from the wrong parent.");
357  }
358 #endif // BITSTAR_DEBUG
359 
360  // Variables
361  // Whether the child has been found (and then deleted);
362  bool foundChild;
363 
364  // Iterate over the vector of children pointers until the child is found. Iterators make erase easier
365  foundChild = false;
366  for (auto childIter = childWPtrs_.begin(); childIter != childWPtrs_.end() && !foundChild;
367  ++childIter)
368  {
369 #ifdef BITSTAR_DEBUG
370  // Check that the weak pointer hasn't expired
371  if (childIter->expired() == true)
372  {
373  throw ompl::Exception("A (weak) pointer to a child was found to have expired while removing a "
374  "child from a vertex.");
375  }
376 #endif // BITSTAR_DEBUG
377 
378  // Check if this is the child we're looking for
379  if (childIter->lock()->getId() == oldChild->getId())
380  {
381  // It is, mark as found
382  foundChild = true;
383 
384  // First, clear the entry in the vector
385  childIter->reset();
386 
387  // Then remove that entry from the vector efficiently
388  swapPopBack(childIter, &childWPtrs_);
389  }
390  // No else, move on
391  }
392 
393  // Leave the costs of the child out of date.
394 
395 #ifdef BITSTAR_DEBUG
396  // Throw if we did not find the child
397  if (foundChild == false)
398  {
399  throw ompl::Exception("Attempting to remove a child vertex not present in the vector of children "
400  "stored in the (supposed) parent vertex.");
401  }
402 #endif // BITSTAR_DEBUG
403  }
404 
406  {
407  ASSERT_NOT_PRUNED
408 
409  return cost_;
410  }
411 
413  {
414  ASSERT_NOT_PRUNED
415 
416 #ifdef BITSTAR_DEBUG
417  if (this->hasParent() == false)
418  {
419  throw ompl::Exception("Attempting to access the incoming-edge cost of a vertex without a parent.");
420  }
421 #endif // BITSTAR_DEBUG
422 
423  return edgeCost_;
424  }
425 
427  {
428  ASSERT_NOT_PRUNED
429 
430  return isNew_;
431  }
432 
434  {
435  PRINT_VERTEX_CHANGE
436  ASSERT_NOT_PRUNED
437 
438  isNew_ = true;
439  }
440 
442  {
443  PRINT_VERTEX_CHANGE
444  ASSERT_NOT_PRUNED
445 
446  isNew_ = false;
447  }
448 
450  {
451  ASSERT_NOT_PRUNED
452 
453  return hasBeenExpandedToSamples_;
454  }
455 
457  {
458  PRINT_VERTEX_CHANGE
459  ASSERT_NOT_PRUNED
460 
461  hasBeenExpandedToSamples_ = true;
462  }
463 
465  {
466  PRINT_VERTEX_CHANGE
467  ASSERT_NOT_PRUNED
468 
469  hasBeenExpandedToSamples_ = false;
470  }
471 
473  {
474  ASSERT_NOT_PRUNED
475 
476  return hasBeenExpandedToVertices_;
477  }
478 
480  {
481  PRINT_VERTEX_CHANGE
482  ASSERT_NOT_PRUNED
483 
484  hasBeenExpandedToVertices_ = true;
485  }
486 
488  {
489  PRINT_VERTEX_CHANGE
490  ASSERT_NOT_PRUNED
491 
492  hasBeenExpandedToVertices_ = false;
493  }
494 
496  {
497  return isPruned_;
498  }
499 
501  {
502  PRINT_VERTEX_CHANGE
503  ASSERT_NOT_PRUNED
504 
505  isPruned_ = true;
506  }
507 
509  {
510  PRINT_VERTEX_CHANGE
511 
512  isPruned_ = false;
513  }
515 
517  // Functions for the vertex's SearchQueue data
519  // Vertex queue info:
521  {
522  ASSERT_NOT_PRUNED
523 
524 #ifdef BITSTAR_DEBUG
525  // Assert available
526  if (isVertexQueueSet_ == false)
527  {
528  throw ompl::Exception("Attempting to access an iterator to the vertex queue before one is set.");
529  }
530 #endif // BITSTAR_DEBUG
531 
532  return vertexQueueIter_;
533  }
534 
536  {
537  ASSERT_NOT_PRUNED
538 
539 #ifdef BITSTAR_DEBUG
540  // Assert not already set
541  if (isVertexQueueSet_ == true)
542  {
543  throw ompl::Exception("Attempting to change an iterator to the vertex queue.");
544  }
545 #endif // BITSTAR_DEBUG
546 
547  // Record that it's set
548  isVertexQueueSet_ = true;
549 
550  // Store
551  vertexQueueIter_ = newPtr;
552  }
553 
555  {
556  ASSERT_NOT_PRUNED
557 
558  // Reset to unset
559  isVertexQueueSet_ = false;
560  }
561 
563  {
564  ASSERT_NOT_PRUNED
565 
566  return isVertexQueueSet_;
567  }
569 
571  // Edge queue info (incoming edges):
572  void BITstar::Vertex::addIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr& newInPtr, unsigned int vertexQueueResetNum)
573  {
574  ASSERT_NOT_PRUNED
575 
576  // Conditionally clear any existing lookups
577  this->clearOldLookups(vertexQueueResetNum);
578 
579 #ifdef BITSTAR_DEBUG
580  // Assert that this edge is NOT _from_ this vertex
581  if (newInPtr->data.second.first->getId() == vId_)
582  {
583  throw ompl::Exception("Attempted to add a cyclic incoming queue edge.");
584  }
585  // Assert that this edge is _to_ this vertex
586  if (newInPtr->data.second.second->getId() != vId_)
587  {
588  throw ompl::Exception("Attempted to add an incoming queue edge to the wrong vertex.");
589  }
590  // Assert that an edge from this source does not already exist
591  for (const auto &elemPtrs : edgeQueueInPtrs_)
592  {
593  if (newInPtr->data.second.first->getId() == elemPtrs->data.second.first->getId())
594  {
595  throw ompl::Exception("Attempted to add a second edge to the queue from a single source vertex.");
596  }
597  }
598 #endif // BITSTAR_DEBUG
599 
600  // Push back
601  edgeQueueInPtrs_.push_back(newInPtr);
602  }
603 
604  void BITstar::Vertex::rmIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
605  {
606  ASSERT_NOT_PRUNED
607 
608 #ifdef BITSTAR_DEBUG
609  // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
610  // 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.
611  if (vertexQueueResetNum != edgeLookupPass_)
612  {
613  throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
614  }
615 #endif // BITSTAR_DEBUG
616 
617  // Variable
618  // Element found
619  bool found = false;
620 
621  // Iterate through the list and find the address of the element to delete
622  for (auto iterToDelete = edgeQueueInPtrs_.begin(); iterToDelete != edgeQueueInPtrs_.end() && !found; ++iterToDelete)
623  {
624  // Is it the element we're looking for? Source id
625  if ((*iterToDelete)->data.second.first->getId() == elemToDelete->data.second.first->getId())
626  {
627  // Remove by iterator
628  this->rmIncomingHelper(iterToDelete);
629 
630  // Mark as found
631  found = true;
632  }
633  // No else, try the next
634  }
635 
636 #ifdef BITSTAR_DEBUG
637  if (!found)
638  {
639  throw ompl::Exception("Attempted to remove an edge not in the incoming lookup.");
640  }
641 #endif // BITSTAR_DEBUG
642  }
643 
644  void BITstar::Vertex::rmIncomingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& constIterToDelete, unsigned int vertexQueueResetNum)
645  {
646  ASSERT_NOT_PRUNED
647 
648 #ifdef BITSTAR_DEBUG
649  // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
650  // 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.
651  if (vertexQueueResetNum != edgeLookupPass_)
652  {
653  throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
654  }
655 #endif // BITSTAR_DEBUG
656 
657  // Remove a non-const version of the given iterator
658  // (trick from https://stackoverflow.com/a/10669041/1442500)
659  this->rmIncomingHelper(edgeQueueInPtrs_.erase(constIterToDelete, constIterToDelete));
660  }
661 
663  {
664  ASSERT_NOT_PRUNED
665 
666  edgeQueueInPtrs_.clear();
667  }
668 
669  BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::incomingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
670  {
671  ASSERT_NOT_PRUNED
672 
673  // Conditionally clear any existing lookups
674  this->clearOldLookups(vertexQueueResetNum);
675 
676  return edgeQueueInPtrs_.cbegin();
677  }
678 
679  BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::incomingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
680  {
681  ASSERT_NOT_PRUNED
682 
683  // Conditionally clear any existing lookups
684  this->clearOldLookups(vertexQueueResetNum);
685 
686  return edgeQueueInPtrs_.cend();
687  }
688 
689  unsigned int BITstar::Vertex::getNumIncomingEdgeQueuePtrs(unsigned int vertexQueueResetNum)
690  {
691  ASSERT_NOT_PRUNED
692 
693  // Conditionally clear any existing lookups
694  this->clearOldLookups(vertexQueueResetNum);
695 
696  return edgeQueueInPtrs_.size();
697  }
698 
699  bool BITstar::Vertex::hasIncomingEdgeQueueEntries(unsigned int vertexQueueResetNum)
700  {
701  ASSERT_NOT_PRUNED
702 
703  // Conditionally clear any existing lookups
704  this->clearOldLookups(vertexQueueResetNum);
705 
706  // We have entries if the storing vector is nonempty.
707  return (!edgeQueueInPtrs_.empty());
708  }
710 
712  // Edge queue info (outgoing edges):
713  void BITstar::Vertex::addOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr& newOutPtr, unsigned int vertexQueueResetNum)
714  {
715  ASSERT_NOT_PRUNED
716 
717  // Conditionally clear any existing lookups
718  this->clearOldLookups(vertexQueueResetNum);
719 
720 #ifdef BITSTAR_DEBUG
721  // Assert that this edge is _from_ this vertex
722  if (newOutPtr->data.second.first->getId() != vId_)
723  {
724  throw ompl::Exception("Attempted to add an outgoing queue edge to the wrong vertex.");
725  }
726  // Assert that this edge is NOT _to_ this vertex
727  if (newOutPtr->data.second.second->getId() == vId_)
728  {
729  throw ompl::Exception("Attempted to add a cyclic outgoing queue edge.");
730  }
731  // Assert that an edge to this target does not already exist
732  for (const auto &elemPtrs : edgeQueueOutPtrs_)
733  {
734  if (newOutPtr->data.second.second->getId() == elemPtrs->data.second.second->getId())
735  {
736  throw ompl::Exception("Attempted to add a second edge to the queue to a single target vertex.");
737  }
738  }
739 #endif // BITSTAR_DEBUG
740 
741  // Push back
742  edgeQueueOutPtrs_.push_back(newOutPtr);
743  }
744 
745  void BITstar::Vertex::rmOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
746  {
747  ASSERT_NOT_PRUNED
748 
749 #ifdef BITSTAR_DEBUG
750  // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
751  // 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.
752  if (vertexQueueResetNum != edgeLookupPass_)
753  {
754  throw ompl::Exception("Attempted to remove an incoming queue edge added under a different expansion id.");
755  }
756 #endif // BITSTAR_DEBUG
757 
758  // Variable
759  // Element found
760  bool found = false;
761 
762  // Iterate through the list and find the address of the element to delete
763  for (auto iterToDelete = edgeQueueOutPtrs_.begin(); iterToDelete != edgeQueueOutPtrs_.end() && !found; ++iterToDelete)
764  {
765  // Is it the element we're looking for? Source id
766  if ((*iterToDelete)->data.second.second->getId() == elemToDelete->data.second.second->getId())
767  {
768  // Remove by iterator
769  this->rmOutgoingHelper(iterToDelete);
770 
771  // Mark as found
772  found = true;
773  }
774  // No else, try the next
775  }
776 
777 #ifdef BITSTAR_DEBUG
778  if (!found)
779  {
780  throw ompl::Exception("Attempted to remove an edge not in the outgoing lookup.");
781  }
782 #endif // BITSTAR_DEBUG
783  }
784 
785  void BITstar::Vertex::rmOutgoingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator& constIterToDelete, unsigned int vertexQueueResetNum)
786  {
787  ASSERT_NOT_PRUNED
788 
789 #ifdef BITSTAR_DEBUG
790  // Assert that the edge queue entries we have are of the same set as the one we're seeking to delete.
791  // 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.
792  if (vertexQueueResetNum != edgeLookupPass_)
793  {
794  throw ompl::Exception("Attempted to remove an outgoing queue edge added under a different expansion id.");
795  }
796 #endif // BITSTAR_DEBUG
797 
798  // Remove a non-const version of the given iterator
799  // (trick from https://stackoverflow.com/a/10669041/1442500)
800  this->rmOutgoingHelper(edgeQueueOutPtrs_.erase(constIterToDelete, constIterToDelete));
801  }
802 
804  {
805  ASSERT_NOT_PRUNED
806 
807  edgeQueueOutPtrs_.clear();
808  }
809 
810  BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::outgoingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
811  {
812  ASSERT_NOT_PRUNED
813 
814  // Conditionally clear any existing lookups
815  this->clearOldLookups(vertexQueueResetNum);
816 
817  return edgeQueueOutPtrs_.cbegin();
818  }
819 
820  BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator BITstar::Vertex::outgoingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
821  {
822  ASSERT_NOT_PRUNED
823 
824  // Conditionally clear any existing lookups
825  this->clearOldLookups(vertexQueueResetNum);
826 
827  return edgeQueueOutPtrs_.cend();
828  }
829 
830  unsigned int BITstar::Vertex::getNumOutgoingEdgeQueuePtrs(unsigned int vertexQueueResetNum)
831  {
832  ASSERT_NOT_PRUNED
833 
834  // Conditionally clear any existing lookups
835  this->clearOldLookups(vertexQueueResetNum);
836 
837  return edgeQueueOutPtrs_.size();
838  }
839 
840  bool BITstar::Vertex::hasOutgoingEdgeQueueEntries(unsigned int vertexQueueResetNum)
841  {
842  ASSERT_NOT_PRUNED
843 
844  // Conditionally clear any existing lookups
845  this->clearOldLookups(vertexQueueResetNum);
846 
847  // We have entries if the storing vector is nonempty.
848  return (!edgeQueueOutPtrs_.empty());
849  }
853 
855  // Protected functions:
856  void BITstar::Vertex::updateCostAndDepth(bool cascadeUpdates /*= true*/)
857  {
858  PRINT_VERTEX_CHANGE
859  ASSERT_NOT_PRUNED
860 
861  if (this->isRoot())
862  {
863  // Am I root? -- I don't really know how this would ever be called, but ok.
864  cost_ = costHelpPtr_->identityCost();
865  depth_ = 0u;
866  }
867  else if (!this->hasParent())
868  {
869  // Am I disconnected?
870  cost_ = costHelpPtr_->infiniteCost();
871 
872  // Set the depth to 0u, getDepth will throw in this condition
873  depth_ = 0u;
874 
875 #ifdef BITSTAR_DEBUG
876  // Assert that I have not been asked to cascade this bad data to my children:
877  if (this->hasChildren() == true && cascadeUpdates == true)
878  {
879  throw ompl::Exception("Attempting to update descendants' costs and depths of a vertex that does "
880  "not have a parent and is not root. This information would therefore be "
881  "gibberish.");
882  }
883 #endif // BITSTAR_DEBUG
884  }
885  else
886  {
887  // I have a parent, so my cost is my parent cost + my edge cost to the parent
888  cost_ = costHelpPtr_->combineCosts(parentSPtr_->getCost(), edgeCost_);
889 
890  // I am one more than my parent's depth:
891  depth_ = (parentSPtr_->getDepth() + 1u);
892  }
893 
894  // Am I updating my children?
895  if (cascadeUpdates)
896  {
897  // Now, iterate over my vector of children and tell each one to update its own damn cost:
898  for (auto &childWPtr : childWPtrs_)
899  {
900 #ifdef BITSTAR_DEBUG
901  // Check that it hasn't expired
902  if (childWPtr.expired() == true)
903  {
904  throw ompl::Exception("A (weak) pointer to a child has was found to have expired while "
905  "updating the costs and depths of descendant vertices.");
906  }
907 #endif // BITSTAR_DEBUG
908 
909  // Get a lock and tell the child to update:
910  childWPtr.lock()->updateCostAndDepth(true);
911  }
912  }
913  // No else, do not update the children. I hope the caller knows what they're doing.
914  }
916 
918  // Private functions:
919 
920  void BITstar::Vertex::rmIncomingHelper(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete)
921  {
922 #ifdef BITSTAR_DEBUG
923  // Store the source id of the edge we're removing
924  VertexId rmSrc = (*iterToDelete)->data.second.first->getId();
925  // Assert that this edge is NOT _from_ this vertex
926  if (rmSrc == vId_)
927  {
928  throw ompl::Exception("Attempted to remove a cyclic incoming queue edge.");
929  }
930  // Assert that this edge is _to_ this vertex
931  if ((*iterToDelete)->data.second.second->getId() != vId_)
932  {
933  throw ompl::Exception("Attempted to remove an incoming queue edge from the wrong vertex.");
934  }
935  // Assert that it could exist
936  if (edgeQueueInPtrs_.empty())
937  {
938  throw ompl::Exception("Attempted to remove an incoming queue edge from a vertex with an empty list.");
939  }
940  // Assert that this edge actually exists
941  bool found = false;
942  for (SearchQueue::EdgeQueueElemPtrVector::iterator ptrIter = edgeQueueInPtrs_.begin(); ptrIter != edgeQueueInPtrs_.end() && !found; ++ptrIter)
943  {
944  found = ((*ptrIter)->data.second.first->getId() == rmSrc);
945  }
946  if (!found)
947  {
948  throw ompl::Exception("Attempted to remove an edge not in the incoming lookup.");
949  }
950 #endif // BITSTAR_DEBUG
951 
952  // Clear our entry in the list
953  *iterToDelete = nullptr;
954 
955  // Remove it efficiently
956  swapPopBack(iterToDelete, &edgeQueueInPtrs_);
957 
958 #ifdef BITSTAR_DEBUG
959  // Assert that it's now gone.
960  for (const auto &edgePtr : edgeQueueInPtrs_)
961  {
962  if (edgePtr->data.second.first->getId() == rmSrc)
963  {
964  throw ompl::Exception("Failed to remove the designated edge in the incoming lookup.");
965  }
966  }
967 #endif // BITSTAR_DEBUG
968  }
969 
970  void BITstar::Vertex::rmOutgoingHelper(const SearchQueue::EdgeQueueElemPtrVector::iterator &iterToDelete)
971  {
972 #ifdef BITSTAR_DEBUG
973  // Store the target id of the edge we're removing
974  VertexId rmTrgt = (*iterToDelete)->data.second.second->getId();
975  // Assert that this edge is _from_ this vertex
976  if ((*iterToDelete)->data.second.first->getId() != vId_)
977  {
978  throw ompl::Exception("Attempted to remove an outgoing queue edge from the wrong vertex.");
979  }
980  // Assert that this edge is NOT _to_ this vertex
981  if (rmTrgt == vId_)
982  {
983  throw ompl::Exception("Attempted to remove a cyclic outgoing queue edge.");
984  }
985  // Assert that it could exist
986  if (edgeQueueOutPtrs_.empty())
987  {
988  throw ompl::Exception("Attempted to remove an outgoing queue edge from a vertex with an empty list.");
989  }
990  // Assert that this edge actually exists
991  bool found = false;
992  for (SearchQueue::EdgeQueueElemPtrVector::iterator ptrIter = edgeQueueOutPtrs_.begin(); ptrIter != edgeQueueOutPtrs_.end() && !found; ++ptrIter)
993  {
994  found = ((*ptrIter)->data.second.second->getId() == rmTrgt);
995  }
996  if (!found)
997  {
998  throw ompl::Exception("Attempted to remove an edge not in the outgoing lookup.");
999  }
1000 #endif // BITSTAR_DEBUG
1001 
1002  // Clear our entry in the list
1003  *iterToDelete = nullptr;
1004 
1005  // Remove it efficiently
1006  swapPopBack(iterToDelete, &edgeQueueOutPtrs_);
1007 
1008 #ifdef BITSTAR_DEBUG
1009  // Assert that it's now gone.
1010  for (const auto &edgePtr : edgeQueueOutPtrs_)
1011  {
1012  if (edgePtr->data.second.second->getId() == rmTrgt)
1013  {
1014  throw ompl::Exception("Failed to remove the designated edge in the outgoing lookup.");
1015  }
1016  }
1017 #endif // BITSTAR_DEBUG
1018  }
1019 
1020  void BITstar::Vertex::clearOldLookups(unsigned int vertexQueueResetNum)
1021  {
1022  // Clean up any old lookups
1023  if (vertexQueueResetNum != edgeLookupPass_)
1024  {
1025  // Clear the existing entries
1026  this->clearIncomingEdgeQueuePtrs();
1027  this->clearOutgoingEdgeQueuePtrs();
1028 
1029  // Update the counter
1030  edgeLookupPass_ = vertexQueueResetNum;
1031  }
1032  // No else, this is the same pass through the vertex queue
1033  }
1034 
1035  void BITstar::Vertex::assertNotPruned() const
1036  {
1037  if (isPruned_ == true)
1038  {
1039  std::cout << std::endl << "vId: " << vId_ << std::endl;
1040  throw ompl::Exception("Attempting to access a pruned vertex.");
1041  }
1042  }
1044  } // geometric
1045 } // ompl
bool hasBeenExpandedToVertices() const
Returns true if the vertex has been expanded towards vertices.
Definition: Vertex.cpp:472
bool hasBeenExpandedToSamples() const
Returns true if the vertex has been expanded towards samples.
Definition: Vertex.cpp:449
bool hasIncomingEdgeQueueEntries(unsigned int vertexQueueResetNum)
Return true if the vertex has links to incoming entries in the Edge Queue. Will clear existing in/out...
Definition: Vertex.cpp:699
void clearVertexQueueIter()
Clear the iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:554
unsigned int getDepth() const
Get the "depth" of the vertex from the root. A root vertex is at depth 0, a direct descendent of the ...
Definition: Vertex.cpp:151
void removeChild(const VertexPtr &oldChild)
Remove a child from this vertex. Does not change this vertex&#39;s cost or those of its descendants...
Definition: Vertex.cpp:339
virtual ~Vertex()
Destructor.
Definition: Vertex.cpp:98
void markNew()
Mark the vertex as new.
Definition: Vertex.cpp:433
bool hasParent() const
Get whether this vertex has a parent.
Definition: Vertex.cpp:137
void removeParent(bool updateChildCosts)
Remove the parent of this vertex. Will always update this vertex&#39;s cost, and can update the descenden...
Definition: Vertex.cpp:236
void markExpandedToSamples()
Mark the vertex as expanded towards samples.
Definition: Vertex.cpp:456
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator outgoingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
Get an iterator to the front of the outgoing edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:810
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:69
STL namespace.
void rmOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
Remove an outgoing edge queue entry by value.
Definition: Vertex.cpp:745
bool isPruned() const
Whether the vertex has been pruned.
Definition: Vertex.cpp:495
VertexPtr getParent()
Get the parent of a vertex as a mutable pointer.
Definition: Vertex.cpp:187
void updateCostAndDepth(bool cascadeUpdates=true)
Calculates the updated cost and depth of the current state, as well as calling all children&#39;s updateC...
Definition: Vertex.cpp:856
ompl::base::Cost getEdgeInCost() const
Get the incremental cost-to-come of a vertex.
Definition: Vertex.cpp:412
void addIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &newInPtr, unsigned int vertexQueueResetNum)
Add to the list of the edge queue entries that point in to this vertex. Will clear existing in/out lo...
Definition: Vertex.cpp:572
bool isNew() const
Returns true if the vertex is marked as new. Vertices are new until marked old.
Definition: Vertex.cpp:426
void addParent(const VertexPtr &newParent, const ompl::base::Cost &edgeInCost, bool updateChildCosts)
Set the parent of this vertex, cannot be used to replace a previous parent. Will always update this v...
Definition: Vertex.cpp:208
void setVertexQueueIter(const SearchQueue::VertexQueueIter &newPtr)
Set the iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:535
void addChild(const VertexPtr &newChild)
Add a child to this vertex. Does not change this vertex&#39;s cost or those of its descendants. Child must already have this vertex listed as it&#39;s parent.
Definition: Vertex.cpp:312
Vertex(ompl::base::SpaceInformationPtr si, const CostHelper *const costHelpPtr, bool root=false)
Constructor.
Definition: Vertex.cpp:79
unsigned int getNumIncomingEdgeQueuePtrs(unsigned int vertexQueueResetNum)
Get the number of edge queue entries incoming to this vertex. Will clear existing in/out lookups if t...
Definition: Vertex.cpp:689
ompl::base::State * state()
The state of a vertex as a mutable pointer.
Definition: Vertex.cpp:120
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
void rmOutgoingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &constIterToDelete, unsigned int vertexQueueResetNum)
Remove an outgoing edge queue entry by iterator to the member vector.
Definition: Vertex.cpp:785
bool hasVertexQueueEntry() const
Return true if the vertex has a link to it&#39;s entry in the Vertex Queue.
Definition: Vertex.cpp:562
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
SearchQueue::VertexQueueIter getVertexQueueIter() const
Get an iterator to this vertex in the vertex queue.
Definition: Vertex.cpp:520
void addOutgoingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &newOutPtr, unsigned int vertexQueueResetNum)
Add to the list of the edge queue entries that point out of this vertex. Will clear existing in/out l...
Definition: Vertex.cpp:713
ompl::base::State const * stateConst() const
The state of a vertex as a constant pointer.
Definition: Vertex.cpp:113
void rmIncomingEdgeQueuePtr(const SearchQueue::EdgeQueueElemPtr &elemToDelete, unsigned int vertexQueueResetNum)
Remove an incoming edge queue entry by value to the member vector.
Definition: Vertex.cpp:604
void clearIncomingEdgeQueuePtrs()
Clear the pointers to all of the incoming edge queue entries.
Definition: Vertex.cpp:662
void markUnexpandedToVertices()
Mark the vertex as not expanded towards vertices.
Definition: Vertex.cpp:487
A shared pointer wrapper for ompl::base::SpaceInformation.
bool isRoot() const
Whether the vertex is root.
Definition: Vertex.cpp:130
ompl::base::Cost getCost() const
Get the cost-to-come of a vertex. Return infinity if the edge is disconnected.
Definition: Vertex.cpp:405
void markOld()
Mark the vertex as old.
Definition: Vertex.cpp:441
Definition of an abstract state.
Definition: State.h:49
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator incomingEdgeQueuePtrsBeginConst(unsigned int vertexQueueResetNum)
Get an iterator to the front of the incoming edge queue entry vector. Will clear existing in/out look...
Definition: Vertex.cpp:669
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition: Vertex.cpp:106
EdgeQueueAsPairBinHeap::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:120
void getChildrenConst(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition: Vertex.cpp:268
The exception type for ompl.
Definition: Exception.h:46
void getChildren(VertexPtrVector *children)
Get the children of a vertex as mutable pointers.
Definition: Vertex.cpp:290
void markUnexpandedToSamples()
Mark the vertex as not expanded towards samples.
Definition: Vertex.cpp:464
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers.
Definition: BITstar.h:130
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:132
void rmIncomingEdgeQueuePtrByIter(const SearchQueue::EdgeQueueElemPtrVector::const_iterator &constIterToDelete, unsigned int vertexQueueResetNum)
Remove an incoming edge queue entry by iterator to the member vector.
Definition: Vertex.cpp:644
bool hasChildren() const
Get whether this vertex has any children.
Definition: Vertex.cpp:261
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
unsigned int getNumOutgoingEdgeQueuePtrs(unsigned int vertexQueueResetNum)
Get the number of edge queue entries outgoing from this vertex. Will clear existing in/out lookups if...
Definition: Vertex.cpp:830
void markUnpruned()
Mark the vertex as unpruned.
Definition: Vertex.cpp:508
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator outgoingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
Get an iterator to the end of the outgoing edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:820
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator incomingEdgeQueuePtrsEndConst(unsigned int vertexQueueResetNum)
Get an iterator to the end of the incoming edge queue entry vector. Will clear existing in/out lookup...
Definition: Vertex.cpp:679
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
bool hasOutgoingEdgeQueueEntries(unsigned int vertexQueueResetNum)
Return true if the vertex has links to outgoing entries in the Edge Queue. Will clear existing in/out...
Definition: Vertex.cpp:840
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void markExpandedToVertices()
Mark the vertex as expanded towards vertices.
Definition: Vertex.cpp:479
VertexConstPtr getParentConst() const
Get the parent of a vertex as a constant pointer.
Definition: Vertex.cpp:166
void markPruned()
Mark the vertex as pruned.
Definition: Vertex.cpp:500
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:134
bool isInTree() const
Get whether a vertex is "in the graph" or not. This returns true if the vertex is the graph root or i...
Definition: Vertex.cpp:144
VertexQueueAsMMap::iterator VertexQueueIter
An iterator into the vertex queue multimap.
Definition: SearchQueue.h:109
void clearOutgoingEdgeQueuePtrs()
Clear the pointers to all of the outgoing edge queue entries.
Definition: Vertex.cpp:803