Loading...
Searching...
No Matches
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
66#define ASSERT_SETUP this->assertSetup();
67#else
68#define ASSERT_SETUP
69#endif // BITSTAR_DEBUG
70
71using namespace std::string_literals;
72
73namespace ompl
74{
75 namespace geometric
76 {
78 // Public functions:
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
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 {
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
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
A helper class to handle the various heuristic functions in one place.
Definition CostHelper.h:70
A conceptual representation of samples as an edge-implicit random geometric graph.
void reset()
Reset the queue to the state of construction.
EdgeQueue::Element * EdgeQueueElemPtr
An element pointer into the edge queue binary heap.
Definition SearchQueue.h:84
VertexPtrPair popFrontEdge()
Pop the best edge off the queue, removing it from the front of the edge queue in the process.
void insertOutgoingEdges(const VertexPtr &vertex)
Update the edge queue by adding all the potential edges from the vertex to nearby states.
double getInflationFactor() const
Get the cost-to-go inflation factor.
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.
bool isEmpty()
Returns true if the queue is empty. In the case where the edge queue is empty but the vertex queue is...
std::array< ompl::base::Cost, 3u > SortKey
A triplet of costs, i.e., the edge queue sorting key.
Definition SearchQueue.h:72
VertexPtrPair getFrontEdge()
Get 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.
bool canPossiblyImproveCurrentSolution(const VertexPtr &state) const
The condition used to insert vertices into the queue. Compares lowerBoundHeuristicVertex to the given...
void setup(CostHelper *costHelpPtr, ImplicitGraph *graphPtr)
Setup the SearchQueue, must be called before use.
std::shared_ptr< const unsigned int > getSearchId() const
Allow access to the current search id.
void clear()
Finish the queue if it is sorted, if not resort the queue. Finishing the queue clears all the edge co...
void insertOutgoingEdgesOfStartVertices()
Insert the outgoing edges of all start vertices.
unsigned int numEdges()
Returns the number of edges in the queue. Will resort/expand the queue if necessary.
void insertOutgoingEdgesOfInconsistentVertices()
Insert the outgoing edges of all inconsistent vertices.
void registerSolutionCost(const ompl::base::Cost &solutionCost)
Mark that a solution has been found.
SortKey getFrontEdgeValue()
Get the value of the best edge on the queue, leaving it at the front of the edge queue.
void addToInconsistentSet(const VertexPtr &vertex)
Add a vertex to the set of inconsistent vertices.
void removeFromInconsistentSet(const VertexPtr &vertex)
Remove a vertex from the set of inconsistent vertices.
void removeOutEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that leave from the given vertex.
unsigned int numEdgesPopped() const
The number of edges popped off the queue for processing (numEdgesPopped_).
void setInflationFactor(double factor)
Set the cost-to-go inflation factor.
SearchQueue(NameFunc nameFunc)
Construct a search queue. It must be setup before use.
void removeAllEdgesConnectedToVertexFromQueue(const VertexPtr &vertex)
Erase all edges in the edge queue that are connected to the given vertex.
void getEdges(VertexConstPtrPairVector *edgeQueue)
Get a copy of the edge queue. This is expensive and is only meant for animations/debugging.
void update(const EdgeQueueElemPtr elementPtr)
Update the sort key of a particular edge and its position in the queue.
std::pair< SortKey, VertexPtrPair > SortKeyAndVertexPtrPair
The data stored in the edge-queue binary heap.
Definition SearchQueue.h:75
void clearInconsistentSet()
Clear the set of inconsistent vertices.
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstEnd()
Get an iterator to the end of the outgoing edge queue entry vector. Will clear existing in/out lookup...
Definition Vertex.cpp:782
bool isConsistent() const
Whether the vertex is consistent.
Definition Vertex.cpp:487
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstBegin()
Get an iterator to the front of the incoming edge queue entry vector. Will clear existing in/out look...
Definition Vertex.cpp:642
void getChildren(VertexConstPtrVector *children) const
Get the children of a vertex as constant pointers.
Definition Vertex.cpp:299
void insertInEdgeQueueInLookup(const SearchQueue::EdgeQueueElemPtr &inEdge)
Add to the list of the edge queue entries that point in to this vertex. Will clear existing in/out lo...
Definition Vertex.cpp:542
bool isInTree() const
Get whether a vertex is in the search tree or a sample (i.e., a vertex of the RRG).
Definition Vertex.cpp:176
bool hasEverBeenExpandedAsRewiring() const
Returns whether the vertex has ever been expanded as a rewiring.
Definition Vertex.cpp:507
void clearEdgeQueueInLookup()
Clear the pointers to all of the incoming edge queue entries.
Definition Vertex.cpp:635
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueInLookupConstEnd()
Get an iterator to the end of the incoming edge queue entry vector. Will clear existing in/out lookup...
Definition Vertex.cpp:652
VertexConstPtr getParent() const
Get a const pointer to the parent of this vertex.
Definition Vertex.cpp:198
bool isPruned() const
Whether the vertex has been pruned.
Definition Vertex.cpp:492
bool isRoot() const
Returns whether the vertex is the root of the search tree.
Definition Vertex.cpp:162
BITstar::SearchQueue::EdgeQueueElemPtrVector::const_iterator edgeQueueOutLookupConstBegin()
Get an iterator to the front of the outgoing edge queue entry vector. Will clear existing in/out look...
Definition Vertex.cpp:772
void registerRewiringExpansion()
Mark expansion to vertices.
Definition Vertex.cpp:522
bool isExpandedOnCurrentSearch() const
Returns whether the vertex is expaned on current search.
Definition Vertex.cpp:502
void clearEdgeQueueOutLookup()
Clear the pointers to all of the outgoing edge queue entries.
Definition Vertex.cpp:765
BITstar::VertexId getId() const
The (unique) vertex ID.
Definition Vertex.cpp:138
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition BITstar.h:125
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition BITstar.h:134
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition BITstar.h:116
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition BITstar.h:143
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition BITstar.h:149
This namespace contains code that is specific to planning under geometric constraints.
Message namespace. This contains classes needed to output error messages (or logging) from within the...
Definition Console.h:82
Main namespace. Contains everything in this library.
STL namespace.