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, Marlin Strub */
36 
37 // My definition:
38 #include "ompl/geometric/planners/informedtrees/bitstar/SearchQueue.h"
39 
40 // For std::lexicographical_compare and the std::*_heap functions.
41 #include <algorithm>
42 // For std::advance.
43 #include <iterator>
44 // For std::move.
45 #include <utility>
46 
47 // OMPL:
48 // For OMPL_INFORM et al.
49 #include "ompl/util/Console.h"
50 // For exceptions:
51 #include "ompl/util/Exception.h"
52 
53 // BIT*:
54 // A collection of common helper functions
55 #include "ompl/geometric/planners/informedtrees/bitstar/HelperFunctions.h"
56 // The vertex class:
57 #include "ompl/geometric/planners/informedtrees/bitstar/Vertex.h"
58 // The cost-helper class:
59 #include "ompl/geometric/planners/informedtrees/bitstar/CostHelper.h"
60 // The implicit graph:
61 #include "ompl/geometric/planners/informedtrees/bitstar/ImplicitGraph.h"
62 
63 // Debug macros
64 #ifdef BITSTAR_DEBUG
65 
66 #define ASSERT_SETUP this->assertSetup();
67 #else
68 #define ASSERT_SETUP
69 #endif // BITSTAR_DEBUG
70 
71 using namespace std::string_literals;
72 
73 namespace ompl
74 {
75  namespace geometric
76  {
78  // Public functions:
79  BITstar::SearchQueue::SearchQueue(NameFunc nameFunc)
80  : nameFunc_(std::move(nameFunc))
81  , edgeQueue_([this](const SortKeyAndVertexPtrPair &lhs, const SortKeyAndVertexPtrPair &rhs) {
82  return lexicographicalBetterThan(lhs.first, rhs.first);
83  }) // This tells the edgeQueue_ to use lexicographical comparison for sorting.
84  , searchId_(std::make_shared<unsigned int>(1u))
85  {
86  }
87 
89  {
90  // Store that I am setup.
91  isSetup_ = true;
92 
93  // Get access to the cost helper and the graph.
94  costHelpPtr_ = costHelpPtr;
95  graphPtr_ = graphPtr;
96 
97  // Set the the cost threshold to infinity to start.
98  solutionCost_ = costHelpPtr_->infiniteCost();
99  }
100 
102  {
103  // Reset everything to the state of construction except for the planner name.
104  // Keep this in the order of the constructors for easy verification.
105 
106  // The queue is not ready for handling data after resetting.
107  isSetup_ = false;
108 
109  // Reset the pointers to external information.
110  costHelpPtr_ = nullptr;
111  graphPtr_ = nullptr;
112 
113  // Clear the queue.
114  edgeQueue_.clear();
115 
116  // Clear the set of inconsistent vertices.
117  inconsistentVertices_.clear();
118 
119  // Reset the inflation factor.
120  inflationFactor_ = 1.0;
121 
122  // Reset the number of queues that have been searched.
123  *searchId_ = 1u;
124 
125  // Reset the cost threshold to infinite cost.
126  solutionCost_ = ompl::base::Cost(std::numeric_limits<double>::infinity());
127 
128  // Make sure the queue doesn't think it has a solution.
129  hasExactSolution_ = false;
130 
131  // Finally, reset the progress info.
132  numEdgesPopped_ = 0u;
133  }
134 
136  {
137  isCascadingOfRewiringsEnabled_ = enable;
138  }
139 
140  void BITstar::SearchQueue::enqueueEdge(const VertexPtrPair &edge)
141  {
142  ASSERT_SETUP
143 
144  // Create convenience aliases.
145  const VertexPtr &parent = edge.first;
146  const VertexPtr &child = edge.second;
147 
148  // If we already have the edge in the queue, we need to update its value.
149  EdgeQueueElemPtr updateEdge = nullptr;
150  for (auto it = child->edgeQueueInLookupConstBegin(); it != child->edgeQueueInLookupConstEnd(); ++it)
151  {
152  if ((*it)->data.second.first->getId() == parent->getId())
153  {
154  updateEdge = (*it);
155  break;
156  }
157  }
158 
159  if (updateEdge) // The edge is already in the queue, we need to update it's sort key (presumably because
160  // the cost-to-come to the source changed).
161  {
162 #ifdef BITSTAR_DEBUG
163  if (updateEdge->data.second.first->getId() != edge.first->getId() ||
164  updateEdge->data.second.second->getId() != edge.second->getId())
165  {
166  throw ompl::Exception("Updating the wrong edge.");
167  }
168 #endif // BITSTAR_DEBUG
169  updateEdge->data.first = this->createSortKey(edge);
170  edgeQueue_.update(updateEdge);
171  }
172  else // This edge is not yet in the queue.
173  {
174  // The iterator to the new edge in the queue:
175  EdgeQueueElemPtr edgeElemPtr;
176 
177  // Insert into the edge queue, getting the element pointer
178  edgeElemPtr = edgeQueue_.insert(std::make_pair(this->createSortKey(edge), edge));
179 
180  // Push the newly created edge back on the vector of edges from the parent.
181  parent->insertInEdgeQueueOutLookup(edgeElemPtr);
182 
183  // Push the newly created edge back on the vector of edges to the child.
184  child->insertInEdgeQueueInLookup(edgeElemPtr);
185  }
186  }
187 
189  {
190  ASSERT_SETUP
191 
192 #ifdef BITSTAR_DEBUG
193  if (edgeQueue_.empty())
194  {
195  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
196  }
197 #endif // BITSTAR_DEBUG
198 
199  // Return the a copy of the front edge.
200  return edgeQueue_.top()->data.second;
201  }
202 
204  {
205  ASSERT_SETUP
206 
207 #ifdef BITSTAR_DEBUG
208  if (edgeQueue_.empty())
209  {
210  throw ompl::Exception("Attempted to access the first element in an empty SearchQueue.");
211  }
212 #endif // BITSTAR_DEBUG
213 
214  // Return a copy of the front value.
215  return edgeQueue_.top()->data.first;
216  }
217 
219  {
220  ASSERT_SETUP
221 #ifdef BITSTAR_DEBUG
222  if (edgeQueue_.empty())
223  {
224  throw ompl::Exception("Attempted to pop an empty SearchQueue.");
225  }
226 #endif // BITSTAR_DEBUG
227 
228  // Increment the counter of popped edges.
229  ++numEdgesPopped_;
230 
231  // Get the front element in the edge queue.
232  EdgeQueueElemPtr frontEdgeQueueElement = edgeQueue_.top();
233  VertexPtrPair frontEdge = frontEdgeQueueElement->data.second;
234 
235 #ifdef BITSTAR_DEBUG
236  if (frontEdge.first->isPruned() || frontEdge.second->isPruned())
237  {
238  throw ompl::Exception("The edge queue contains an edge with a pruned vertex.");
239  }
240 #endif // BITSTAR_DEBUG
241 
242  // Remove the edge from the respective vertex lookups.
243  frontEdge.first->removeFromEdgeQueueOutLookup(frontEdgeQueueElement);
244  frontEdge.second->removeFromEdgeQueueInLookup(frontEdgeQueueElement);
245 
246  // Remove it from the queue.
247  edgeQueue_.pop();
248 
249  // Return the edge.
250  return frontEdge;
251  }
252 
254  {
255  ASSERT_SETUP
256 
257  // Flag
258  hasExactSolution_ = true;
259 
260  // Store
261  solutionCost_ = solutionCost;
262  }
263 
265  {
266  ASSERT_SETUP
267 
268  if (!edgeQueue_.empty())
269  {
270  // Iterate over the vector of incoming edges to this vertex and remove them from the queue (and clean up
271  // their other lookup).
272  for (auto it = vertex->edgeQueueInLookupConstBegin(); it != vertex->edgeQueueInLookupConstEnd(); ++it)
273  {
274  // Remove the edge from the *other* lookup (by value since this is NOT an iter to THAT container).
275  // No need to remove from this lookup, as that's being cleared.
276  (*it)->data.second.first->removeFromEdgeQueueOutLookup(*it);
277 
278  // Finally remove it from the queue
279  edgeQueue_.remove(*it);
280  }
281 
282  // Clear the list.
283  vertex->clearEdgeQueueInLookup();
284  }
285  // No else, nothing to remove from.
286  }
287 
289  {
290  ASSERT_SETUP
291 
292  if (!edgeQueue_.empty())
293  {
294  // Iterate over the vector of outgoing edges to this vertex and remove them from the queue (and clean up
295  // their other lookup).
296  for (auto it = vertex->edgeQueueOutLookupConstBegin(); it != vertex->edgeQueueOutLookupConstEnd(); ++it)
297  {
298  // Remove the edge from the *other* lookup (by value since this is NOT an iter to THAT container).
299  // No need to remove from this lookup, as that's being cleared.
300  (*it)->data.second.second->removeFromEdgeQueueInLookup(*it);
301 
302  // Finally, remove it from the queue.
303  edgeQueue_.remove(*it);
304  }
305 
306  // Clear the list.
307  vertex->clearEdgeQueueOutLookup();
308  }
309  // No else, nothing to remove from.
310  }
311 
313  {
314  this->removeOutEdgesConnectedToVertexFromQueue(vertex);
315  this->removeInEdgesConnectedToVertexFromQueue(vertex);
316  }
317 
319  {
320 #ifdef BITSTAR_DEBUG
321  if (vertex->isConsistent())
322  {
323  throw ompl::Exception("Attempting to remove a consistent vertex from the set of inconsistent "
324  "vertices.");
325  }
326 #endif // BITSTAR_DEBUG
327  inconsistentVertices_.erase(
328  std::remove_if(inconsistentVertices_.begin(), inconsistentVertices_.end(),
329  [vertex](const VertexPtr &element) { return vertex->getId() == element->getId(); }),
330  inconsistentVertices_.end());
331  }
332 
334  {
335  ASSERT_SETUP
336 
337  // Clear the edge queue.
338  edgeQueue_.clear();
339 
340  // Increment the queue processing number.
341  ++(*searchId_);
342  }
343 
345  {
346  ASSERT_SETUP
347 
348  // Insert the outgoing edges of the start vertices.
349  for (auto it = graphPtr_->startVerticesBeginConst(); it != graphPtr_->startVerticesEndConst(); ++it)
350  {
351 #ifdef BITSTAR_DEBUG
352  if ((*it)->isPruned())
353  {
354  throw ompl::Exception("Inserting outgoing edges of a pruned start vertex.");
355  }
356 #endif // BITSTAR_DEBUG
357  this->insertOutgoingEdges(*it);
358  }
359  }
360 
362  {
363  inflationFactor_ = factor;
364  }
365 
367  {
368  return inflationFactor_;
369  }
370 
371  std::shared_ptr<const unsigned int> BITstar::SearchQueue::getSearchId() const
372  {
373  return searchId_;
374  }
375 
377  {
378  ASSERT_SETUP
379 
380 #ifdef BITSTAR_DEBUG
381  if (state->isPruned())
382  {
383  throw ompl::Exception("Asking whether pruned state can possibly improve current solution.");
384  }
385 #endif // BITSTAR_DEBUG
386 
387  // Threshold should always be g_t(x_g)
388 
389  // Can it ever be a better solution?
390  // Just in case we're using a vertex that is exactly optimally connected
391  // g^(v) + h^(v) <= g_t(x_g)?
392  return costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicVertex(state),
393  solutionCost_);
394  }
395 
397  {
398  ASSERT_SETUP
399 
400  // Can it ever be a better solution? Less-than-equal to just in case we're using an edge that is exactly
401  // optimally connected
402  // g^(v) + c^(v,x) + h^(x) <= g_t(x_g)?
403  bool canImprove = costHelpPtr_->isCostBetterThanOrEquivalentTo(costHelpPtr_->lowerBoundHeuristicEdge(edge),
404  solutionCost_);
405 
406  // If the child is connected already, we need to check if we could do better than it's current connection.
407  // But only if we're inserting the edge
408  if (edge.second->hasParent() && canImprove)
409  {
410  // Can it ever be a better path to the vertex? Less-than-equal to just in case we're using an edge that
411  // is exactly optimally connected
412  // g^(v) + c^(v,x) <= g_t(x)?
413  canImprove = costHelpPtr_->isCostBetterThanOrEquivalentTo(
414  costHelpPtr_->lowerBoundHeuristicToTarget(edge), edge.second->getCost()); // Ever rewire?
415  }
416 
417  return canImprove;
418  }
419 
421  {
422  ASSERT_SETUP
423 
424  return edgeQueue_.size();
425  }
426 
428  {
429  ASSERT_SETUP
430 
431  return edgeQueue_.empty();
432  }
433 
435  {
436  ASSERT_SETUP
437 
438  // Clear the inout argument.
439  edgeQueue->clear();
440 
441  // Get the contents on the binary heap (key and edge).
442  std::vector<SortKeyAndVertexPtrPair> queueContents;
443  edgeQueue_.getContent(queueContents);
444 
445  // Fill the inout argument.
446  for (const auto &queueElement : queueContents)
447  {
448  edgeQueue->push_back(queueElement.second);
449  }
450  }
451 
453  {
454 #ifdef BITSTAR_DEBUG
455  if (vertex->isPruned())
456  {
457  throw ompl::Exception("Inserting outgoing edges of pruned vertex.");
458  }
459 #endif // BITSTAR_DEBUG
460  // Should we expand this vertex?
461  if (this->canPossiblyImproveCurrentSolution(vertex))
462  {
463  // Get the neighbouring samples.
464  VertexPtrVector neighbourSamples;
465  graphPtr_->nearestSamples(vertex, &neighbourSamples);
466 
467  // Add all outgoing edges to neighbouring vertices and samples.
468  this->enqueueEdges(vertex, neighbourSamples);
469  }
470  // No else
471  }
472 
474  {
475  // Insert all outgoing edges of the inconsistent vertices.
476  for (const auto &vertex : inconsistentVertices_)
477  {
478 #ifdef BITSTAR_DEBUG
479  if (vertex->isPruned())
480  {
481  throw ompl::Exception("Attempting to insert outgoing edges of an inconsistent "
482  "vertex that has been pruned.");
483  }
484 #endif // BITSTAR_DEBUG
485  this->insertOutgoingEdges(vertex);
486  }
487  }
488 
490  {
491  inconsistentVertices_.clear();
492  }
493 
495  {
496  // Ok this is going to be kinda dirty. We would like to have access to the actual underlying
497  // std::vector of the binary heap, holding pointers to the elements in the heap.
498  // Unfortunately, the binary heap interface only provides access to a copy. Now, we can still
499  // access get the pointers to the elements because we stored them upon insertion. But it's a mess
500  // (and suggests a flawed encapsulation or incomplete interface of the bin heap class?).
501 
502  // Get a copy of the contents.
503  std::vector<SortKeyAndVertexPtrPair> contentCopy;
504  edgeQueue_.getContent(contentCopy);
505 
506  // Now, get the parent vertices of all the edges still in the queue.
507  std::set<VertexPtr> parents;
508  for (const auto &element : contentCopy)
509  {
510  parents.insert(element.second.first);
511  }
512 
513  // Update the sort keys of all outgoing edges of all vertices that have outgoing edges in the queue.
514  for (const auto &parent : parents)
515  {
516  for (auto it = parent->edgeQueueOutLookupConstBegin(); it != parent->edgeQueueOutLookupConstEnd(); ++it)
517  {
518  (*it)->data.first = this->createSortKey((*it)->data.second);
519  }
520  }
521 
522  // All edges have an updated key, let's rebuild the queue. Presumably this is more efficient than calling
523  // EdgeQueue::update() on each edge individually while looping?
524  edgeQueue_.rebuild();
525  }
526 
528  {
529  assert(elementPtr);
530 
531  // Create the up-to-date sort key for this edge.
532  elementPtr->data.first = createSortKey(elementPtr->data.second);
533 
534  // Update its position in the queue.
535  edgeQueue_.update(elementPtr);
536  }
537 
539  {
540 #ifdef BITSTAR_DEBUG
541  if (vertex->isConsistent())
542  {
543  ompl::Exception("Attempted to add a consistent vertex to the inconsistent set.");
544  }
545  if (!vertex->isExpandedOnCurrentSearch())
546  {
547  ompl::Exception("Attempted to add an unexpanded vertex to the inconsistent set.");
548  }
549 #endif // BITSTAR_DEBUG
550 
551  inconsistentVertices_.push_back(vertex);
552  }
553 
554  void BITstar::SearchQueue::enqueueEdges(const VertexPtr &parent, const VertexPtrVector &possibleChildren)
555  {
556 #ifdef BITSTAR_DEBUG
557  if (!parent->isInTree())
558  {
559  auto msg = "Trying to enqueue edges from a parent (" + std::to_string(parent->getId()) +
560  ") that's not in the tree."s;
561  throw std::runtime_error(msg);
562  }
563 #endif
564  // Start with this vertex' current kiddos.
565  VertexPtrVector currentChildren;
566  parent->getChildren(&currentChildren);
567  for (const auto &child : currentChildren)
568  {
569  this->enqueueEdgeConditionally(parent, child);
570  }
571 
572  // We need to store whether an outgoing edge is a rewiring.
573  bool isExpandedAsRewiring = false;
574 
575  // Now consider all neighbouring vertices that are not already my kids.
576  for (auto &child : possibleChildren)
577  {
578  // If this sample is not connected to the search tree, just enqueue the edge if it's useful.
579  if (!child->isInTree())
580  {
581  this->enqueueEdgeConditionally(parent, child);
582  }
583  else // If this sample is part of the tree, we need to be a little more careful.
584  {
585  if (isCascadingOfRewiringsEnabled_ || !parent->hasEverBeenExpandedAsRewiring())
586  {
587  // Remember that this parent is expanded as a rewiring.
588  isExpandedAsRewiring = true;
589 
590  // Make sure the child is not the root and distinct from this vertex (which is the parent).
591  if (!child->isRoot() && child->getId() != parent->getId())
592  {
593  // Make sure edges to kiddos aren't added twice.
594  if (child->getParent()->getId() != parent->getId())
595  {
596  // Make sure the neighbour vertex is not already my parent.
597  if (parent->isRoot() || child->getId() != parent->getParent()->getId())
598  {
599  // The neighbour is not my parent, attempt to queue the edge.
600  this->enqueueEdgeConditionally(parent, child);
601  }
602  // No else, this vertex is my parent.
603  }
604  // No else
605  }
606  // No else
607  }
608  }
609  }
610 
611  // If the parent is expanded to a vertex in the tree, it is a rewiring. This needs to be registered.
612  if (isExpandedAsRewiring)
613  {
614  parent->registerRewiringExpansion();
615  }
616  }
617 
618  void BITstar::SearchQueue::enqueueEdgeConditionally(const VertexPtr &parent, const VertexPtr &child)
619  {
620  // Don't enqueue the edge if it's blacklisted.
621  if (parent->isBlacklistedAsChild(child))
622  {
623  return;
624  }
625  else
626  {
627  // Create the edge object.
628  VertexPtrPair newEdge = std::make_pair(parent, child);
629 
630  // Enqueue the edge only if it can possibly improve the current solution.
631  if (this->canPossiblyImproveCurrentSolution(newEdge))
632  {
633  this->enqueueEdge(newEdge);
634  }
635  }
636  }
637 
638  BITstar::SearchQueue::SortKey BITstar::SearchQueue::createSortKey(const VertexPtrPair &edge) const
639  {
640  // The sort key of an edge (u, v) is [ g_t(u) + c^hat(u, v) + epsilon_s * h^hat(v); g_t(u) + c^hat(u, v);
641  // g_t(u) ].
642  return {{costHelpPtr_->combineCosts(
643  costHelpPtr_->currentHeuristicToTarget(edge),
644  costHelpPtr_->inflateCost(costHelpPtr_->costToGoHeuristic(edge.second), inflationFactor_)),
645  costHelpPtr_->currentHeuristicToTarget(edge), edge.first->getCost()}};
646  }
647 
648  bool BITstar::SearchQueue::lexicographicalBetterThan(const std::array<ompl::base::Cost, 3> &lhs,
649  const std::array<ompl::base::Cost, 3> &rhs) const
650  {
651  return std::lexicographical_compare(lhs.cbegin(), lhs.cend(), rhs.cbegin(), rhs.cend(),
652  [this](const ompl::base::Cost &a, const ompl::base::Cost &b) {
653  return costHelpPtr_->isCostBetterThan(a, b);
654  });
655  }
656 
657  void BITstar::SearchQueue::assertSetup() const
658  {
659  if (isSetup_ == false)
660  {
661  throw ompl::Exception("BITstar::SearchQueue was used before it was setup.");
662  }
663  }
664 
666  {
667  return numEdgesPopped_;
668  }
669 
670  } // namespace geometric
671 } // namespace ompl
void insertOutgoingEdgesOfInconsistentVertices()
Insert the outgoing edges of all inconsistent vertices.
SortKey getFrontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
void removeInEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that lead to the given vertex.
void clear()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:245
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition: SearchQueue.h:180
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:230
std::shared_ptr< const unsigned int > getSearchId() const
Allow access to the current search id.
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:133
bool canPossiblyImproveCurrentSolution(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
void removeOutEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that leave from the given vertex.
void enableCascadingRewirings(bool enable)
Set whether cascading of rewirings is enabled.
void rebuildEdgeQueue()
Update all the sort keys of the edges in the queue and resort.
std::pair< SortKey, VertexPtrPair > SortKeyAndVertexPtrPair
The data stored in the edge-queue binary heap.
Definition: SearchQueue.h:171
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:221
void clearInconsistentSet()
Clear the set of inconsistent vertices.
void setInflationFactor(double factor)
Set the cost-to-go inflation factor.
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:212
std::array< ompl::base::Cost, 3u > SortKey
A triplet of costs, i.e., the edge queue sorting key.
Definition: SearchQueue.h:168
void reset()
Reset the queue to the state of construction.
void registerSolutionCost(const ompl::base::Cost &solutionCost)
Mark that a solution has been found.
double getInflationFactor() const
Get the cost-to-go inflation factor.
A conceptual representation of samples as an edge-implicit random geometric graph.
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:239
void addToInconsistentSet(const VertexPtr &vertex)
Add a vertex to the set of inconsistent vertices.
VertexPtrPair getFrontEdge()
Get the best edge on the queue, leaving it at the front of the edge queue.
void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr)
Setup the SearchQueue, must be called before use.
Definition: SearchQueue.cpp:88
void removeAllEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that are connected to the given vertex.
void insertOutgoingEdges(const VertexPtr &vertex)
Update the edge queue by adding all the potential edges from the vertex to nearby states.
The exception type for ompl.
Definition: Exception.h:78
void removeFromInconsistentSet(const VertexPtr &vertex)
Remove a vertex from the set of inconsistent vertices.
void update(const EdgeQueueElemPtr elementPtr)
Update the sort key of a particular edge and its position in the queue.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void insertOutgoingEdgesOfStartVertices()
Insert the outgoing edges of all start vertices.