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 {
55 AITstar::AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
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(),
449 graph_.getNumberOfValidSamples(),
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.
687 if (pis_.haveMoreStartStates() || pis_.haveMoreGoalStates())
688 {
689 graph_.updateStartAndGoalStates(ompl::base::plannerAlwaysTerminatingCondition(), &pis_);
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
void getContent(std::vector< _T > &content) const
Get the data stored in this heap.
Definition BinaryHeap.h:207
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
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....
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
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
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
#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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
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.