SearchQueue.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/SearchQueue.h"
39 
40 // For std::move
41 #include <utility>
42 // For std::lexicographical_compare and the std::*_heap functions.
43 #include <algorithm>
44 
45 // OMPL:
46 // For OMPL_INFORM et al.
47 #include "ompl/util/Console.h"
48 // For exceptions:
49 #include "ompl/util/Exception.h"
50 
51 // BIT*:
52 // A collection of common helper functions
53 #include "ompl/geometric/planners/bitstar/datastructures/HelperFunctions.h"
54 // The vertex class:
55 #include "ompl/geometric/planners/bitstar/datastructures/Vertex.h"
56 // The cost-helper class:
57 #include "ompl/geometric/planners/bitstar/datastructures/CostHelper.h"
58 // The implicit graph:
59 #include "ompl/geometric/planners/bitstar/datastructures/ImplicitGraph.h"
60 
61 // Debug macros
62 #ifdef BITSTAR_DEBUG
63 
64  #define ASSERT_SETUP this->assertSetup();
65 #else
66  #define ASSERT_SETUP
67 #endif // BITSTAR_DEBUG
68 
69 namespace ompl
70 {
71  namespace geometric
72  {
74  // Public functions:
76  : nameFunc_(std::move(nameFunc))
77  , vertexQueue_([this](const CostDouble &lhs, const CostDouble &rhs)
78  {
79  return queueComparison(lhs, rhs);
80  }) // This tells the vertexQueue_ to use the queueComparison for sorting
81  , vertexToExpand_(vertexQueue_.begin())
82  , edgeQueue_([this](const CostTripleAndVertexPtrPair &lhs, const CostTripleAndVertexPtrPair &rhs)
83  {
84  return queueComparison(lhs.first, rhs.first);
85  }) // This tells the edgeQueue_ to use the queueComparison for sorting
86  {
87  }
88 
90  {
91  // Store that I am setup
92  isSetup_ = true;
93 
94  // Get my copies
95  costHelpPtr_ = costHelpPtr;
96  graphPtr_ = graphPtr;
97 
98  // Set the the cost threshold to infinity to start:
99  solnCost_ = costHelpPtr_->infiniteCost();
100  }
101 
103  {
104  // Reset everything to the state of construction OTHER than planner name and settings/parameters
105  // Keep this in the order of the constructors for easy verification:
106 
107  // Mark as cleared
108  isSetup_ = false;
109 
110  // The pointers
111  costHelpPtr_ = nullptr;
112  graphPtr_ = nullptr;
113 
114  // The vertex queue:
115  vertexQueue_.clear();
116  vertexToExpand_ = vertexQueue_.begin();
117 
118  // The edge queue:
119  edgeQueue_.clear();
120 
121  // The number of times we're gone through the vertex queue:
122  numQueueResets_ = 0u;
123 
124  // The resort vector:
125  resortVertices_.clear();
126 
127  // The cost threshold:
128  solnCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
129 
130  // The existence of a solution:
131  hasExactSolution_ = false;
132 
133  // Progress properties
134  numEdgesPopped_ = 0u;
135 
136  // DO NOT reset the settings:
137  // useStrictQueueOrdering_
138  // delayRewiring_
139  // pruneDuringResort_
140  }
141 
142  void BITstar::SearchQueue::enqueueVertex(const VertexPtr &newVertex, bool removeFromFree)
143  {
144  ASSERT_SETUP
145 
146  // Insert the vertex:
147  this->vertexInsertHelper(newVertex, true, removeFromFree, true);
148  }
149 
151  {
152  ASSERT_SETUP
153 
154 #ifdef BITSTAR_DEBUG
155  // Assert that the parent vertex is in the vertex queue
156  if (!newEdge.first->hasVertexQueueEntry())
157  {
158  throw ompl::Exception("Attempted to enqueue an edge from a vertex not in the vertex queue.");
159  }
160 #endif // BITSTAR_DEBUG
161 
162  // Variable:
163  // The iterator to the new edge in the queue:
164  EdgeQueueElemPtr edgeElemPtr;
165 
166  // Insert into the edge queue, getting the element pointer
167  edgeElemPtr = edgeQueue_.insert(std::make_pair(this->edgeQueueValue(newEdge), newEdge));
168 
169  // Push the newly created edge back on the vector of edges from the parent.
170  newEdge.first->addOutgoingEdgeQueuePtr(edgeElemPtr, numQueueResets_);
171 
172  // Push the newly created edge back on the vector of edges to the child.
173  newEdge.second->addIncomingEdgeQueuePtr(edgeElemPtr, numQueueResets_);
174  }
175 
176  void BITstar::SearchQueue::enqueueEdge(const VertexPtr &sourceVertex, const VertexPtr &targetVertex)
177  {
178  ASSERT_SETUP
179 
180  // Call my helper function:
181  this->enqueueEdge(std::make_pair(sourceVertex, targetVertex));
182  }
183 
185  {
186  ASSERT_SETUP
187 
188  // Disconnect from parent if necessary, cascading cost updates:
189  if (oldVertex->hasParent())
190  {
191  this->disconnectParent(oldVertex, true);
192  }
193 
194  // Remove it from vertex queue and lookup, and edge queues:
195  this->vertexRemoveHelper(oldVertex, true);
196  }
197 
199  {
200  ASSERT_SETUP
201 
202 #ifdef BITSTAR_DEBUG
203  if (this->isEmpty() == true)
204  {
205  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
206  }
207 #endif // BITSTAR_DEBUG
208 
209  // Update the queue:
210  this->updateQueue();
211 
212  // Return the front edge
213  return vertexQueue_.begin()->second;
214  }
215 
217  {
218  ASSERT_SETUP
219 
220 #ifdef BITSTAR_DEBUG
221  if (this->isEmpty() == true)
222  {
223  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
224  }
225 #endif // BITSTAR_DEBUG
226 
227  // Update the queue:
228  this->updateQueue();
229 
230  // Return the front edge
231  return edgeQueue_.top()->data.second;
232  }
233 
235  {
236  ASSERT_SETUP
237 
238 #ifdef BITSTAR_DEBUG
239  if (this->isEmpty() == true)
240  {
241  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
242  }
243 #endif // BITSTAR_DEBUG
244 
245  // Update the queue:
246  this->updateQueue();
247 
248  // Return the front value
249  return vertexQueue_.begin()->first;
250  }
251 
253  {
254  ASSERT_SETUP
255 
256 #ifdef BITSTAR_DEBUG
257  if (this->isEmpty() == true)
258  {
259  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
260  }
261 #endif // BITSTAR_DEBUG
262 
263  // Update the queue:
264  this->updateQueue();
265 
266  // Return the front value
267  return edgeQueue_.top()->data.first;
268  }
269 
271  {
272  ASSERT_SETUP
273 
274  // Variable
275  // The top of the binary heap
276  EdgeQueueElemPtr bestElemPtr;
277 
278 #ifdef BITSTAR_DEBUG
279  if (this->isEmpty() == true)
280  {
281  throw ompl::Exception("Attempted to pop an empty SearchQueue.");
282  }
283 #endif // BITSTAR_DEBUG
284 
285  // Update the queue:
286  this->updateQueue();
287 
288 #ifdef BITSTAR_DEBUG
289  if (edgeQueue_.empty())
290  {
291  throw ompl::Exception("Edge queue is still empty after an update.");
292  }
293 #endif // BITSTAR_DEBUG
294 
295  // Get the front:
296  bestElemPtr = edgeQueue_.top();
297 
298  // Store it in the return value;
299  *bestEdge = bestElemPtr->data.second;
300 
301  // Remove the lookups to the element
302  bestElemPtr->data.second.first->rmOutgoingEdgeQueuePtr(bestElemPtr, numQueueResets_);
303  bestElemPtr->data.second.second->rmIncomingEdgeQueuePtr(bestElemPtr, numQueueResets_);
304 
305  // Finally, remove from the queue itself
306  edgeQueue_.remove(bestElemPtr);
307 
308  // Increment my counter
309  ++numEdgesPopped_;
310  }
311 
313  {
314  ASSERT_SETUP
315 
316  VertexPtrPair rval;
317 
318  this->popFrontEdge(&rval);
319 
320  return rval;
321  }
322 
324  {
325  ASSERT_SETUP
326 
327  // Flag
328  hasExactSolution_ = true;
329 
330  // Store
331  solnCost_ = solnCost;
332  }
333 
335  {
336  ASSERT_SETUP
337 
338  if (!edgeQueue_.empty())
339  {
340  // Iterate over the vector of incoming edges to this vertex and remove them from the queue (and clean up their other lookup)
341  for (auto inQueueElemIter = cVertex->incomingEdgeQueuePtrsBeginConst(numQueueResets_);
342  inQueueElemIter != cVertex->incomingEdgeQueuePtrsEndConst(numQueueResets_);
343  ++inQueueElemIter)
344  {
345  // Remove the edge from the *other* lookup (by value since this is NOT an iter to THAT container).
346  // No need to remove from this lookup, as that's being cleared:
347  (*inQueueElemIter)->data.second.first->rmOutgoingEdgeQueuePtr(*inQueueElemIter, numQueueResets_);
348 
349  // Finally remove it from the queue
350  edgeQueue_.remove(*inQueueElemIter);
351  }
352 
353  // Clear the list:
354  cVertex->clearIncomingEdgeQueuePtrs();
355  }
356  // No else, nothing to remove_to
357  }
358 
360  {
361  ASSERT_SETUP
362 
363  if (!edgeQueue_.empty())
364  {
365  // Iterate over the vector of outgoing edges to this vertex and remove them from the queue (and clean up their other lookup)
366  for (auto outQueueElemIter = pVertex->outgoingEdgeQueuePtrsBeginConst(numQueueResets_);
367  outQueueElemIter != pVertex->outgoingEdgeQueuePtrsEndConst(numQueueResets_);
368  ++outQueueElemIter)
369  {
370  // Remove the edge from the *other* lookup (by value since this is NOT an iter to THAT container).
371  // No need to remove from this lookup, as that's being cleared:
372  (*outQueueElemIter)->data.second.second->rmIncomingEdgeQueuePtr(*outQueueElemIter, numQueueResets_);
373 
374  // Finally, remove it from the queue
375  edgeQueue_.remove(*outQueueElemIter);
376  }
377 
378  // Clear the list:
379  pVertex->clearOutgoingEdgeQueuePtrs();
380  }
381  // No else, nothing to remove_from
382  }
383 
385  {
386  ASSERT_SETUP
387 
388  if (!edgeQueue_.empty())
389  {
390  // Variable
391  // The vector of edges to delete in the vector:
392  std::vector<EdgeQueueElemPtrVector::const_iterator> inItersToDelete;
393 
394  // Iterate over the incoming edges and record those that are to be deleted
395  for (auto inQueueElemIter = cVertex->incomingEdgeQueuePtrsBeginConst(numQueueResets_);
396  inQueueElemIter != cVertex->incomingEdgeQueuePtrsEndConst(numQueueResets_);
397  ++inQueueElemIter)
398  {
399  // Check if it would have been inserted
400  if (!this->edgeInsertCondition((*inQueueElemIter)->data.second))
401  {
402  // It would not, delete
403  inItersToDelete.push_back(inQueueElemIter);
404  }
405  // No else, we're not deleting this iterator
406  }
407 
408  // Now, iterate over the vector of iterators to delete and remove them from the queue and their lookups
409  for (const auto &inVectorIter : inItersToDelete)
410  {
411  // Remove the entry in the other lookup (by value since this is NOT an iter to THAT container)
412  (*inVectorIter)->data.second.first->rmOutgoingEdgeQueuePtr(*inVectorIter, numQueueResets_);
413 
414  // Remove from the queue itself
415  edgeQueue_.remove(*inVectorIter);
416 
417  // And finally erase the lookup iterator from my lookup. If this was done first, the
418  // iterator would have been invalidated for the above.
419  cVertex->rmIncomingEdgeQueuePtrByIter(inVectorIter, numQueueResets_);
420  }
421  }
422  // No else, nothing to prune_to
423  }
424 
426  {
427  ASSERT_SETUP
428 
429  if (!edgeQueue_.empty())
430  {
431  // Variable
432  // The vector of edges to delete in the vector:
433  std::vector<EdgeQueueElemPtrVector::const_iterator> outItersToDelete;
434 
435  // Iterate over the incoming edges and record those that are to be deleted
436  for (auto outQueueElemIter = pVertex->outgoingEdgeQueuePtrsBeginConst(numQueueResets_);
437  outQueueElemIter != pVertex->outgoingEdgeQueuePtrsEndConst(numQueueResets_);
438  ++outQueueElemIter)
439  {
440  // Check if it would have been inserted
441  if (!this->edgeInsertCondition((*outQueueElemIter)->data.second))
442  {
443  // It would not, delete
444  outItersToDelete.push_back(outQueueElemIter);
445  }
446  // No else, we're not deleting this iterator
447  }
448 
449  // Now, iterate over the vector of iterators to delete and remove them from the queue and their lookups
450  for (const auto &outVectorIter : outItersToDelete)
451  {
452  // Remove the entry in the other lookup (by value since this is NOT an iter to THAT container)
453  (*outVectorIter)->data.second.second->rmIncomingEdgeQueuePtr(*outVectorIter, numQueueResets_);
454 
455  // Remove from the queue itself
456  edgeQueue_.remove(*outVectorIter);
457 
458  // And finally erase the lookup iterator from my lookup. If this was done first, the
459  // iterator would have been invalidated for the above.
460  pVertex->rmOutgoingEdgeQueuePtrByIter(outVectorIter, numQueueResets_);
461  }
462  }
463  // No else, nothing to prune_from
464  }
465 
467  {
468  ASSERT_SETUP
469 
470  resortVertices_.push_back(vertex);
471  }
472 
473  std::pair<unsigned int, unsigned int> BITstar::SearchQueue::prune(const VertexConstPtr &vertex)
474  {
475  ASSERT_SETUP
476 
477 #ifdef BITSTAR_DEBUG
478  if (hasExactSolution_ == false)
479  {
480  throw ompl::Exception("Prune cannot be called until a solution is found");
481  }
482  if (this->isSorted() == false)
483  {
484  throw ompl::Exception("Prune cannot be called on an unsorted queue.");
485  }
486 #endif // BITSTAR_DEBUG
487 
488  // The vertex expansion queue is sorted on an estimated solution cost considering the *current* cost-to-come
489  // of the vertices, while we prune by considering the best-case cost-to-come.
490  // This means that the value of the vertices in the queue are an upper-bounding estimate of the value we
491  // will use to prune them.
492  // Therefore, we can start our pruning at the goal vertex and iterate forward through the queue from there.
493 
494  // Variables:
495  // The number of vertices and samples pruned:
496  std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
497  // The iterator into the queue:
498  VertexQueueIter queueIter;
499 
500  // Get the vertex queue iterator of the given starting point.:
501  queueIter = vertex->getVertexQueueIter();
502 
503  // Iterate through to the end of the queue
504  while (queueIter != vertexQueue_.end())
505  {
506  // Check if it should be pruned (value) or has lost its parent.
507  if (this->vertexPruneCondition(queueIter->second))
508  {
509  // The vertex should be pruned.
510  // Variables
511  // An iter to the vertex to prune:
512  VertexQueueIter pruneIter;
513 
514  // Copy the iterator to prune:
515  pruneIter = queueIter;
516 
517  // Move the queue iterator back one so we can step to the next *valid* vertex after pruning:
518  --queueIter;
519 
520  // Prune the branch:
521  numPruned = numPruned + this->pruneBranch(pruneIter->second);
522  }
523  // No else, skip this vertex.
524 
525  // Iterate forward to the next value in the queue
526  ++queueIter;
527  }
528 
529  // Return the number of vertices and samples pruned.
530  return numPruned;
531  }
532 
534  {
535  ASSERT_SETUP
536 
537  // Iterate through every vertex marked for resorting:
538  if (!resortVertices_.empty())
539  {
540  // Variable:
541  // The container ordered on vertex depth:
542  typedef std::unordered_map<BITstar::VertexId, VertexPtr> VertexIdToVertexPtrUMap;
543  typedef std::map<unsigned int, VertexIdToVertexPtrUMap> DepthToUMapMap;
544  DepthToUMapMap uniqueResorts;
545  // The number of vertices and samples pruned, respectively:
546  std::pair<unsigned int, unsigned int> numPruned(0u, 0u);
547 
548  OMPL_INFORM("%s: Resorting %d vertices in the queue.", nameFunc_().c_str(), resortVertices_.size());
549 
550  // Variable:
551 
552  // Iterate over the vector and place into the unique queue indexed on *depth*. This guarantees that we
553  // won't process a branch multiple times by being given different vertices down its chain
554  for (auto &vertex : resortVertices_)
555  {
556  // Add the vertex to the unordered map stored at the given depth.
557  // The [] return a reference to the existing entry, or creates a new entry:
558  uniqueResorts[vertex->getDepth()].emplace(vertex->getId(), vertex);
559  }
560 
561  // Clear the vector of vertices to resort from:
562  resortVertices_.clear();
563 
564  // Now process the vertices in order of depth.
565  for (auto &depthAndVertexMapPair : uniqueResorts)
566  {
567  for (auto &vIdAndPtrPair : depthAndVertexMapPair.second)
568  {
569  // Make sure it has not already been pruned:
570  if (!vIdAndPtrPair.second->isPruned())
571  {
572  // Make sure it has not already been returned to the set of samples:
573  if (vIdAndPtrPair.second->isInTree())
574  {
575  // If this was a new vertex, would we *not* insert it in the queue (and do we have
576  // "permission" not to do so)?
577  if (pruneDuringResort_ &&
578  !this->vertexInsertCondition(vIdAndPtrPair.second))
579  {
580  // The vertex should just be pruned and forgotten about.
581  // Prune the branch:
582  numPruned = numPruned + this->pruneBranch(vIdAndPtrPair.second);
583  }
584  else
585  {
586  // The vertex is going to be kept.
587  // Does it have any children?
588  if (vIdAndPtrPair.second->hasChildren())
589  {
590  // Variables:
591  // The vector of children:
592  VertexPtrVector resortChildren;
593 
594  // Put its children in the vector to be resorted:
595  // Get the vector of children:
596  vIdAndPtrPair.second->getChildren(&resortChildren);
597 
598  // Get a reference to the container for the children, all children are 1 level
599  // deeper than their parent.:
600  // The [] return a reference to the existing entry, or creates a new entry:
601  VertexIdToVertexPtrUMap &depthContainer =
602  uniqueResorts[vIdAndPtrPair.second->getDepth() + 1u];
603 
604  // Place the children into the container, as the container is a map, it will not
605  // allow the children to be entered twice.
606  for (auto &childPtr : resortChildren)
607  {
608  depthContainer.emplace(childPtr->getId(), childPtr);
609  }
610  }
611 
612  // Resort the vertex:
613  this->resortVertex(vIdAndPtrPair.second);
614  }
615  }
616  // No else, this vertex was a child of a vertex pruned during the resort. It has already
617  // been returned to the set of free samples by the call to pruneBranch.
618  }
619  // No else, this vertex was a child of a vertex pruned during the resort. It has already been
620  // deleted by the call to pruneBranch.
621  }
622  }
623 
624  if (numPruned.first > 0u || numPruned.second > 0u)
625  {
626  OMPL_INFORM("%s: Resorting the queue disconnected %d vertices from the tree and completely removed "
627  "%d samples.",
628  nameFunc_().c_str(), numPruned.first, numPruned.second);
629  }
630  // No else, sshhh
631  }
632  // No else, nothing to resort
633  }
634 
636  {
637  ASSERT_SETUP
638 
639  // Is there anything to resort before we're marked as finished?
640  if (!resortVertices_.empty())
641  {
642  // There are unsorted vertices, sort them:
643  OMPL_DEBUG("%s (SearchQueue): Resorting an unsorted queue instead of marking it as finished.",
644  nameFunc_().c_str());
645  this->resort();
646  }
647  else
648  {
649  // We have exhausted this queue.
650  // Clear the edge container:
651  edgeQueue_.clear();
652 
653  // Increment the queue processing number
654  ++numQueueResets_;
655 
656  // Move the token to the end:
657  vertexToExpand_ = vertexQueue_.end();
658  }
659  }
660 
662  {
663  ASSERT_SETUP
664 
665 #ifdef BITSTAR_DEBUG
666  if (resortVertices_.empty() == false)
667  {
668  throw ompl::Exception("SearchQueue::reset() called with an unsorted queue.");
669  }
670 #endif // BITSTAR_DEBUG
671 
672  // Clear the edge container:
673  edgeQueue_.clear();
674 
675  // Increment the queue processing number
676  ++numQueueResets_;
677 
678  // The resort vector:
679  resortVertices_.clear();
680 
681  // Restart the expansion queue:
682  vertexToExpand_ = vertexQueue_.begin();
683  }
684 
686  {
687  ASSERT_SETUP
688 
689  // Threshold should always be g_t(x_g)
690 
691  // Can it ever be a better solution?
692  // Just in case we're using a vertex that is exactly optimally connected
693  // g^(v) + h^(v) <= g_t(x_g)?
694  return costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
695  solnCost_);
696  }
697 
699  {
700  ASSERT_SETUP
701 
702  bool rval;
703 
704  // Can it ever be a better solution? Less-than-equal to just in case we're using an edge that is exactly
705  // optimally connected
706  // g^(v) + c^(v,x) + h^(x) <= g_t(x_g)?
707  rval = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicEdge(edge),
708  solnCost_);
709 
710  // If the child is connected already, we need to check if we could do better than it's current connection.
711  // But only if we're inserting the edge
712  if (edge.second->hasParent() && rval)
713  {
714  // Can it ever be a better path to the vertex? Less-than-equal to just in case we're using an edge that
715  // is exactly optimally connected
716  // g^(v) + c^(v,x) <= g_t(x)?
717  rval = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicToTarget(edge),
718  edge.second->getCost()); // Ever rewire?
719  }
720 
721  return rval;
722  }
723 
725  {
726  ASSERT_SETUP
727 
728  // Threshold should always be g_t(x_g)
729 
730  // Prune the vertex if it could cannot part of a better solution in the current graph. Greater-than just in
731  // case we're using an edge that is exactly optimally connected.
732  // g_t(v) + h^(v) > g_t(x_g)?
733  return costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicVertex(state), solnCost_);
734  }
735 
737  {
738  ASSERT_SETUP
739 
740  // Threshold should always be g_t(x_g)
741  // Prune the unconnected sample if it could never be better of a better solution.
742  // g^(v) + h^(v) >= g_t(x_g)?
743  return costHelpPtr_->isCostWorseThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
744  solnCost_);
745  }
746 
748  {
749  ASSERT_SETUP
750 
751  bool rval;
752  // Threshold should always be g_t(x_g)
753 
754  // Prune the edge if it could cannot part of a better solution in the current graph. Greater-than just in
755  // case we're using an edge that is exactly optimally connected.
756  // g_t(v) + c^(v,x) + h^(x) > g_t(x_g)?
757  rval = costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicEdge(edge), solnCost_);
758 
759  // If the child is connected already, we need to check if we could do better than it's current connection.
760  // But only if we're not pruning based on the first check
761  if (edge.second->hasParent() && !rval)
762  {
763  // Can it ever be a better path to the vertex in the current graph? Greater-than to just in case we're
764  // using an edge that is exactly optimally connected
765  // g_t(v) + c^(v,x) > g_t(x)?
766  rval = costHelpPtr_->isCostWorseThan(costHelpPtr_->currentHeuristicToTarget(edge),
767  edge.second->getCost()); // Currently rewire?
768  }
769 
770  return rval;
771  }
772 
774  {
775  ASSERT_SETUP
776 
777  // Update the queue:
778  this->updateQueue();
779 
780  return edgeQueue_.size();
781  }
782 
784  {
785  ASSERT_SETUP
786 
787  // Update the queue:
788  this->updateQueue();
789 
790  // Variables:
791  // The number of vertices left to expand:
792  unsigned int numToExpand;
793 
794  // Start at 0:
795  numToExpand = 0u;
796 
797  // Iterate until the end:
798  for (VertexQueueAsMMap::const_iterator vIter = vertexToExpand_; vIter != vertexQueue_.end(); ++vIter)
799  {
800  // Increment counter:
801  ++numToExpand;
802  }
803 
804  // Return
805  return numToExpand;
806  }
807 
808  unsigned int BITstar::SearchQueue::numEdgesTo(const VertexPtr &cVertex)
809  {
810  ASSERT_SETUP
811 
812  // Update the queue:
813  this->updateQueue();
814 
815  // Return:
816  return cVertex->getNumIncomingEdgeQueuePtrs(numQueueResets_);
817  }
818 
819  unsigned int BITstar::SearchQueue::numEdgesFrom(const VertexPtr &pVertex)
820  {
821  ASSERT_SETUP
822 
823  // Update the queue:
824  this->updateQueue();
825 
826  // Return:
827  return pVertex->getNumOutgoingEdgeQueuePtrs(numQueueResets_);
828  }
829 
831  {
832  ASSERT_SETUP
833 
834  return resortVertices_.size();
835  }
836 
838  {
839  ASSERT_SETUP
840 
841  return resortVertices_.empty();
842  }
843 
845  {
846  ASSERT_SETUP
847 
848  return (vertexToExpand_ == vertexQueue_.begin() && edgeQueue_.empty());
849  }
850 
852  {
853  ASSERT_SETUP
854 
855  // Update the queue:
856  this->updateQueue();
857 
858  // Expand if the edge queue is empty but the vertex queue is not:
859  while (edgeQueue_.empty() && vertexToExpand_ != vertexQueue_.end())
860  {
861  // Expand the next vertex, this pushes the token:
862  this->expandNextVertex();
863  }
864 
865  // If the edge queue is actually empty, than use this opportunity to resort any unsorted vertices
866  if (edgeQueue_.empty())
867  {
868  this->resort();
869  }
870  // No else
871 
872  // Return whether the edge queue is empty:
873  return edgeQueue_.empty();
874  }
875 
877  {
878  ASSERT_SETUP
879 
880  // Compare the value used to currently sort the vertex in the queue to the value of the token.
881  if (vertexToExpand_ == vertexQueue_.end())
882  {
883  // If the token is at the end of the queue, obviously the vertex is expanded:
884  return true;
885  }
886  // else:
887 
888  // By virtue of the vertex expansion rules, the token will always sit at the front of a group of
889  // equivalent cost vertices (that is to say, all vertices with the same cost get expanded at the same
890  // time). Therefore, the vertex is expanded if it's cost is strictly better than the token.
891  return this->queueComparison(vertex->getVertexQueueIter()->first, vertexToExpand_->first);
892  }
893 
895  {
896  ASSERT_SETUP
897 
898  // Update the queue:
899  this->updateQueue();
900 
901  // Clear the given vector:
902  vertexQueue->clear();
903 
904  // Iterate until the end, pushing back:
905  for (VertexQueueAsMMap::const_iterator vIter = vertexToExpand_; vIter != vertexQueue_.end(); ++vIter)
906  {
907  // Push back:
908  vertexQueue->push_back(vIter->second);
909  }
910  }
911 
913  {
914  ASSERT_SETUP
915 
916  // Variable
917  // The contents on the binary heap (key and edge)
918  std::vector<CostTripleAndVertexPtrPair> queueContents;
919 
920  // Update the queue:
921  this->updateQueue();
922 
923  // Get the contents
924  edgeQueue_.getContent(queueContents);
925 
926  // Clear the vector
927  edgeQueue->clear();
928 
929  // I don't think there's a std::copy way to do this, so just iterate
930  for (const auto &queueElement : queueContents)
931  {
932  edgeQueue->push_back(queueElement.second);
933  }
934  }
936 
938  // Private functions:
939  void BITstar::SearchQueue::updateQueue()
940  {
941  // If we are using strict queue ordering, we must resort the queue before we update it.
942  if (useStrictQueueOrdering_)
943  {
944  // Resort and vertices that have been rewired.
945  this->resort();
946  }
947 
948  // Variables:
949  // Whether to expand:
950  bool expand;
951 
952  expand = true;
953  while (expand)
954  {
955  // Check if there are any vertices to expand:
956  if (vertexToExpand_ != vertexQueue_.end())
957  {
958  // Expand a vertex if the edge queue is empty, or the vertex could place a better edge into it:
959  if (edgeQueue_.empty())
960  {
961  // The edge queue is empty, any edge is better than this!
962  this->expandNextVertex();
963  }
964  // This is isCostBetterThanOrEquivalentTo because of the second ordering criteria. The vertex
965  // expanded could match the edge in queue on total cost, but have less cost-to-come.
966  else if (costHelpPtr_->isCostBetterThanOrEquivalentTo(vertexToExpand_->first.at(0u),
967  edgeQueue_.top()->data.first.at(0u)))
968  {
969  // The vertex *could* give a better edge than our current best edge:
970  this->expandNextVertex();
971  }
972  else
973  {
974  // We are done expanding for now:
975  expand = false;
976  }
977  }
978  else
979  {
980  // There are no vertices left to expand
981  expand = false;
982  }
983  }
984  }
985 
986  void BITstar::SearchQueue::expandNextVertex()
987  {
988  // Should we expand the next vertex?
989  if (this->vertexInsertCondition(vertexToExpand_->second))
990  {
991  // Expand the vertex in the front:
992  this->expandVertex(vertexToExpand_->second);
993 
994  // Increment the vertex token:
995  ++vertexToExpand_;
996  }
997  else
998  {
999  // The next vertex would get pruned, so just jump to the end:
1000  vertexToExpand_ = vertexQueue_.end();
1001  }
1002  }
1003 
1004  void BITstar::SearchQueue::expandVertex(const VertexPtr &vertex)
1005  {
1006 #ifdef BITSTAR_DEBUG
1007  // Assert that this vertex has no outgoing edge queue entries.
1008  if (vertex->hasOutgoingEdgeQueueEntries(numQueueResets_))
1009  {
1010  std::cout << std::endl << "vId: " << vertex->getId() << std::endl;
1011  throw ompl::Exception("Unexpanded vertex already has outgoing entries in the edge queue.");
1012  }
1013 #endif // BITSTAR_DEBUG
1014 
1015  // Should we expand this vertex?
1016  if (this->vertexInsertCondition(vertex))
1017  {
1018  // Variables:
1019  // The vector of nearby samples (either within r or the k-nearest)
1020  VertexPtrVector neighbourSamples;
1021  // The vector of nearby vertices
1022  VertexPtrVector neighbourVertices;
1023 
1024  // Get the set of nearby free states
1025  graphPtr_->nearestSamples(vertex, &neighbourSamples);
1026 
1027  // If we're usjng k-nearest, we technically need to be doing to combined k-nearest.
1028  // So get the nearestVertices and do some post-processing
1029  if (graphPtr_->getUseKNearest())
1030  {
1031  // Get the set of nearby vertices
1032  graphPtr_->nearestVertices(vertex, &neighbourVertices);
1033 
1034  // Post process them:
1035  this->processKNearest(vertex, &neighbourSamples, &neighbourVertices);
1036  }
1037  // No else
1038 
1039  // Add potential edges from the vertex to nearby states.
1040  // Do so intelligently to avoid repeatedly considering the same failed edges (likely due to collision).
1041 
1042  // Add edges to unconnected targets who could ever provide a better solution:
1043  // Has the vertex been expanded into edges towards unconnected samples before?
1044  if (!vertex->hasBeenExpandedToSamples())
1045  {
1046  // It has not, that means none of its outgoing edges have been considered. Add them all
1047  this->enqueueSamples(vertex, neighbourSamples, true);
1048  }
1049  else
1050  {
1051  // It has, which means that outgoing edges to old unconnected vertices have already been considered.
1052  // Only add those that lead to new vertices
1053  this->enqueueSamples(vertex, neighbourSamples, false);
1054  }
1055 
1056  // If the vertex has never been expanded into possible rewiring edges *and* either we're not delaying
1057  // rewiring or we have a solution, we add those rewiring candidates:
1058  if (!vertex->hasBeenExpandedToVertices() && (!delayRewiring_ || hasExactSolution_))
1059  {
1060  // If we're using an r-disc RGG, we will not have gotten the neighbour vertices yet, get them now
1061  if (!graphPtr_->getUseKNearest())
1062  {
1063  // Get the set of nearby vertices
1064  graphPtr_->nearestVertices(vertex, &neighbourVertices);
1065  }
1066  // No else
1067 
1068  // Iterate over the vector of connected targets and add only those who could ever provide a better
1069  // solution:
1070  this->enqueueVertices(vertex, neighbourVertices);
1071  }
1072  // No else
1073  }
1074  // No else
1075  }
1076 
1077  void BITstar::SearchQueue::enqueueSamples(const VertexPtr &vertex, const VertexPtrVector& neighbourSamples, bool addAll)
1078  {
1079  // Iterate through the samples and add each one
1080  for (auto &targetSample : neighbourSamples)
1081  {
1082  // Is the target new? Do we care?
1083  if (addAll || targetSample->isNew())
1084  {
1085  // It is new or we don't care, attempt to queue the edge.
1086  this->enqueueEdgeConditionally(vertex, targetSample);
1087  }
1088  // No else, we've considered this edge before and we're being selective.
1089  }
1090 
1091  // Mark it as expanded
1092  vertex->markExpandedToSamples();
1093  }
1094 
1095  void BITstar::SearchQueue::enqueueVertices(const VertexPtr &vertex, const VertexPtrVector& neighbourVertices)
1096  {
1097  // Iterate over the vector of connected targets and add only those who could ever provide a better
1098  // solution:
1099  for (auto &targetVertex : neighbourVertices)
1100  {
1101  // Make sure it is not the root or myself.
1102  if (!targetVertex->isRoot() && targetVertex->getId() != vertex->getId())
1103  {
1104  // Make sure I am not already the parent
1105  if (targetVertex->getParent()->getId() != vertex->getId())
1106  {
1107  // Make sure the neighbour vertex is not already my parent:
1108  if (vertex->isRoot())
1109  {
1110  // I am root, I have no parent, so attempt to queue the edge:
1111  this->enqueueEdgeConditionally(vertex, targetVertex);
1112  }
1113  else if (targetVertex->getId() != vertex->getParent()->getId())
1114  {
1115  // The neighbour is not my parent, attempt to queue the edge:
1116  this->enqueueEdgeConditionally(vertex, targetVertex);
1117  }
1118  // No else, this vertex is my parent.
1119  }
1120  // No else
1121  }
1122  // No else
1123  }
1124 
1125  // Mark the vertex as expanded into rewirings
1126  vertex->markExpandedToVertices();
1127  }
1128 
1129  void BITstar::SearchQueue::enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child)
1130  {
1131  // Variable:
1132  // The edge:
1133  VertexPtrPair newEdge;
1134 
1135  // Make the edge
1136  newEdge = std::make_pair(parent, child);
1137 
1138  // Should this edge be in the queue?
1139  if (this->edgeInsertCondition(newEdge))
1140  {
1141  this->enqueueEdge(newEdge);
1142  }
1143  // No else, it can never provide a better solution
1144  }
1145 
1146  void BITstar::SearchQueue::processKNearest(const VertexConstPtr &vertex, VertexPtrVector *kNearSamples,
1147  VertexPtrVector *kNearVertices)
1148  {
1149  // Variables
1150  // The position in the sample vector
1151  unsigned int samplePos = 0u;
1152  // The position in the vertex vector
1153  unsigned int vertexPos = 0u;
1154 
1155  // Iterate through the first k in the combined vectors
1156  while (samplePos + vertexPos < graphPtr_->getConnectivityK() &&
1157  (samplePos < kNearSamples->size() || vertexPos < kNearVertices->size()))
1158  {
1159  // Where along are we in the relative vectors?
1160  if (samplePos < kNearSamples->size() && vertexPos >= kNearVertices->size())
1161  {
1162  // There are just samples left. Easy, move the sample token:
1163  ++samplePos;
1164  }
1165  else if (samplePos >= kNearSamples->size() && vertexPos < kNearVertices->size())
1166  {
1167  // There are just vertices left. Easy, move the vertex token:
1168  ++vertexPos;
1169  }
1170  else
1171  {
1172  // Both are left, which is closest?
1173  if (graphPtr_->distanceFunction(kNearVertices->at(vertexPos), vertex) <
1174  graphPtr_->distanceFunction(kNearSamples->at(samplePos), vertex))
1175  {
1176  // The vertex is closer than the sample, move that token:
1177  ++vertexPos;
1178  }
1179  else
1180  {
1181  // The vertex is not closer than the sample, move the sample token:
1182  ++samplePos;
1183  }
1184  }
1185  }
1186 
1187  // Now erase the extra. Resize will truncate the extras
1188  kNearSamples->resize(samplePos);
1189  kNearVertices->resize(vertexPos);
1190  }
1191 
1192  void BITstar::SearchQueue::resortVertex(const VertexPtr &unorderedVertex)
1193  {
1194  // Variables:
1195  // Whether the vertex is expanded.
1196  bool alreadyExpanded;
1197 
1198  // Test if it I am currently expanded.
1199  if (vertexToExpand_ == vertexQueue_.end())
1200  {
1201  // The token is at the end, therefore this vertex is in front of it:
1202  alreadyExpanded = true;
1203  }
1204  else if (this->queueComparison(unorderedVertex->getVertexQueueIter()->first, vertexToExpand_->first))
1205  {
1206  // This vertex is currently in the queue with a cost that is in front of the current token. It has been expanded:
1207  alreadyExpanded = true;
1208  }
1209  else
1210  {
1211  // Otherwise I have not been expanded yet.
1212  alreadyExpanded = false;
1213  }
1214 
1215 #ifdef BITSTAR_DEBUG
1216  // Assert that unexpanded vertices have no outgoing edges in the queue
1217  if (!alreadyExpanded && unorderedVertex->hasOutgoingEdgeQueueEntries(numQueueResets_))
1218  {
1219  throw ompl::Exception("Unexpanded vertex has outgoing queue edges during a resort.");
1220  }
1221 #endif // BITSTAR_DEBUG
1222 
1223  // Update my place in the vertex queue by removing and adding myself:
1224  // Remove myself, not touching my edge-queue entries
1225  this->vertexRemoveHelper(unorderedVertex, false);
1226 
1227  // Reinsert myself, expanding if I cross the token if I am not already expanded but not removing/adding
1228  // either NN struct
1229  this->vertexInsertHelper(unorderedVertex, !alreadyExpanded, false, false);
1230 
1231  // If I was already expanded my edge-queue entries are out of date
1232  if (alreadyExpanded == true)
1233  {
1234  // I have been previously expanded.
1235  // Iterate over my outgoing edges and update them in the edge queue:
1236  for (auto edgePtr = unorderedVertex->outgoingEdgeQueuePtrsBeginConst(numQueueResets_);
1237  edgePtr != unorderedVertex->outgoingEdgeQueuePtrsEndConst(numQueueResets_);
1238  ++edgePtr)
1239  {
1240  // Update the queue value
1241  (*edgePtr)->data.first = this->edgeQueueValue((*edgePtr)->data.second);
1242 
1243  // Update the entry in the queue
1244  edgeQueue_.update(*edgePtr);
1245  }
1246  }
1247  // No else, I was not previously expanded so my edges (if there are now any) were created up to date
1248  }
1249 
1250  std::pair<unsigned int, unsigned int> BITstar::SearchQueue::pruneBranch(const VertexPtr &branchBase)
1251  {
1252 #ifdef BITSTAR_DEBUG
1253  // Assert the vertex is in the tree
1254  if (branchBase->isInTree() == false)
1255  {
1256  throw ompl::Exception("Trying to prune a disconnected vertex. Something went wrong.");
1257  }
1258 #endif // BITSTAR_DEBUG
1259 
1260  // We must iterate over the children of this vertex and prune each one.
1261  // Then we must decide if this vertex (a) gets deleted or (b) placed back on the sample set.
1262  //(a) occurs if it has a lower-bound heuristic greater than the current solution
1263  //(b) occurs if it doesn't.
1264 
1265  // Variables:
1266  // The counter of vertices and samples pruned:
1267  std::pair<unsigned int, unsigned int> numPruned(1u, 0u);
1268  // The vector of my children:
1269  VertexPtrVector children;
1270 
1271  // Disconnect myself from my parent, not cascading costs as I know my children are also being disconnected:
1272  this->disconnectParent(branchBase, false);
1273 
1274  // Get the vector of children
1275  branchBase->getChildren(&children);
1276 
1277  // Remove myself from everything:
1278  numPruned.second = this->vertexRemoveHelper(branchBase, true);
1279 
1280  // Prune my children:
1281  for (auto &child : children)
1282  {
1283  // Prune my children:
1284  numPruned = numPruned + this->pruneBranch(child);
1285  }
1286 
1287  // Return the number pruned
1288  return numPruned;
1289  }
1290 
1291  void BITstar::SearchQueue::disconnectParent(const VertexPtr &oldVertex, bool cascadeCostUpdates)
1292  {
1293 #ifdef BITSTAR_DEBUG
1294  if (oldVertex->hasParent() == false)
1295  {
1296  throw ompl::Exception("An orphaned vertex has been passed for disconnection. Something went wrong.");
1297  }
1298 #endif // BITSTAR_DEBUG
1299 
1300  // Check if my parent has already been pruned. This can occur if we're cascading vertex disconnections.
1301  if (!oldVertex->getParent()->isPruned())
1302  {
1303  // If not, remove myself from my parent's vector of children, not updating down-stream costs
1304  oldVertex->getParent()->removeChild(oldVertex);
1305  }
1306 
1307  // Remove my parent link, cascading cost updates if requested:
1308  oldVertex->removeParent(cascadeCostUpdates);
1309  }
1310 
1311  void BITstar::SearchQueue::vertexInsertHelper(const VertexPtr &newVertex, bool expandIfBeforeToken,
1312  bool removeFromFree, bool addToNNStruct)
1313  {
1314  // Variable:
1315  // The iterator to the new edge in the queue:
1316  VertexQueueIter vertexIter;
1317 
1318  // Add the vertex to the graph
1319  if (addToNNStruct)
1320  {
1321  graphPtr_->addVertex(newVertex, removeFromFree);
1322  }
1323 
1324  // Insert into the order map, getting the iterator
1325  vertexIter = vertexQueue_.insert(std::make_pair(this->vertexQueueValue(newVertex), newVertex));
1326 
1327  // Store the iterator.
1328  newVertex->setVertexQueueIter(vertexIter);
1329 
1330  // Check if we are in front of the token and expand if so:
1331  if (vertexQueue_.size() == 1u)
1332  {
1333  // If the vertex queue is now of size 1, that means that this was the first vertex. Set the token to it
1334  // and don't even think of expanding anything:
1335  vertexToExpand_ = vertexQueue_.begin();
1336  }
1337  else if (expandIfBeforeToken)
1338  {
1339  /*
1340  There are 3ish cases:
1341  1 The new vertex is immediately before the token.
1342  a The token is not at the end: Don't expand and shift the token to the new vertex.
1343  b The token is at the end: Don't expand and shift the token to the new vertex.
1344  2 The new vertex is before the token, but *not* immediately (i.e., there are vertices between it):
1345  a The token is at the end: Expand the vertex
1346  b The token is not at the end: Expand the vertex
1347  3 The new vertex is after the token: Don't expand. It cleanly goes into the vector of vertices to
1348  expand
1349  Note: By shifting the token, we assure that if the new vertex is better than the best edge, it will get
1350  expanded on the next pop.
1351 
1352  The cases look like this (-: expanded vertex, x: unexpanded vertex, X: token (next to expand), *: new
1353  vertex):
1354  We represent the token at the end with no X in the line:
1355 
1356  1a: ---*Xxx -> ---Xxxx
1357  1b: ------* -> ------X
1358  2a: ---*--- -> -------
1359  2b: --*-Xxx -> ----Xxx
1360  3: ---Xx*x -> ---Xxxx
1361  */
1362 
1363  // Variable:
1364  // The vertex before the token. Remember that since we have already added the new vertex, this could be
1365  // ourselves:
1366  VertexQueueIter preToken;
1367 
1368  // Get the vertex before the current token:
1369  preToken = vertexToExpand_;
1370  --preToken;
1371 
1372  // Check if we are immediately before: (1a & 1b)
1373  if (preToken == vertexIter)
1374  {
1375  // The vertex before the token is the newly added vertex. Therefore we can just move the token up to
1376  // the newly added vertex:
1377  vertexToExpand_ = vertexIter;
1378  }
1379  else
1380  {
1381  // We are not immediately before the token.
1382 
1383  // Check if the token is at the end (2a)
1384  if (vertexToExpand_ == vertexQueue_.end())
1385  {
1386  // It is. We've expanded the whole queue, and the new vertex isn't at the end of the queue.
1387  // Expand!
1388  this->expandVertex(newVertex);
1389  }
1390  else
1391  {
1392  // The token is not at the end. That means we can safely dereference it:
1393  // Are we in front of it (2b)?
1394  if (this->queueComparison(vertexIter->first, vertexToExpand_->first))
1395  {
1396  // We're before it, so expand it:
1397  this->expandVertex(newVertex);
1398  }
1399  // No else, the vertex is behind the current token (3) and will get expanded as necessary.
1400  }
1401  }
1402  }
1403  // No else, this vertex must have already been expanded
1404  }
1405 
1406  unsigned int BITstar::SearchQueue::vertexRemoveHelper(const VertexPtr &oldVertex, bool fullyRemove)
1407  {
1408  // Variables
1409  // The number of samples deleted (i.e., if this vertex is NOT recycled as a sample, this is a 1)
1410  unsigned int deleted = 0u;
1411 #ifdef BITSTAR_DEBUG
1412  // The use count of the passed shared pointer. Used in debug mode to assert that we took ownership of our own copy.
1413  unsigned int initCount = oldVertex.use_count();
1414 #endif // BITSTAR_DEBUG
1415  // A copy of the vertex pointer to be removed so we can't delete it out from under ourselves (occurs when
1416  // this function is given an element of the maintained set as the argument)
1417  VertexPtr vertexToDelete(oldVertex);
1418 
1419 #ifdef BITSTAR_DEBUG
1420  // Assert that the vertexToDelete took it's own copy
1421  if (vertexToDelete.use_count() <= initCount)
1422  {
1423  throw ompl::Exception("A code change has prevented SearchQueue::vertexRemoveHelper() "
1424  "from taking it's own copy of the given shared pointer. See "
1425  "https://bitbucket.org/ompl/ompl/issues/364/code-cleanup-breaking-bit");
1426  }
1427  // Check that the vertex is not connected to a parent:
1428  if (vertexToDelete->hasParent() == true && fullyRemove == true)
1429  {
1430  throw ompl::Exception("Cannot delete a vertex connected to a parent unless the vertex is being "
1431  "immediately reinserted, in which case fullyRemove should be false.");
1432  }
1433  // Assert there is something to delete:
1434  if (vertexQueue_.empty() == true)
1435  {
1436  std::cout << std::endl << "vId: " << vertexToDelete->getId() << std::endl;
1437  throw ompl::Exception("Removing a nonexistent vertex. Something went wrong.");
1438  }
1439 #endif // BITSTAR_DEBUG
1440 
1441  // Check if we need to move the expansion token:
1442  if (vertexToDelete->getVertexQueueIter() == vertexToExpand_)
1443  {
1444  // The token is this vertex, move it to the next:
1445  ++vertexToExpand_;
1446  }
1447  // No else, not the token.
1448 
1449  // Remove myself from the vertex queue:
1450  vertexQueue_.erase(vertexToDelete->getVertexQueueIter());
1451  vertexToDelete->clearVertexQueueIter();
1452 
1453  // Remove from lookups map as requested
1454  if (fullyRemove)
1455  {
1456  this->removeEdgesFrom(vertexToDelete);
1457  this->removeEdgesTo(vertexToDelete);
1458 
1459  // Remove myself from the set of connected vertices, this will recycle if necessary.
1460  deleted = graphPtr_->removeVertex(vertexToDelete, true);
1461  }
1462 
1463  // Return if the sample was deleted:
1464  return deleted;
1465  }
1466 
1467  BITstar::SearchQueue::CostDouble BITstar::SearchQueue::vertexQueueValue(const VertexPtr &vertex) const
1468  {
1469  // Construct and return an array
1470  return {{costHelpPtr_->currentHeuristicVertex(vertex), vertex->getCost()}};
1471  }
1472 
1473  BITstar::SearchQueue::CostTriple BITstar::SearchQueue::edgeQueueValue(const VertexPtrPair &edge) const
1474  {
1475  // Construct and return an array
1476  return {{costHelpPtr_->currentHeuristicEdge(edge), costHelpPtr_->currentHeuristicToTarget(edge),
1477  edge.first->getCost()}};
1478  }
1479 
1480  template <std::size_t SIZE>
1481  bool BITstar::SearchQueue::queueComparison(const std::array<ompl::base::Cost, SIZE> &lhs,
1482  const std::array<ompl::base::Cost, SIZE> &rhs) const
1483  {
1484  return std::lexicographical_compare(lhs.cbegin(), lhs.cend(), rhs.cbegin(), rhs.cend(),
1485  [this](const ompl::base::Cost &a, const ompl::base::Cost &b)
1486  {
1487  return costHelpPtr_->isCostBetterThan(a, b);
1488  });
1489  }
1490 
1491  void BITstar::SearchQueue::assertSetup() const
1492  {
1493  if (isSetup_ == false)
1494  {
1495  throw ompl::Exception("BITstar::SearchQueue was used before it was setup.");
1496  }
1497  }
1499 
1501  // Boring sets/gets (Public):
1503  {
1504  useStrictQueueOrdering_ = beStrict;
1505  }
1506 
1508  {
1509  return useStrictQueueOrdering_;
1510  }
1511 
1513  {
1514  delayRewiring_ = delayRewiring;
1515  }
1516 
1518  {
1519  return delayRewiring_;
1520  }
1521 
1523  {
1524  pruneDuringResort_ = prune;
1525  }
1526 
1528  {
1529  return pruneDuringResort_;
1530  }
1531 
1533  {
1534  return numEdgesPopped_;
1535  }
1537  } // geometric
1538 } // ompl
bool edgePruneCondition(const VertexPtrPair &edge) const
The condition used to prune edge (i.e., vertex-pair) out of the queue. Compares currentHeuristicEdge ...
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
bool getPruneDuringResort() const
Prune during resorts.
void markVertexUnsorted(const VertexPtr &vertex)
Mark the queue as requiring resorting downstream of the specified vertex.
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:142
bool isReset() const
Returns true if the queue is reset. This means that no edges have been expanded and the vertex expans...
std::pair< unsigned int, unsigned int > prune(const VertexConstPtr &vertex)
Prune the vertex queue of vertices whose their lower-bound heuristic is greater then the threshold...
bool edgeInsertCondition(const VertexPtrPair &edge) const
The condition used to insert edges into the queue. Compares lowerBoundHeuristicEdge to the given thre...
CostTriple frontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
bool vertexInsertCondition(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
void removeExtraEdgesTo(const VertexPtr &cVertex)
Removes edges in the edge queue that lead to the given vertex that would not be added to the queue no...
unsigned int numVertices()
Returns the number of vertices left to expand. This has nontrivial cost, as the token must be moved t...
void finish()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:69
std::array< ompl::base::Cost, 2u > CostDouble
A pair of costs, i.e., the vertex queue sorting key. Done as an array instead of a pair for consisten...
Definition: SearchQueue.h:100
STL namespace.
bool getStrictQueueOrdering() const
Get whether the queue stays strictly sorted with each rewiring.
void enqueueEdge(const VertexPtrPair &newEdge)
Insert an edge into the edge processing queue. The source vertex of this edge must be in the expansio...
bool samplePruneCondition(const VertexPtr &state) const
The condition used to prune disconnected samples from the free set. Compares lowerBoundHeuristicVerte...
bool isSorted() const
Return whether the queue is still sorted.
SearchQueue(NameFunc nameFunc)
Construct a search queue. It must be setup before use.
Definition: SearchQueue.cpp:75
void getVertices(VertexConstPtrVector *vertexQueue)
Get a copy of the vertices in the vertex queue that are left to be expanded. This is expensive and is...
void setDelayedRewiring(bool delayRewiring)
Delay considering rewiring edges until an initial solution is found.
unsigned int numEdgesFrom(const VertexPtr &pVertex)
Get the number of edges in the queue coming from a specific vertex. Will resort/expand the queue if n...
std::shared_ptr< const Vertex > VertexConstPtr
A constant vertex shared pointer.
Definition: BITstar.h:126
unsigned int numEdgesTo(const VertexPtr &cVertex)
Get the number of edges in the queue pointing to a specific vertex. Will resort/expand the queue if n...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void hasSolution(const ompl::base::Cost &solnCost)
Mark that a solution has been found.
bool getDelayedRewiring() const
Get whether BIT* is delaying rewiring until a solution is found.
std::array< ompl::base::Cost, 3u > CostTriple
A triplet of costs, i.e., the edge queue sorting key.
Definition: SearchQueue.h:112
VertexPtr frontVertex()
Get the best vertex on the queue, leaving it at the front of the vertex queue.
void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:89
unsigned int numUnsorted() const
Return the number of vertices marked as unsorted.
void removeEdgesFrom(const VertexPtr &pVertex)
Erase all edges in the edge queue that leave from the given vertex.
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:136
EdgeQueueAsPairBinHeap::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:120
The exception type for ompl.
Definition: Exception.h:46
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:147
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers.
Definition: BITstar.h:130
void setStrictQueueOrdering(bool beStrict)
Set the queue to stay strictly sorted with each rewiring.
A conceptual representation of samples as an edge-implicit random geometric graph.
Definition: ImplicitGraph.h:64
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared const pointers.
Definition: BITstar.h:132
void unqueueVertex(const VertexPtr &oldVertex)
Erase a vertex from the vertex expansion queue. Will disconnect the vertex from its parent and remove...
CostDouble frontVertexValue()
Get the value of the best vertex on the queue, leaving it at the front of the vertex queue...
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process...
void enqueueVertex(const VertexPtr &newVertex, bool removeFromFree)
Insert a vertex into the vertex expansion queue. Vertices remain in the vertex queue until pruned or ...
bool isVertexExpanded(const VertexConstPtr &vertex) const
Returns whether a given vertex has been expanded or not.
void removeEdgesTo(const VertexPtr &cVertex)
Erase all edges in the edge queue that lead to the given vertex.
VertexPtrPair frontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
void reset()
Reset the queue, clearing all the edge containers and moving the vertex expansion token to the start...
std::shared_ptr< Vertex > VertexPtr
A vertex shared pointer.
Definition: BITstar.h:121
void clear()
Clear the queue to the state of construction.
bool vertexPruneCondition(const VertexPtr &state) const
The condition used to prune vertices out of the queue. Compares currentHeuristicVertex to the given t...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void setPruneDuringResort(bool prune)
Prune during resorts.
void resort()
Resort the queue around the marked unsorted vertices. If allowed, will remove any vertices that need ...
void removeExtraEdgesFrom(const VertexPtr &pVertex)
Removes edges in the edge queue that leave from the given vertex that would not be added to the queue...
VertexQueueAsMMap::iterator VertexQueueIter
An iterator into the vertex queue multimap.
Definition: SearchQueue.h:109
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68