Loading...
Searching...
No Matches
AITstar.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Oxford
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 names of the copyright holders 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: Marlin Strub
36
37#include "ompl/geometric/planners/informedtrees/AITstar.h"
38
39#include <algorithm>
40#include <cmath>
41#include <string>
42
43#include <boost/range/adaptor/reversed.hpp>
44
45#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
46#include "ompl/util/Console.h"
47
48using namespace std::string_literals;
49using namespace ompl::geometric::aitstar;
50
51namespace ompl
52{
53 namespace geometric
54 {
56 : ompl::base::Planner(spaceInformation, "AITstar")
57 , solutionCost_()
58 , graph_(solutionCost_)
59 , forwardQueue_([this](const auto &lhs, const auto &rhs) { return isEdgeBetter(lhs, rhs); })
60 , reverseQueue_([this](const auto &lhs, const auto &rhs) { return isVertexBetter(lhs, rhs); })
61 {
62 // Specify AIT*'s planner specs.
63 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
64 specs_.multithreaded = false;
65 specs_.approximateSolutions = true;
66 specs_.optimizingPaths = true;
67 specs_.directed = true;
68 specs_.provingSolutionNonExistence = false;
69 specs_.canReportIntermediateSolutions = true;
70
71 // Register the setting callbacks.
72 declareParam<bool>("use_k_nearest", this, &AITstar::setUseKNearest, &AITstar::getUseKNearest, "0,1");
73 declareParam<double>("rewire_factor", this, &AITstar::setRewireFactor, &AITstar::getRewireFactor,
74 "1.0:0.01:3.0");
75 declareParam<std::size_t>("samples_per_batch", this, &AITstar::setBatchSize, &AITstar::getBatchSize,
76 "1:1:1000");
77 declareParam<bool>("use_graph_pruning", this, &AITstar::enablePruning, &AITstar::isPruningEnabled, "0,1");
78 declareParam<bool>("find_approximate_solutions", this, &AITstar::trackApproximateSolutions,
80 declareParam<std::size_t>("set_max_num_goals", this, &AITstar::setMaxNumberOfGoals,
81 &AITstar::getMaxNumberOfGoals, "1:1:1000");
82
83 // Register the progress properties.
84 addPlannerProgressProperty("iterations INTEGER", [this]() { return std::to_string(numIterations_); });
85 addPlannerProgressProperty("best cost DOUBLE", [this]() { return std::to_string(solutionCost_.value()); });
86 addPlannerProgressProperty("state collision checks INTEGER",
87 [this]() { return std::to_string(graph_.getNumberOfStateCollisionChecks()); });
88 addPlannerProgressProperty("edge collision checks INTEGER",
89 [this]() { return std::to_string(numEdgeCollisionChecks_); });
90 addPlannerProgressProperty("nearest neighbour calls INTEGER",
91 [this]() { return std::to_string(graph_.getNumberOfNearestNeighborCalls()); });
92 }
93
95 {
96 // Call the base-class setup.
97 Planner::setup();
98
99 // Check that a problem definition has been set.
100 if (static_cast<bool>(Planner::pdef_))
101 {
102 // Default to path length optimization objective if none has been specified.
103 if (!pdef_->hasOptimizationObjective())
104 {
105 OMPL_WARN("%s: No optimization objective has been specified. Defaulting to path length.",
106 Planner::getName().c_str());
107 Planner::pdef_->setOptimizationObjective(
108 std::make_shared<ompl::base::PathLengthOptimizationObjective>(Planner::si_));
109 }
110
111 if (static_cast<bool>(pdef_->getGoal()))
112 {
113 // If we were given a goal, make sure its of appropriate type.
114 if (!(pdef_->getGoal()->hasType(ompl::base::GOAL_SAMPLEABLE_REGION)))
115 {
116 OMPL_ERROR("AIT* is currently only implemented for goals that can be cast to "
117 "ompl::base::GOAL_SAMPLEABLE_GOAL_REGION.");
118 setup_ = false;
119 return;
120 }
121 }
122
123 // Pull the optimization objective through the problem definition.
124 objective_ = pdef_->getOptimizationObjective();
125
126 // Initialize the solution cost to be infinite.
127 solutionCost_ = objective_->infiniteCost();
128 approximateSolutionCost_ = objective_->infiniteCost();
129 approximateSolutionCostToGoal_ = objective_->infiniteCost();
130
131 // Pull the motion validator through the space information.
132 motionValidator_ = si_->getMotionValidator();
133
134 // Setup a graph.
135 graph_.setup(si_, pdef_, &pis_);
136 }
137 else
138 {
139 // AIT* can't be setup without a problem definition.
140 setup_ = false;
141 OMPL_WARN("AIT*: Unable to setup without a problem definition.");
142 }
143 }
144
146 {
147 // Call the base planners validity check. This checks if the
148 // planner is setup if not then it calls setup().
150
151 // Ensure the planner is setup.
152 if (!setup_)
153 {
154 OMPL_ERROR("%s: The planner is not setup.", name_.c_str());
156 }
157
158 // Ensure the space is setup.
159 if (!si_->isSetup())
160 {
161 OMPL_ERROR("%s: The space information is not setup.", name_.c_str());
163 }
164
166 }
167
170 {
171 // If the graph currently does not have a start state, try to get one.
172 if (!graph_.hasAStartState())
173 {
174 graph_.updateStartAndGoalStates(terminationCondition, &pis_);
175
176 // If we could not get a start state, then there's nothing to solve.
177 if (!graph_.hasAStartState())
178 {
179 OMPL_WARN("%s: No solution can be found as no start states are available", name_.c_str());
181 }
182 }
183
184 // If the graph currently does not have a goal state, we wait until we get one.
185 if (!graph_.hasAGoalState())
186 {
187 graph_.updateStartAndGoalStates(terminationCondition, &pis_);
188
189 // If the graph still doesn't have a goal after waiting, then there's nothing to solve.
190 if (!graph_.hasAGoalState())
191 {
192 OMPL_WARN("%s: No solution can be found as no goal states are available", name_.c_str());
194 }
195 }
196
197 // Would it be worth implementing a 'setup' or 'checked' status type?
199 }
200
202 {
203 graph_.clear();
204 forwardQueue_.clear();
205 reverseQueue_.clear();
206 solutionCost_ = objective_->infiniteCost();
207 approximateSolutionCost_ = objective_->infiniteCost();
208 approximateSolutionCostToGoal_ = objective_->infiniteCost();
209 numIterations_ = 0u;
210 numInconsistentOrUnconnectedTargets_ = 0u;
211 Planner::clear();
212 setup_ = false;
213 }
214
216 {
217 // Ensure that the planner and state space are setup before solving.
218 auto status = ensureSetup();
219
220 // Return early if the planner or state space are not setup.
222 {
223 return status;
224 }
225
226 // Ensure that the problem has start and goal states before solving.
227 status = ensureStartAndGoalStates(terminationCondition);
228
229 // Return early if the problem cannot be solved.
232 {
233 return status;
234 }
235
236 OMPL_INFORM("%s: Solving the given planning problem. The current best solution cost is %.4f", name_.c_str(),
237 solutionCost_.value());
238
239 // Iterate to solve the problem.
240 while (!terminationCondition && !objective_->isSatisfied(solutionCost_))
241 {
242 iterate(terminationCondition);
243 }
244
245 // Someone might call ProblemDefinition::clearSolutionPaths() between invocations of Planner::sovle(), in
246 // which case previously found solutions are not registered with the problem definition anymore.
247 status = updateSolution();
248
249 // Let the caller know the status.
250 informAboutPlannerStatus(status);
251 return status;
252 }
253
255 {
256 return solutionCost_;
257 }
258
260 {
261 // base::PlannerDataVertex takes a raw pointer to a state. I want to guarantee, that the state lives as
262 // long as the program lives.
263 static std::set<std::shared_ptr<Vertex>,
264 std::function<bool(const std::shared_ptr<Vertex> &, const std::shared_ptr<Vertex> &)>>
265 liveStates([](const auto &lhs, const auto &rhs) { return lhs->getId() < rhs->getId(); });
266
267 // Fill the planner progress properties.
268 Planner::getPlannerData(data);
269
270 // Get the vertices.
271 auto vertices = graph_.getVertices();
272
273 // Add the vertices and edges.
274 for (const auto &vertex : vertices)
275 {
276 // Add the vertex to the live states.
277 liveStates.insert(vertex);
278
279 // Add the vertex as the right kind of vertex.
280 if (graph_.isStart(vertex))
281 {
282 data.addStartVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
283 }
284 else if (graph_.isGoal(vertex))
285 {
286 data.addGoalVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
287 }
288 else
289 {
290 data.addVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
291 }
292
293 // If it has a parent, add the corresponding edge.
294 if (vertex->hasForwardParent())
295 {
296 data.addEdge(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()),
297 ompl::base::PlannerDataVertex(vertex->getForwardParent()->getState(),
298 vertex->getForwardParent()->getId()));
299 }
300 }
301 }
302
303 void AITstar::setBatchSize(std::size_t batchSize)
304 {
305 batchSize_ = batchSize;
306 }
307
308 std::size_t AITstar::getBatchSize() const
309 {
310 return batchSize_;
311 }
312
313 void AITstar::setRewireFactor(double rewireFactor)
314 {
315 graph_.setRewireFactor(rewireFactor);
316 }
317
319 {
320 return graph_.getRewireFactor();
321 }
322
324 {
325 trackApproximateSolutions_ = track;
326 if (!trackApproximateSolutions_)
327 {
328 if (static_cast<bool>(objective_))
329 {
330 approximateSolutionCost_ = objective_->infiniteCost();
331 approximateSolutionCostToGoal_ = objective_->infiniteCost();
332 }
333 }
334 }
335
337 {
338 return trackApproximateSolutions_;
339 }
340
341 void AITstar::enablePruning(bool prune)
342 {
343 isPruningEnabled_ = prune;
344 }
345
347 {
348 return isPruningEnabled_;
349 }
350
351 void AITstar::setUseKNearest(bool useKNearest)
352 {
353 graph_.setUseKNearest(useKNearest);
354 }
355
357 {
358 return graph_.getUseKNearest();
359 }
360
361 void AITstar::setMaxNumberOfGoals(unsigned int numberOfGoals)
362 {
363 graph_.setMaxNumberOfGoals(numberOfGoals);
364 }
365
366 unsigned int AITstar::getMaxNumberOfGoals() const
367 {
368 return graph_.getMaxNumberOfGoals();
369 }
370
371 void AITstar::rebuildForwardQueue()
372 {
373 // Get all edges from the queue.
374 std::vector<Edge> edges;
375 forwardQueue_.getContent(edges);
376
377 // Rebuilding the queue invalidates the incoming and outgoing lookup.
378 for (const auto &edge : edges)
379 {
380 edge.getChild()->resetForwardQueueIncomingLookup();
381 edge.getParent()->resetForwardQueueOutgoingLookup();
382 }
383
384 // Clear the queue.
385 forwardQueue_.clear();
386 numInconsistentOrUnconnectedTargets_ = 0u;
387
388 // Insert all edges into the queue if they connect vertices that have been processed, otherwise store
389 // them in the cache of edges that are to be inserted.
390 for (auto &edge : edges)
391 {
392 insertOrUpdateInForwardQueue(
393 Edge(edge.getParent(), edge.getChild(), computeSortKey(edge.getParent(), edge.getChild())));
394 }
395 }
396
397 void AITstar::clearForwardQueue()
398 {
399 std::vector<Edge> forwardQueue;
400 forwardQueue_.getContent(forwardQueue);
401 for (const auto &element : forwardQueue)
402 {
403 element.getChild()->resetForwardQueueIncomingLookup();
404 element.getParent()->resetForwardQueueOutgoingLookup();
405 }
406 forwardQueue_.clear();
407 numInconsistentOrUnconnectedTargets_ = 0u;
408 }
409
410 void AITstar::rebuildReverseQueue()
411 {
412 // Rebuilding the reverse queue invalidates the reverse queue pointers.
413 std::vector<aitstar::KeyVertexPair> content;
414 reverseQueue_.getContent(content);
415 for (auto &element : content)
416 {
417 element.second->resetReverseQueuePointer();
418 }
419 reverseQueue_.clear();
420
421 for (auto &vertex : content)
422 {
423 // Compute the sort key for the vertex queue.
424 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> element(
425 computeSortKey(vertex.second), vertex.second);
426 auto reverseQueuePointer = reverseQueue_.insert(element);
427 element.second->setReverseQueuePointer(reverseQueuePointer);
428 }
429 }
430
431 void AITstar::clearReverseQueue()
432 {
433 std::vector<aitstar::KeyVertexPair> reverseQueue;
434 reverseQueue_.getContent(reverseQueue);
435 for (const auto &element : reverseQueue)
436 {
437 element.second->resetReverseQueuePointer();
438 }
439 reverseQueue_.clear();
440 }
441
442 void AITstar::informAboutNewSolution() const
443 {
444 OMPL_INFORM("%s (%u iterations): Found a new exact solution of cost %.4f. Sampled a total of %u states, %u "
445 "of which were valid samples (%.1f \%). Processed %u edges, %u of which were collision checked "
446 "(%.1f \%). The forward search tree has %u vertices, %u of which are start states. The reverse "
447 "search tree has %u vertices, %u of which are goal states.",
448 name_.c_str(), numIterations_, solutionCost_.value(), graph_.getNumberOfSampledStates(),
450 graph_.getNumberOfSampledStates() == 0u ?
451 0.0 :
452 100.0 * (static_cast<double>(graph_.getNumberOfValidSamples()) /
453 static_cast<double>(graph_.getNumberOfSampledStates())),
454 numProcessedEdges_, numEdgeCollisionChecks_,
455 numProcessedEdges_ == 0u ? 0.0 :
456 100.0 * (static_cast<float>(numEdgeCollisionChecks_) /
457 static_cast<float>(numProcessedEdges_)),
458 countNumVerticesInForwardTree(), graph_.getStartVertices().size(),
459 countNumVerticesInReverseTree(), graph_.getGoalVertices().size());
460 }
461
462 void AITstar::informAboutPlannerStatus(ompl::base::PlannerStatus::StatusType status) const
463 {
464 switch (status)
465 {
467 {
468 OMPL_INFORM("%s (%u iterations): Found an exact solution of cost %.4f.", name_.c_str(),
469 numIterations_, solutionCost_.value());
470 break;
471 }
473 {
474 OMPL_INFORM("%s (%u iterations): Did not find an exact solution, but found an approximate "
475 "solution "
476 "of cost %.4f which is %.4f away from a goal (in cost space).",
477 name_.c_str(), numIterations_, approximateSolutionCost_.value(),
478 approximateSolutionCostToGoal_.value());
479 break;
480 }
482 {
483 if (trackApproximateSolutions_)
484 {
485 OMPL_INFORM("%s (%u iterations): Did not find any solution.", name_.c_str(), numIterations_);
486 }
487 else
488 {
489 OMPL_INFORM("%s (%u iterations): Did not find an exact solution, and tracking approximate "
490 "solutions is disabled.",
491 name_.c_str(), numIterations_);
492 }
493 break;
494 }
503 {
504 OMPL_INFORM("%s (%u iterations): Unable to solve the given planning problem.", name_.c_str(),
505 numIterations_);
506 }
507 }
508
510 "%s (%u iterations): Sampled a total of %u states, %u of which were valid samples (%.1f \%). "
511 "Processed %u edges, %u of which were collision checked (%.1f \%). The forward search tree "
512 "has %u vertices. The reverse search tree has %u vertices.",
513 name_.c_str(), numIterations_, graph_.getNumberOfSampledStates(), graph_.getNumberOfValidSamples(),
514 graph_.getNumberOfSampledStates() == 0u ?
515 0.0 :
516 100.0 * (static_cast<double>(graph_.getNumberOfValidSamples()) /
517 static_cast<double>(graph_.getNumberOfSampledStates())),
518 numProcessedEdges_, numEdgeCollisionChecks_,
519 numProcessedEdges_ == 0u ?
520 0.0 :
521 100.0 * (static_cast<float>(numEdgeCollisionChecks_) / static_cast<float>(numProcessedEdges_)),
522 countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
523 }
524
525 void AITstar::insertGoalVerticesInReverseQueue()
526 {
527 for (const auto &goal : graph_.getGoalVertices())
528 {
529 // Set the cost to come from the goal to identity and the expanded cost to infinity.
530 goal->setExpandedCostToComeFromGoal(objective_->infiniteCost());
531 goal->setCostToComeFromGoal(objective_->identityCost());
532
533 // Create an element for the queue.
534 aitstar::KeyVertexPair element({computeCostToGoToStartHeuristic(goal), objective_->identityCost()},
535 goal);
536
537 // Insert the element into the queue and set the corresponding pointer.
538 auto reverseQueuePointer = reverseQueue_.insert(element);
539 goal->setReverseQueuePointer(reverseQueuePointer);
540 }
541 }
542
543 void AITstar::expandStartVerticesIntoForwardQueue()
544 {
545 for (const auto &start : graph_.getStartVertices())
546 {
547 start->setCostToComeFromStart(objective_->identityCost());
548 insertOrUpdateInForwardQueue(getOutgoingEdges(start));
549 }
550 }
551
552 bool AITstar::continueReverseSearch() const
553 {
554 // Never continue the reverse search if the reverse of forward queue is empty.
555 if (reverseQueue_.empty() || forwardQueue_.empty())
556 {
557 return false;
558 }
559
560 // Get references to the best edge and vertex in the queues.
561 const auto &bestEdge = forwardQueue_.top()->data;
562 const auto &bestVertex = reverseQueue_.top()->data;
563
564 // The reverse search must be continued if the best edge has an inconsistent child state or if the best
565 // vertex can potentially lead to a better solution than the best edge.
566 return !((bestEdge.getChild()->isConsistent() &&
567 objective_->isCostBetterThan(bestEdge.getSortKey()[0u], bestVertex.first[0u])) ||
568 numInconsistentOrUnconnectedTargets_ == 0u);
569 }
570
571 bool AITstar::continueForwardSearch()
572 {
573 // Never continue the forward search if its queue is empty.
574 if (forwardQueue_.empty())
575 {
576 return false;
577 }
578
579 // If the best edge in the forward queue has a potential total solution cost of infinity, the forward
580 // search does not need to be continued. This can happen if the reverse search did not reach any target
581 // state of the edges in the forward queue.
582 const auto &bestEdgeCost = forwardQueue_.top()->data.getSortKey()[0u];
583 if (!objective_->isFinite(bestEdgeCost))
584 {
585 return false;
586 }
587
588 // The forward search can be stopped once the resolution optimal solution has been found.
589 return objective_->isCostBetterThan(bestEdgeCost, solutionCost_);
590 }
591
592 std::vector<Edge> AITstar::getEdgesInQueue() const
593 {
594 std::vector<Edge> edges;
595 forwardQueue_.getContent(edges);
596 return edges;
597 }
598
599 std::vector<std::shared_ptr<Vertex>> AITstar::getVerticesInQueue() const
600 {
601 // Get the content from the queue.
602 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>> content;
603 reverseQueue_.getContent(content);
604
605 // Return the vertices.
606 std::vector<std::shared_ptr<Vertex>> vertices;
607 for (const auto &pair : content)
608 {
609 vertices.emplace_back(pair.second);
610 }
611 return vertices;
612 }
613
615 {
616 if (!forwardQueue_.empty())
617 {
618 return forwardQueue_.top()->data;
619 }
620
621 return {};
622 }
623
624 std::shared_ptr<Vertex> AITstar::getNextVertexInQueue() const
625 {
626 if (!reverseQueue_.empty())
627 {
628 return reverseQueue_.top()->data.second;
629 }
630
631 return {};
632 }
633
634 std::vector<std::shared_ptr<Vertex>> AITstar::getVerticesInReverseSearchTree() const
635 {
636 // Get all vertices from the graph.
637 auto vertices = graph_.getVertices();
638
639 // Erase the vertices that are not in the reverse search tree.
640 vertices.erase(std::remove_if(vertices.begin(), vertices.end(),
641 [this](const std::shared_ptr<Vertex> &vertex) {
642 return !graph_.isGoal(vertex) && !vertex->hasReverseParent();
643 }),
644 vertices.end());
645 return vertices;
646 }
647
648 void AITstar::iterate(const ompl::base::PlannerTerminationCondition &terminationCondition)
649 {
650 // If this is the first time solve is called, populate the queues.
651 if (numIterations_ == 0u)
652 {
653 insertGoalVerticesInReverseQueue();
654 expandStartVerticesIntoForwardQueue();
655 }
656
657 // Keep track of the number of iterations.
658 ++numIterations_;
659
660 // If the reverse search needs to be continued, do that now.
661 if (continueReverseSearch())
662 {
663 iterateReverseSearch();
664 } // If the reverse search is suspended, check whether the forward search needs to be continued.
665 else if (continueForwardSearch())
666 {
667 iterateForwardSearch();
668 } // If neither the forward search nor the reverse search needs to be continued, add more samples.
669 else
670 {
671 // Add new samples to the graph, respecting the termination condition.
672 if (graph_.addSamples(batchSize_, terminationCondition))
673 {
674 // Remove useless samples from the graph.
675 if (isPruningEnabled_)
676 {
677 graph_.prune();
678 }
679
680 // Clear the reverse search tree.
681 for (auto &goal : graph_.getGoalVertices())
682 {
683 invalidateCostToComeFromGoalOfReverseBranch(goal);
684 }
685
686 // Add new start and goal states if necessary.
688 {
690 }
691
692 // Reinitialize the queues.
693 clearReverseQueue();
694 clearForwardQueue();
695 insertGoalVerticesInReverseQueue();
696 expandStartVerticesIntoForwardQueue();
697 }
698 }
699 }
700
701 void AITstar::iterateForwardSearch()
702 {
703 assert(!forwardQueue_.empty());
704
705 // Get the most promising edge.
706 auto parent = forwardQueue_.top()->data.getParent();
707 auto child = forwardQueue_.top()->data.getChild();
708 child->removeFromForwardQueueIncomingLookup(forwardQueue_.top());
709 parent->removeFromForwardQueueOutgoingLookup(forwardQueue_.top());
710 forwardQueue_.pop();
711
712 // Ensure that the child is consistent and the parent isn't the goal.
713 assert(child->isConsistent());
714 assert(!graph_.isGoal(parent));
715
716 // This counts as processing an edge.
717 ++numProcessedEdges_;
718
719 // If this edge is already in the forward tree, it's a freeby.
720 if (child->hasForwardParent() && child->getForwardParent()->getId() == parent->getId())
721 {
722 insertOrUpdateInForwardQueue(getOutgoingEdges(child));
723 return;
724 } // Check if this edge can possibly improve the current search tree.
725 else if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(),
726 objective_->motionCostHeuristic(
727 parent->getState(), child->getState())),
728 child->getCostToComeFromStart()))
729 {
730 // The edge can possibly improve the solution and the path to the child. Let's check it for
731 // collision.
732 if (parent->isWhitelistedAsChild(child) ||
733 motionValidator_->checkMotion(parent->getState(), child->getState()))
734 {
735 // Remember that this is a good edge.
736 if (!parent->isWhitelistedAsChild(child))
737 {
738 parent->whitelistAsChild(child);
739 numEdgeCollisionChecks_++;
740 }
741
742 // Compute the edge cost.
743 const auto edgeCost = objective_->motionCost(parent->getState(), child->getState());
744
745 // Check if the edge can improve the cost to come to the child.
746 if (objective_->isCostBetterThan(
747 objective_->combineCosts(parent->getCostToComeFromStart(), edgeCost),
748 child->getCostToComeFromStart()))
749 {
750 // Rewire the child.
751 child->setForwardParent(parent, edgeCost);
752
753 // Add it to the children of the parent.
754 parent->addToForwardChildren(child);
755
756 // Share the good news with the whole branch.
757 child->updateCostOfForwardBranch();
758
759 // Check if the solution can benefit from this.
760 updateSolution(child);
761
762 // Insert the child's outgoing edges into the queue.
763 insertOrUpdateInForwardQueue(getOutgoingEdges(child));
764 }
765 }
766 else // This edge is in collision
767 {
768 // The edge should be blacklisted in both directions.
769 parent->blacklistAsChild(child);
770 child->blacklistAsChild(parent);
771
772 // Repair the reverse search if this edge was in the reverse search tree.
773 if (parent->hasReverseParent() && parent->getReverseParent()->getId() == child->getId())
774 {
775 // The parent was connected to the child through an invalid edge, so we need to invalidate
776 // the branch of the reverse search tree starting from the parent.
777 invalidateCostToComeFromGoalOfReverseBranch(parent);
778 }
779 }
780 }
781 }
782
783 void AITstar::iterateReverseSearch()
784 {
785 assert(!reverseQueue_.empty());
786
787 // Get the most promising vertex and remove it from the queue.
788 auto vertex = reverseQueue_.top()->data.second;
789 reverseQueue_.pop();
790 vertex->resetReverseQueuePointer();
791
792 // The open queue should not contain consistent vertices.
793 assert(!vertex->isConsistent());
794
795 // Check if the vertex is underconsistent. g[s] < v[s].
796 if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
797 {
798 // Make the vertex consistent and update the vertex.
799 vertex->setExpandedCostToComeFromGoal(vertex->getCostToComeFromGoal());
800 updateReverseSearchNeighbors(vertex);
801
802 // Update the number of inconsistent targets in the forward queue.
803 numInconsistentOrUnconnectedTargets_ -= vertex->getForwardQueueIncomingLookup().size();
804 }
805 else
806 {
807 // Make the vertex overconsistent.
808 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
809
810 // Update the vertex and its neighbors.
811 updateReverseSearchVertex(vertex);
812 updateReverseSearchNeighbors(vertex);
813 }
814 }
815
816 bool AITstar::isEdgeBetter(const Edge &lhs, const Edge &rhs) const
817 {
818 return std::lexicographical_compare(
819 lhs.getSortKey().cbegin(), lhs.getSortKey().cend(), rhs.getSortKey().cbegin(), rhs.getSortKey().cend(),
820 [this](const auto &a, const auto &b) { return objective_->isCostBetterThan(a, b); });
821 }
822
823 bool AITstar::isVertexBetter(const aitstar::KeyVertexPair &lhs, const aitstar::KeyVertexPair &rhs) const
824 {
825 // If the costs of two vertices are equal then we prioritize inconsistent vertices that are targets of
826 // edges in the forward queue.
827 if (objective_->isCostEquivalentTo(lhs.first[0u], rhs.first[0u]) &&
828 objective_->isCostEquivalentTo(lhs.first[1u], rhs.first[1u]))
829 {
830 return !lhs.second->getForwardQueueIncomingLookup().empty() && !lhs.second->isConsistent();
831 }
832 else
833 {
834 // Otherwise it's a regular lexicographical comparison of the keys.
835 return std::lexicographical_compare(
836 lhs.first.cbegin(), lhs.first.cend(), rhs.first.cbegin(), rhs.first.cend(),
837 [this](const auto &a, const auto &b) { return objective_->isCostBetterThan(a, b); });
838 }
839 }
840
841 void AITstar::updateReverseSearchVertex(const std::shared_ptr<Vertex> &vertex)
842 {
843 // If the vertex is a goal, there's no updating to do.
844 if (graph_.isGoal(vertex))
845 {
846 assert(objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(), objective_->identityCost()));
847 return;
848 }
849
850 // Get the best parent for this vertex.
851 auto bestParent = vertex->getReverseParent();
852 auto bestCost = vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
853
854 // Check all neighbors as defined by the RGG.
855 for (const auto &neighbor : graph_.getNeighbors(vertex))
856 {
857 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
858 !vertex->isBlacklistedAsChild(neighbor))
859 {
860 auto edgeCost = objective_->motionCostHeuristic(neighbor->getState(), vertex->getState());
861 auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
862 if (objective_->isCostBetterThan(parentCost, bestCost))
863 {
864 bestParent = neighbor;
865 bestCost = parentCost;
866 }
867 }
868 }
869
870 // Check all children this vertex holds in the forward search.
871 for (const auto &forwardChild : vertex->getForwardChildren())
872 {
873 auto edgeCost = objective_->motionCostHeuristic(forwardChild->getState(), vertex->getState());
874 auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
875
876 if (objective_->isCostBetterThan(parentCost, bestCost))
877 {
878 bestParent = forwardChild;
879 bestCost = parentCost;
880 }
881 }
882
883 // Check the parent of this vertex in the forward search.
884 if (vertex->hasForwardParent())
885 {
886 auto forwardParent = vertex->getForwardParent();
887 auto edgeCost = objective_->motionCostHeuristic(forwardParent->getState(), vertex->getState());
888 auto parentCost = objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
889
890 if (objective_->isCostBetterThan(parentCost, bestCost))
891 {
892 bestParent = forwardParent;
893 bestCost = parentCost;
894 }
895 }
896
897 // Set the best cost as the cost-to-come from the goal.
898 vertex->setCostToComeFromGoal(bestCost);
899
900 // What happens next depends on whether the vertex is disconnected or not.
901 if (objective_->isFinite(bestCost))
902 {
903 // The vertex is connected. Update the reverse parent.
904 vertex->setReverseParent(bestParent);
905 bestParent->addToReverseChildren(vertex);
906 }
907 else
908 {
909 // This vertex is now orphaned. Reset the reverse parent if the vertex had one.
910 if (vertex->hasReverseParent())
911 {
912 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
913 vertex->resetReverseParent();
914 }
915 }
916
917 // If this has made the vertex inconsistent, insert or update it in the open queue.
918 if (!vertex->isConsistent())
919 {
920 insertOrUpdateInReverseQueue(vertex);
921 }
922 else // Remove this vertex from the queue if it is in the queue if it is consistent.
923 {
924 auto reverseQueuePointer = vertex->getReverseQueuePointer();
925 if (reverseQueuePointer)
926 {
927 reverseQueue_.remove(reverseQueuePointer);
928 vertex->resetReverseQueuePointer();
929 }
930 }
931
932 // This vertex now has a changed cost-to-come in the reverse search. All edges in the forward queue that
933 // have this vertex as a target must be updated. This cannot be delayed, as whether the reverse search
934 // can be suspended depends on the best edge in the forward queue.
935 for (const auto &element : vertex->getForwardQueueIncomingLookup())
936 {
937 auto &edge = element->data;
938 edge.setSortKey(computeSortKey(edge.getParent(), edge.getChild()));
939 forwardQueue_.update(element);
940 }
941 }
942
943 void AITstar::updateReverseSearchNeighbors(const std::shared_ptr<Vertex> &vertex)
944 {
945 // Start with the reverse search children, because if this vertex becomes the parent of a neighbor, that
946 // neighbor would be updated again as part of the reverse children.
947 for (const auto &child : vertex->getReverseChildren())
948 {
949 updateReverseSearchVertex(child);
950 }
951
952 // We can now process the neighbors.
953 for (const auto &neighbor : graph_.getNeighbors(vertex))
954 {
955 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
956 !vertex->isBlacklistedAsChild(neighbor))
957 {
958 updateReverseSearchVertex(neighbor);
959 }
960 }
961
962 // We also need to update the forward search children.
963 for (const auto &child : vertex->getForwardChildren())
964 {
965 updateReverseSearchVertex(child);
966 }
967
968 // We also need to update the forward search parent if it exists.
969 if (vertex->hasForwardParent())
970 {
971 updateReverseSearchVertex(vertex->getForwardParent());
972 }
973 }
974
975 void AITstar::insertOrUpdateInReverseQueue(const std::shared_ptr<Vertex> &vertex)
976 {
977 // Get the pointer to the element in the queue.
978 auto element = vertex->getReverseQueuePointer();
979
980 // Update it if it is in the queue.
981 if (element)
982 {
983 element->data.first = computeSortKey(vertex);
984 reverseQueue_.update(element);
985 }
986 else // Insert it into the queue otherwise.
987 {
988 // Compute the sort key for the vertex queue.
989 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> element(computeSortKey(vertex),
990 vertex);
991
992 // Insert the vertex into the queue, storing the corresponding pointer.
993 auto reverseQueuePointer = reverseQueue_.insert(element);
994 vertex->setReverseQueuePointer(reverseQueuePointer);
995 }
996 }
997
998 void AITstar::insertOrUpdateInForwardQueue(const Edge &edge)
999 {
1000 // Check if the edge is already in the queue and can be updated.
1001 const auto lookup = edge.getChild()->getForwardQueueIncomingLookup();
1002 const auto it = std::find_if(lookup.begin(), lookup.end(), [&edge](const auto element) {
1003 return element->data.getParent()->getId() == edge.getParent()->getId();
1004 });
1005
1006 if (it != lookup.end())
1007 {
1008 // We used the incoming lookup of the child. Assert that it is already in the outgoing lookup of the
1009 // parent.
1010 assert(std::find_if(edge.getParent()->getForwardQueueOutgoingLookup().begin(),
1012 [&edge](const auto element) {
1013 return element->data.getChild()->getId() == edge.getChild()->getId();
1014 }) != edge.getParent()->getForwardQueueOutgoingLookup().end());
1015
1016 // This edge exists in the queue. If the new sort key is better than the old, we update it.
1017 if (isEdgeBetter(edge, (*it)->data))
1018 {
1019 (*it)->data.setSortKey(edge.getSortKey());
1020 forwardQueue_.update(*it);
1021 }
1022 }
1023 else
1024 {
1025 auto element = forwardQueue_.insert(edge);
1028
1029 // Incement the counter if the target is inconsistent.
1030 if (!edge.getChild()->isConsistent() || !objective_->isFinite(edge.getChild()->getCostToComeFromGoal()))
1031 {
1032 ++numInconsistentOrUnconnectedTargets_;
1033 }
1034 }
1035 }
1036
1037 void AITstar::insertOrUpdateInForwardQueue(const std::vector<Edge> &edges)
1038 {
1039 for (const auto &edge : edges)
1040 {
1041 insertOrUpdateInForwardQueue(edge);
1042 }
1043 }
1044
1045 std::shared_ptr<ompl::geometric::PathGeometric>
1046 AITstar::getPathToVertex(const std::shared_ptr<Vertex> &vertex) const
1047 {
1048 // Create the reverse path by following the parents to the start.
1049 std::vector<std::shared_ptr<Vertex>> reversePath;
1050 auto current = vertex;
1051 while (!graph_.isStart(current))
1052 {
1053 reversePath.emplace_back(current);
1054 current = current->getForwardParent();
1055 }
1056 reversePath.emplace_back(current);
1057
1058 // Reverse the reverse path to get the forward path.
1059 auto path = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
1060 for (const auto &vertex : boost::adaptors::reverse(reversePath))
1061 {
1062 path->append(vertex->getState());
1063 }
1064
1065 return path;
1066 }
1067
1068 std::array<ompl::base::Cost, 3u> AITstar::computeSortKey(const std::shared_ptr<Vertex> &parent,
1069 const std::shared_ptr<Vertex> &child) const
1070 {
1071 // Compute the sort key [g_T(start) + c_hat(start, neighbor) + h_hat(neighbor), g_T(start) +
1072 // c_hat(start, neighbor), g_T(start)].
1073 ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1074 return {
1075 objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1076 child->getCostToGoToGoal()),
1077 objective_->combineCosts(edgeCostHeuristic, child->getCostToGoToGoal()),
1078 parent->getCostToComeFromStart()};
1079 }
1080
1081 std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(const std::shared_ptr<Vertex> &vertex) const
1082 {
1083 // LPA* sort key is [min(g(x), v(x)) + h(x); min(g(x), v(x))].
1084 return {objective_->combineCosts(objective_->betterCost(vertex->getCostToComeFromGoal(),
1085 vertex->getExpandedCostToComeFromGoal()),
1086 computeCostToGoToStartHeuristic(vertex)),
1087 objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1088 }
1089
1090 std::vector<Edge> AITstar::getOutgoingEdges(const std::shared_ptr<Vertex> &vertex) const
1091 {
1092 // Prepare the return variable.
1093 std::vector<Edge> outgoingEdges;
1094
1095 // Insert the edges to the current children.
1096 for (const auto &child : vertex->getForwardChildren())
1097 {
1098 outgoingEdges.emplace_back(vertex, child, computeSortKey(vertex, child));
1099 }
1100
1101 // Insert the edges to the current neighbors.
1102 for (const auto &neighbor : graph_.getNeighbors(vertex))
1103 {
1104 // We do not want self loops.
1105 if (vertex->getId() == neighbor->getId())
1106 {
1107 continue;
1108 }
1109
1110 // If the neighbor is the reverse parent, it will explicitly be added later.
1111 if (vertex->hasReverseParent() && neighbor->getId() == vertex->getReverseParent()->getId())
1112 {
1113 continue;
1114 }
1115
1116 // We do not want blacklisted edges.
1117 if (neighbor->isBlacklistedAsChild(vertex) || vertex->isBlacklistedAsChild(neighbor))
1118 {
1119 continue;
1120 }
1121
1122 // If none of the above tests caught on, we can insert the edge.
1123 outgoingEdges.emplace_back(vertex, neighbor, computeSortKey(vertex, neighbor));
1124 }
1125
1126 // Insert the edge to the reverse search parent.
1127 if (vertex->hasReverseParent())
1128 {
1129 const auto &reverseParent = vertex->getReverseParent();
1130 outgoingEdges.emplace_back(vertex, reverseParent, computeSortKey(vertex, reverseParent));
1131 }
1132
1133 return outgoingEdges;
1134 }
1135
1136 void AITstar::updateExactSolution()
1137 {
1138 // Check if any of the goals have a cost to come less than the current solution cost.
1139 for (const auto &goal : graph_.getGoalVertices())
1140 {
1141 // We need to check whether the cost is better, or whether someone has removed the exact solution
1142 // from the problem definition.
1143 if (objective_->isCostBetterThan(goal->getCostToComeFromStart(), solutionCost_) ||
1144 (!pdef_->hasExactSolution() && objective_->isFinite(goal->getCostToComeFromStart())))
1145 {
1146 // Remember the incumbent cost.
1147 solutionCost_ = goal->getCostToComeFromStart();
1148
1149 // Create a solution.
1150 ompl::base::PlannerSolution solution(getPathToVertex(goal));
1151 solution.setPlannerName(name_);
1152
1153 // Set the optimized flag.
1154 solution.setOptimized(objective_, solutionCost_, objective_->isSatisfied(solutionCost_));
1155
1156 // Let the problem definition know that a new solution exists.
1157 pdef_->addSolutionPath(solution);
1158
1159 // Let the user know about the new solution.
1160 informAboutNewSolution();
1161 }
1162 }
1163 }
1164
1165 void AITstar::updateApproximateSolution()
1166 {
1167 for (auto &start : graph_.getStartVertices())
1168 {
1169 start->callOnForwardBranch([this](const auto &vertex) -> void { updateApproximateSolution(vertex); });
1170 }
1171 }
1172
1173 void AITstar::updateApproximateSolution(const std::shared_ptr<Vertex> &vertex)
1174 {
1175 assert(trackApproximateSolutions_);
1176 if (vertex->hasForwardParent() || graph_.isStart(vertex))
1177 {
1178 auto costToGoal = computeCostToGoToGoal(vertex);
1179
1180 // We need to check whether this is better than the current approximate solution or whether someone
1181 // has removed all approximate solutions from the problem definition.
1182 if (objective_->isCostBetterThan(costToGoal, approximateSolutionCostToGoal_) ||
1183 !pdef_->hasApproximateSolution())
1184 {
1185 // Remember the incumbent approximate cost.
1186 approximateSolutionCost_ = vertex->getCostToComeFromStart();
1187 approximateSolutionCostToGoal_ = costToGoal;
1188 ompl::base::PlannerSolution solution(getPathToVertex(vertex));
1189 solution.setPlannerName(name_);
1190
1191 // Set the approximate flag.
1192 solution.setApproximate(costToGoal.value());
1193
1194 // This solution is approximate and can not satisfy the objective.
1195 solution.setOptimized(objective_, approximateSolutionCost_, false);
1196
1197 // Let the problem definition know that a new solution exists.
1198 pdef_->addSolutionPath(solution);
1199 }
1200 }
1201 };
1202
1203 ompl::base::PlannerStatus::StatusType AITstar::updateSolution()
1204 {
1205 updateExactSolution();
1206 if (objective_->isFinite(solutionCost_))
1207 {
1209 }
1210 else if (trackApproximateSolutions_)
1211 {
1212 updateApproximateSolution();
1214 }
1215 else
1216 {
1218 }
1219 }
1220
1221 ompl::base::PlannerStatus::StatusType AITstar::updateSolution(const std::shared_ptr<Vertex> &vertex)
1222 {
1223 updateExactSolution();
1224 if (objective_->isFinite(solutionCost_))
1225 {
1227 }
1228 else if (trackApproximateSolutions_)
1229 {
1230 updateApproximateSolution(vertex);
1232 }
1233 else
1234 {
1236 }
1237 }
1238
1239 ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(const std::shared_ptr<Vertex> &vertex) const
1240 {
1241 // We need to loop over all start vertices and see which is the closest one.
1242 ompl::base::Cost bestCost = objective_->infiniteCost();
1243 for (const auto &start : graph_.getStartVertices())
1244 {
1245 bestCost = objective_->betterCost(
1246 bestCost, objective_->motionCostHeuristic(vertex->getState(), start->getState()));
1247 }
1248 return bestCost;
1249 }
1250
1251 ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(const std::shared_ptr<Vertex> &vertex) const
1252 {
1253 // We need to loop over all goal vertices and see which is the closest one.
1254 ompl::base::Cost bestCost = objective_->infiniteCost();
1255 for (const auto &goal : graph_.getGoalVertices())
1256 {
1257 bestCost = objective_->betterCost(
1258 bestCost, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
1259 }
1260 return bestCost;
1261 }
1262
1263 ompl::base::Cost AITstar::computeCostToGoToGoal(const std::shared_ptr<Vertex> &vertex) const
1264 {
1265 // We need to loop over all goal vertices and see which is the closest one.
1266 ompl::base::Cost bestCost = objective_->infiniteCost();
1267 for (const auto &goal : graph_.getGoalVertices())
1268 {
1269 bestCost =
1270 objective_->betterCost(bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1271 }
1272 return bestCost;
1273 }
1274
1275 ompl::base::Cost AITstar::computeBestCostToComeFromGoalOfAnyStart() const
1276 {
1277 // We need to loop over all start vertices and see which is the closest one.
1278 ompl::base::Cost bestCost = objective_->infiniteCost();
1279 for (const auto &start : graph_.getStartVertices())
1280 {
1281 bestCost = objective_->betterCost(bestCost, start->getCostToComeFromGoal());
1282 }
1283 return bestCost;
1284 }
1285
1286 std::size_t AITstar::countNumVerticesInForwardTree() const
1287 {
1288 std::size_t numVerticesInForwardTree = 0u;
1289 auto vertices = graph_.getVertices();
1290 for (const auto &vertex : vertices)
1291 {
1292 if (graph_.isStart(vertex) || vertex->hasForwardParent())
1293 {
1294 ++numVerticesInForwardTree;
1295 }
1296 }
1297 return numVerticesInForwardTree;
1298 }
1299
1300 std::size_t AITstar::countNumVerticesInReverseTree() const
1301 {
1302 std::size_t numVerticesInReverseTree = 0u;
1303 auto vertices = graph_.getVertices();
1304 for (const auto &vertex : vertices)
1305 {
1306 if (graph_.isGoal(vertex) || vertex->hasReverseParent())
1307 {
1308 ++numVerticesInReverseTree;
1309 }
1310 }
1311 return numVerticesInReverseTree;
1312 }
1313
1314 void AITstar::invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<Vertex> &vertex)
1315 {
1316 // If this vertex is consistent before invalidation, then all incoming edges now have targets that are
1317 // inconsistent.
1318 if (vertex->isConsistent())
1319 {
1320 numInconsistentOrUnconnectedTargets_ += vertex->getForwardQueueIncomingLookup().size();
1321 }
1322
1323 // Reset the cost to come from the goal and the reverse parent unless the vertex is itself a goal.
1324 if (!graph_.isGoal(vertex))
1325 {
1326 // Reset the cost to come from the goal.
1327 vertex->resetCostToComeFromGoal();
1328
1329 // Reset the reverse parent.
1330 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
1331 vertex->resetReverseParent();
1332 }
1333
1334 // Reset the expanded cost to come from goal.
1335 vertex->resetExpandedCostToComeFromGoal();
1336
1337 // Update all affected edges in the forward queue.
1338 for (const auto &edge : vertex->getForwardQueueIncomingLookup())
1339 {
1340 edge->data.setSortKey(computeSortKey(edge->data.getParent(), edge->data.getChild()));
1341 forwardQueue_.update(edge);
1342 }
1343
1344 // Remove this vertex from the reverse search queue if it is in it.
1345 auto reverseQueuePointer = vertex->getReverseQueuePointer();
1346 if (reverseQueuePointer)
1347 {
1348 reverseQueue_.remove(reverseQueuePointer);
1349 vertex->resetReverseQueuePointer();
1350 }
1351
1352 // Update the cost of all reverse children.
1353 for (const auto &child : vertex->getReverseChildren())
1354 {
1355 invalidateCostToComeFromGoalOfReverseBranch(child);
1356 }
1357
1358 // Update the reverse search vertex to ensure that this vertex is placed in open if necessary.
1359 updateReverseSearchVertex(vertex);
1360 }
1361
1362 } // namespace geometric
1363} // namespace ompl
Element * insert(const _T &data)
Add a new element.
Definition BinaryHeap.h:140
void pop()
Remove the top element.
Definition BinaryHeap.h:126
bool empty() const
Check if the heap is empty.
Definition BinaryHeap.h:195
void remove(Element *element)
Remove a specific element.
Definition BinaryHeap.h:132
void update(Element *element)
Update an element in the heap.
Definition BinaryHeap.h:186
void getContent(std::vector< _T > &content) const
Get the data stored in this heap.
Definition BinaryHeap.h:207
Element * top() const
Return the top element. nullptr for an empty heap.
Definition BinaryHeap.h:120
void clear()
Clear the heap.
Definition BinaryHeap.h:112
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition Planner.cpp:339
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition Planner.cpp:346
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states
Definition Planner.h:416
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
std::string name_
The name of this planner.
Definition Planner.h:419
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
bool setup_
Flag indicating whether setup() has been called.
Definition Planner.h:433
A shared pointer wrapper for ompl::base::SpaceInformation.
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition AITstar.cpp:624
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition AITstar.cpp:341
void setMaxNumberOfGoals(unsigned int numberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
Definition AITstar.cpp:361
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition AITstar.cpp:318
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition AITstar.cpp:346
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition AITstar.cpp:313
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition AITstar.cpp:303
ompl::base::PlannerStatus::StatusType ensureStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition)
Checks whether the problem is successfully setup.
Definition AITstar.cpp:169
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition AITstar.cpp:254
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
Definition AITstar.cpp:366
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition AITstar.cpp:599
AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
Constructs a AIT*.
Definition AITstar.cpp:55
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition AITstar.cpp:356
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
Definition AITstar.cpp:614
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition AITstar.cpp:259
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition AITstar.cpp:94
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition AITstar.cpp:215
ompl::base::PlannerStatus::StatusType ensureSetup()
Checks whether the planner is successfully setup.
Definition AITstar.cpp:145
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition AITstar.cpp:634
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition AITstar.cpp:323
void setUseKNearest(bool useKNearest)
Set whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition AITstar.cpp:351
void clear() override
Clears the algorithm's internal state.
Definition AITstar.cpp:201
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
Definition AITstar.cpp:592
std::size_t getBatchSize() const
Get the batch size.
Definition AITstar.cpp:308
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition AITstar.cpp:336
std::shared_ptr< Vertex > getChild() const
Returns the child in this edge.
Definition Edge.cpp:65
const std::array< ompl::base::Cost, 3u > & getSortKey() const
Returns the sort key associated with this edge.
Definition Edge.cpp:70
void setSortKey(const std::array< ompl::base::Cost, 3u > &key)
Sets the sort key associated with this edge.
Definition Edge.cpp:75
std::shared_ptr< Vertex > getParent() const
Returns the parent in this edge.
Definition Edge.cpp:60
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
bool hasAStartState() const
Returns whether the graph has a goal state.
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
void setMaxNumberOfGoals(unsigned int maxNumberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
double getRewireFactor() const
Get the reqire factor of the RGG.
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
void prune()
Prune all samples that can not contribute to a solution better than the current one.
void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition, ompl::base::PlannerInputStates *inputStates)
Adds new start and goals to the graph if avavilable and creates a new informed sampler if necessary.
void clear()
Resets the graph to its construction state, without resetting options.
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
bool addSamples(std::size_t numNewSamples, const ompl::base::PlannerTerminationCondition &terminationCondition)
Adds a batch of samples and returns the samples it has added.
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, ompl::base::PlannerInputStates *inputStates)
The setup method for the graph. Needed to have it on the stack.
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
bool hasAGoalState() const
Returns whether the graph has a goal state.
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
std::vector< EdgeQueue::Element * > getForwardQueueIncomingLookup() const
Returns the forward queue incoming lookup of this vertex.
Definition Vertex.cpp:511
void addToForwardQueueOutgoingLookup(typename EdgeQueue::Element *pointer)
Adds an element to the forward queue outgoing lookup.
Definition Vertex.cpp:502
std::vector< EdgeQueue::Element * > getForwardQueueOutgoingLookup() const
Returns the forward queue outgoing lookup of this vertex.
Definition Vertex.cpp:518
bool isConsistent() const
Returns whether the vertex is consistent, i.e., whether its cost-to-come is equal to the cost-to-come...
Definition Vertex.cpp:459
ompl::base::Cost getCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal.
Definition Vertex.cpp:134
void addToForwardQueueIncomingLookup(typename EdgeQueue::Element *pointer)
Adds an element to the forward queue incoming lookup.
Definition Vertex.cpp:495
void removeFromForwardQueueIncomingLookup(typename EdgeQueue::Element *element)
Remove an element from the incoming queue lookup.
Definition Vertex.cpp:523
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
Main namespace. Contains everything in this library.
Representation of a solution to a planning problem.
A class to store the exit status of Planner::solve()
StatusType
The possible values of the status returned by a planner.
@ ABORT
The planner did not find a solution for some other reason.
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
@ TIMEOUT
The planner failed to find a solution.
@ UNKNOWN
Uninitialized status.
@ TYPE_COUNT
The number of possible status values.
@ INFEASIBLE
The planner decided that problem is infeasible.
@ CRASH
The planner crashed.