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 name of the University of Toronto nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35// Authors: Marlin Strub
36
37#include <algorithm>
38#include <cmath>
39#include <string>
40
41#include <boost/range/adaptor/reversed.hpp>
42
43#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
44#include "ompl/geometric/planners/informedtrees/AITstar.h"
45#include "ompl/util/Console.h"
46
47using namespace std::string_literals;
48
49namespace ompl
50{
51 namespace geometric
52 {
54 : ompl::base::Planner(spaceInformation, "AITstar")
55 , solutionCost_()
56 , graph_(solutionCost_)
57 , forwardQueue_([this](const aitstar::Edge &lhs, const aitstar::Edge &rhs) {
58 return std::lexicographical_compare(lhs.getSortKey().cbegin(), lhs.getSortKey().cend(),
59 rhs.getSortKey().cbegin(), rhs.getSortKey().cend(),
60 [this](const ompl::base::Cost &a, const ompl::base::Cost &b) {
61 return objective_->isCostBetterThan(a, b);
62 });
63 })
64 , reverseQueue_(
65 [this](const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> &lhs,
66 const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> &rhs) {
67 return std::lexicographical_compare(lhs.first.cbegin(), lhs.first.cend(), rhs.first.cbegin(),
68 rhs.first.cend(),
69 [this](const ompl::base::Cost &a, const ompl::base::Cost &b) {
70 return objective_->isCostBetterThan(a, b);
71 });
72 })
73 {
74 // Specify AIT*'s planner specs.
75 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
76 specs_.multithreaded = false;
77 specs_.approximateSolutions = true;
78 specs_.optimizingPaths = true;
79 specs_.directed = true;
80 specs_.provingSolutionNonExistence = false;
81 specs_.canReportIntermediateSolutions = true;
82
83 // Register the setting callbacks.
84 declareParam<bool>("use_k_nearest", this, &AITstar::setUseKNearest, &AITstar::getUseKNearest, "0,1");
85 declareParam<double>("rewire_factor", this, &AITstar::setRewireFactor, &AITstar::getRewireFactor,
86 "1.0:0.01:3.0");
87 declareParam<std::size_t>("samples_per_batch", this, &AITstar::setBatchSize, &AITstar::getBatchSize,
88 "1:1:1000");
89 declareParam<bool>("use_graph_pruning", this, &AITstar::enablePruning, &AITstar::isPruningEnabled, "0,1");
90 declareParam<bool>("find_approximate_solutions", this, &AITstar::trackApproximateSolutions,
92
93 // Register the progress properties.
94 addPlannerProgressProperty("iterations INTEGER", [this]() { return std::to_string(numIterations_); });
95 addPlannerProgressProperty("best cost DOUBLE", [this]() { return std::to_string(solutionCost_.value()); });
96 addPlannerProgressProperty("state collision checks INTEGER",
97 [this]() { return std::to_string(graph_.getNumberOfStateCollisionChecks()); });
98 addPlannerProgressProperty("edge collision checks INTEGER",
99 [this]() { return std::to_string(numEdgeCollisionChecks_); });
100 addPlannerProgressProperty("nearest neighbour calls INTEGER",
101 [this]() { return std::to_string(graph_.getNumberOfNearestNeighborCalls()); });
102 }
103
105 {
106 // Call the base-class setup.
107 Planner::setup();
108
109 // Check that a problem definition has been set.
110 if (static_cast<bool>(Planner::pdef_))
111 {
112 // Default to path length optimization objective if none has been specified.
113 if (!pdef_->hasOptimizationObjective())
114 {
115 OMPL_WARN("%s: No optimization objective has been specified. Defaulting to path length.",
116 Planner::getName().c_str());
117 Planner::pdef_->setOptimizationObjective(
118 std::make_shared<ompl::base::PathLengthOptimizationObjective>(Planner::si_));
119 }
120
121 if (static_cast<bool>(pdef_->getGoal()))
122 {
123 // If we were given a goal, make sure its of appropriate type.
124 if (!(pdef_->getGoal()->hasType(ompl::base::GOAL_SAMPLEABLE_REGION)))
125 {
126 OMPL_ERROR("AIT* is currently only implemented for goals that can be cast to "
127 "ompl::base::GOAL_SAMPLEABLE_GOAL_REGION.");
128 setup_ = false;
129 return;
130 }
131 }
132
133 // Pull the optimization objective through the problem definition.
134 objective_ = pdef_->getOptimizationObjective();
135
136 // Initialize the solution cost to be infinite.
137 solutionCost_ = objective_->infiniteCost();
138 approximateSolutionCost_ = objective_->infiniteCost();
139 approximateSolutionCostToGoal_ = objective_->infiniteCost();
140
141 // Pull the motion validator through the space information.
142 motionValidator_ = si_->getMotionValidator();
143
144 // Setup a graph.
145 graph_.setup(si_, pdef_, &pis_);
146 }
147 else
148 {
149 // AIT* can't be setup without a problem definition.
150 setup_ = false;
151 OMPL_WARN("AIT*: Unable to setup without a problem definition.");
152 }
153 }
154
156 {
157 graph_.clear();
158 forwardQueue_.clear();
159 reverseQueue_.clear();
160 solutionCost_ = objective_->infiniteCost();
161 approximateSolutionCost_ = objective_->infiniteCost();
162 approximateSolutionCostToGoal_ = objective_->infiniteCost();
163 edgesToBeInserted_.clear();
164 numIterations_ = 0u;
165 performReverseSearchIteration_ = true;
166 isForwardSearchStartedOnBatch_ = false;
167 forwardQueueMustBeRebuilt_ = false;
168 Planner::clear();
169 }
170
172 {
173 // The planner status to return.
175
176 // Ensure the planner is setup.
177 Planner::checkValidity();
178 if (!Planner::setup_)
179 {
180 OMPL_WARN("%s: Failed to setup and thus solve can not do anything meaningful.", name_.c_str());
182 informAboutPlannerStatus(status);
183 return status;
184 }
185
186 // If the graph currently does not have a goal state, we wait until we get one.
187 if (!graph_.hasAGoalState())
188 {
189 graph_.updateStartAndGoalStates(terminationCondition, &pis_);
190 }
191
192 if (!graph_.hasAStartState())
193 {
194 OMPL_WARN("%s: No solution can be found as no start states are available", name_.c_str());
196 informAboutPlannerStatus(status);
197 return status;
198 }
199
200 // If the graph still doesn't have a goal after waiting there's nothing to solve.
201 if (!graph_.hasAGoalState())
202 {
203 OMPL_WARN("%s: No solution can be found as no goal states are available", name_.c_str());
205 informAboutPlannerStatus(status);
206 return status;
207 }
208
209 OMPL_INFORM("%s: Searching for a solution to the given planning problem. The current best solution cost is "
210 "%.4f",
211 name_.c_str(), solutionCost_.value());
212
213 // Iterate to solve the problem.
214 while (!terminationCondition && !objective_->isSatisfied(solutionCost_))
215 {
216 iterate();
217 }
218
219 // Someone might call ProblemDefinition::clearSolutionPaths() between invokations of Planner::solve(), in
220 // which case previously found solutions are not registered with the problem definition anymore.
221 updateExactSolution();
222
223 // If there are no exact solutions registered in the problem definition and we're tracking approximate
224 // solutions, find the best vertex in the graph.
225 if (!pdef_->hasExactSolution() && trackApproximateSolutions_)
226 {
227 for (const auto &vertex : graph_.getVertices())
228 {
229 updateApproximateSolution(vertex);
230 }
231 }
232
233 // Return the right planner status.
234 if (objective_->isFinite(solutionCost_))
235 {
237 }
238 else if (trackApproximateSolutions_ && objective_->isFinite(approximateSolutionCost_))
239 {
241 }
242 else
243 {
245 }
246
247 informAboutPlannerStatus(status);
248 return status;
249 }
250
252 {
253 return solutionCost_;
254 }
255
257 {
258 // base::PlannerDataVertex takes a raw pointer to a state. I want to guarantee, that the state lives as long
259 // as the program lives.
260 static std::set<
261 std::shared_ptr<aitstar::Vertex>,
262 std::function<bool(const std::shared_ptr<aitstar::Vertex> &, const std::shared_ptr<aitstar::Vertex> &)>>
263 liveStates([](const auto &lhs, const auto &rhs) { return lhs->getId() < rhs->getId(); });
264
265 // Fill the planner progress properties.
266 Planner::getPlannerData(data);
267
268 // Get the vertices.
269 auto vertices = graph_.getVertices();
270
271 // Add the vertices and edges.
272 for (const auto &vertex : vertices)
273 {
274 // Add the vertex to the live states.
275 liveStates.insert(vertex);
276
277 // Add the vertex as the right kind of vertex.
278 if (graph_.isStart(vertex))
279 {
280 data.addStartVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
281 }
282 else if (graph_.isGoal(vertex))
283 {
284 data.addGoalVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
285 }
286 else
287 {
288 data.addVertex(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()));
289 }
290
291 // If it has a parent, add the corresponding edge.
292 if (vertex->hasForwardParent())
293 {
294 data.addEdge(ompl::base::PlannerDataVertex(vertex->getState(), vertex->getId()),
295 ompl::base::PlannerDataVertex(vertex->getForwardParent()->getState(),
296 vertex->getForwardParent()->getId()));
297 }
298 }
299 }
300
301 void AITstar::setBatchSize(std::size_t batchSize)
302 {
303 batchSize_ = batchSize;
304 }
305
306 std::size_t AITstar::getBatchSize() const
307 {
308 return batchSize_;
309 }
310
311 void AITstar::setRewireFactor(double rewireFactor)
312 {
313 graph_.setRewireFactor(rewireFactor);
314 }
315
317 {
318 return graph_.getRewireFactor();
319 }
320
322 {
323 trackApproximateSolutions_ = track;
324 if (!trackApproximateSolutions_)
325 {
326 if (static_cast<bool>(objective_))
327 {
328 approximateSolutionCost_ = objective_->infiniteCost();
329 approximateSolutionCostToGoal_ = objective_->infiniteCost();
330 }
331 }
332 }
333
335 {
336 return trackApproximateSolutions_;
337 }
338
339 void AITstar::enablePruning(bool prune)
340 {
341 isPruningEnabled_ = prune;
342 }
343
345 {
346 return isPruningEnabled_;
347 }
348
349 void AITstar::setUseKNearest(bool useKNearest)
350 {
351 graph_.setUseKNearest(useKNearest);
352 }
353
355 {
356 return graph_.getUseKNearest();
357 }
358
359 void AITstar::setRepairReverseSearch(bool repairReverseSearch)
360 {
361 repairReverseSearch_ = repairReverseSearch;
362 }
363
364 void AITstar::rebuildForwardQueue()
365 {
366 // Get all edges from the queue.
367 std::vector<aitstar::Edge> edges;
368 forwardQueue_.getContent(edges);
369
370 // Rebuilding the queue invalidates the incoming and outgoing lookup.
371 for (const auto &edge : edges)
372 {
373 edge.getChild()->resetForwardQueueIncomingLookup();
374 edge.getParent()->resetForwardQueueOutgoingLookup();
375 }
376
377 // Clear the queue.
378 forwardQueue_.clear();
379
380 // Insert all edges into the queue if they connect vertices that have been processed, otherwise store them
381 // in the cache of edges that are to be inserted.
382 if (haveAllVerticesBeenProcessed(edges))
383 {
384 for (auto &edge : edges)
385 {
386 insertOrUpdateInForwardQueue(aitstar::Edge(edge.getParent(), edge.getChild(),
387 computeSortKey(edge.getParent(), edge.getChild())));
388 }
389 }
390 else
391 {
392 edgesToBeInserted_ = edges;
393 performReverseSearchIteration_ = true;
394 }
395 }
396
397 void AITstar::rebuildReverseQueue()
398 {
399 // Rebuilding the reverse queue invalidates the reverse queue pointers.
400 std::vector<KeyVertexPair> content;
401 reverseQueue_.getContent(content);
402 for (auto &element : content)
403 {
404 element.second->resetReverseQueuePointer();
405 }
406 reverseQueue_.clear();
407
408 for (auto &vertex : content)
409 {
410 // Compute the sort key for the vertex queue.
411 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
412 computeSortKey(vertex.second), vertex.second);
413 auto reverseQueuePointer = reverseQueue_.insert(element);
414 element.second->setReverseQueuePointer(reverseQueuePointer);
415 }
416 }
417
418 void AITstar::informAboutNewSolution() const
419 {
420 OMPL_INFORM("%s (%u iterations): Found a new exact solution of cost %.4f. Sampled a total of %u states, %u "
421 "of which were valid samples (%.1f \%). Processed %u edges, %u of which were collision checked "
422 "(%.1f \%). The forward search tree has %u vertices. The reverse search tree has %u vertices.",
423 name_.c_str(), numIterations_, solutionCost_.value(), graph_.getNumberOfSampledStates(),
425 graph_.getNumberOfSampledStates() == 0u ?
426 0.0 :
427 100.0 * (static_cast<double>(graph_.getNumberOfValidSamples()) /
428 static_cast<double>(graph_.getNumberOfSampledStates())),
429 numProcessedEdges_, numEdgeCollisionChecks_,
430 numProcessedEdges_ == 0u ? 0.0 :
431 100.0 * (static_cast<float>(numEdgeCollisionChecks_) /
432 static_cast<float>(numProcessedEdges_)),
433 countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
434 }
435
436 void AITstar::informAboutPlannerStatus(ompl::base::PlannerStatus::StatusType status) const
437 {
438 switch (status)
439 {
441 {
442 OMPL_INFORM("%s (%u iterations): Found an exact solution of cost %.4f.", name_.c_str(),
443 numIterations_, solutionCost_.value());
444 break;
445 }
447 {
448 OMPL_INFORM("%s (%u iterations): Did not find an exact solution, but found an approximate solution "
449 "of cost %.4f which is %.4f away from a goal (in cost space).",
450 name_.c_str(), numIterations_, approximateSolutionCost_.value(),
451 approximateSolutionCostToGoal_.value());
452 break;
453 }
455 {
456 if (trackApproximateSolutions_)
457 {
458 OMPL_INFORM("%s (%u iterations): Did not find any solution.", name_.c_str(), numIterations_);
459 }
460 else
461 {
462 OMPL_INFORM("%s (%u iterations): Did not find an exact solution, and tracking approximate "
463 "solutions is disabled.",
464 name_.c_str(), numIterations_);
465 }
466 break;
467 }
475 {
476 OMPL_INFORM("%s (%u iterations): Unable to solve the given planning problem.", name_.c_str(),
477 numIterations_);
478 }
479 }
480
482 "%s (%u iterations): Sampled a total of %u states, %u of which were valid samples (%.1f \%). "
483 "Processed %u edges, %u of which were collision checked (%.1f \%). The forward search tree "
484 "has %u vertices. The reverse search tree has %u vertices.",
485 name_.c_str(), numIterations_, graph_.getNumberOfSampledStates(), graph_.getNumberOfValidSamples(),
486 graph_.getNumberOfSampledStates() == 0u ?
487 0.0 :
488 100.0 * (static_cast<double>(graph_.getNumberOfValidSamples()) /
489 static_cast<double>(graph_.getNumberOfSampledStates())),
490 numProcessedEdges_, numEdgeCollisionChecks_,
491 numProcessedEdges_ == 0u ?
492 0.0 :
493 100.0 * (static_cast<float>(numEdgeCollisionChecks_) / static_cast<float>(numProcessedEdges_)),
494 countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
495 }
496
497 std::vector<aitstar::Edge> AITstar::getEdgesInQueue() const
498 {
499 std::vector<aitstar::Edge> edges;
500 forwardQueue_.getContent(edges);
501 return edges;
502 }
503
504 std::vector<std::shared_ptr<aitstar::Vertex>> AITstar::getVerticesInQueue() const
505 {
506 // Get the content from the queue.
507 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>> content;
508 reverseQueue_.getContent(content);
509
510 // Return the vertices.
511 std::vector<std::shared_ptr<aitstar::Vertex>> vertices;
512 for (const auto &pair : content)
513 {
514 vertices.emplace_back(pair.second);
515 }
516 return vertices;
517 }
518
520 {
521 if (!forwardQueue_.empty())
522 {
523 return forwardQueue_.top()->data;
524 }
525
526 return {};
527 }
528
529 std::shared_ptr<aitstar::Vertex> AITstar::getNextVertexInQueue() const
530 {
531 if (!reverseQueue_.empty())
532 {
533 return reverseQueue_.top()->data.second;
534 }
535
536 return {};
537 }
538
539 std::vector<std::shared_ptr<aitstar::Vertex>> AITstar::getVerticesInReverseSearchTree() const
540 {
541 // Get all vertices from the graph.
542 auto vertices = graph_.getVertices();
543
544 // Erase the vertices that are not in the reverse search tree.
545 vertices.erase(std::remove_if(vertices.begin(), vertices.end(),
546 [this](const std::shared_ptr<aitstar::Vertex> &vertex) {
547 return !graph_.isGoal(vertex) && !vertex->hasReverseParent();
548 }),
549 vertices.end());
550 return vertices;
551 }
552
553 void AITstar::iterate()
554 {
555 // If this is the first time solve is called, populate the reverse queue.
556 if (numIterations_ == 0u)
557 {
558 for (const auto &goal : graph_.getGoalVertices())
559 {
560 // Set the cost to come from the goal to identity cost.
561 goal->setCostToComeFromGoal(objective_->identityCost());
562
563 // Create an element for the queue.
564 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
565 std::array<ompl::base::Cost, 2u>(
566 {computeCostToGoToStartHeuristic(goal), ompl::base::Cost(0.0)}),
567 goal);
568
569 // Insert the element into the queue and set the corresponding pointer.
570 auto reverseQueuePointer = reverseQueue_.insert(element);
571 goal->setReverseQueuePointer(reverseQueuePointer);
572 }
573 }
574
575 // Keep track of the number of iterations.
576 ++numIterations_;
577
578 // If the algorithm is in a state that requires performing a reverse search iteration, try to perform one.
579 if (performReverseSearchIteration_)
580 {
581 // If the reverse queue is not empty, perform a reverse search iteration.
582 if (!reverseQueue_.empty())
583 {
584 performReverseSearchIteration();
585 }
586 else
587 {
588 // If the reverse queue is empty, check if there are forward edges to be inserted.
589 // Only insert forward edges that connect vertices that have been processed with the reverse search.
590 // If the reverse queue is empty and a vertex has not been processed with the reverse queue, it
591 // means that it's not in the same connected component of the RGG as the goal. We can not reach the
592 // goal from this vertex and therefore this edge can be disregarded.
593 for (const auto &edge : edgesToBeInserted_)
594 {
595 if (haveAllVerticesBeenProcessed(edge))
596 {
597 insertOrUpdateInForwardQueue(aitstar::Edge(
598 edge.getParent(), edge.getChild(), computeSortKey(edge.getParent(), edge.getChild())));
599 }
600 }
601 edgesToBeInserted_.clear();
602 performReverseSearchIteration_ = false;
603 forwardQueueMustBeRebuilt_ = true;
604 }
605 }
606 else
607 {
608 if (!isForwardSearchStartedOnBatch_)
609 {
610 // Remember that we've started the forward search on this batch.
611 isForwardSearchStartedOnBatch_ = true;
612
613 // If no start vertex has finite cost to come from the goal, there is no need to start the
614 // forward search.
615 std::vector<aitstar::Edge> outgoingStartEdges;
616 for (const auto &start : graph_.getStartVertices())
617 {
618 if (objective_->isFinite(start->getCostToComeFromGoal()))
619 {
620 // Add the outgoing edges of all start vertices to the queue.
621 for (const auto &start : graph_.getStartVertices())
622 {
623 const auto outgoingEdges = getOutgoingEdges(start);
624 outgoingStartEdges.insert(outgoingStartEdges.end(), outgoingEdges.begin(),
625 outgoingEdges.end());
626 }
627 }
628 }
629 // If all vertices of the outgoing start edges have been processed, insert the edges into the
630 // forward queue. If not, remember that they are to be inserted.
631 if (haveAllVerticesBeenProcessed(outgoingStartEdges))
632 {
633 for (const auto &edge : outgoingStartEdges)
634 {
635 insertOrUpdateInForwardQueue(edge);
636 }
637 }
638 else
639 {
640 assert(edgesToBeInserted_.empty());
641 edgesToBeInserted_ = outgoingStartEdges;
642 performReverseSearchIteration_ = true;
643 }
644 }
645 else if (forwardQueueMustBeRebuilt_)
646 {
647 // Rebuild the forwared queue if necessary.
648 rebuildForwardQueue();
649 forwardQueueMustBeRebuilt_ = false;
650 }
651 else if (!forwardQueue_.empty())
652 {
653 // If the forward queue is not empty, perform a forward search iteration.
654 performForwardSearchIteration();
655 }
656 else // We should not perform a reverse search iteration and the forward queue is empty. Add more
657 // samples.
658 {
659 // Clear the reverse queue.
660 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>>
661 reverseQueue;
662 reverseQueue_.getContent(reverseQueue);
663 for (const auto &element : reverseQueue)
664 {
665 element.second->resetReverseQueuePointer();
666 }
667 reverseQueue_.clear();
668
669 // Clear the forward queue.
670 std::vector<aitstar::Edge> forwardQueue;
671 forwardQueue_.getContent(forwardQueue);
672 for (const auto &element : forwardQueue)
673 {
674 element.getChild()->resetForwardQueueIncomingLookup();
675 element.getParent()->resetForwardQueueOutgoingLookup();
676 }
677 forwardQueue_.clear();
678
679 // Clear the cache of edges to be inserted.
680 edgesToBeInserted_.clear();
681
682 // Add new start and goal states if necessary.
684 {
686 }
687
688 // Remove useless samples from the graph.
689 if (isPruningEnabled_)
690 {
691 graph_.prune();
692 }
693
694 // Add new samples to the graph.
695 graph_.addSamples(batchSize_);
696
697 // Add the goals to the reverse queue.
698 for (const auto &goal : graph_.getGoalVertices())
699 {
700 auto reverseQueuePointer = reverseQueue_.insert(std::make_pair(computeSortKey(goal), goal));
701 goal->setReverseQueuePointer(reverseQueuePointer);
702 goal->setCostToComeFromGoal(objective_->identityCost());
703 }
704
705 // This is a new batch, so the search hasn't been started.
706 isForwardSearchStartedOnBatch_ = false;
707
708 // We have to update the heuristic. Start with a reverse iteration.
709 performReverseSearchIteration_ = true;
710 }
711 }
712 }
713
714 void AITstar::performForwardSearchIteration()
715 {
716 // We should never perform a forward search iteration while there are still edges to be inserted.
717 assert(edgesToBeInserted_.empty());
718
719 // Get the most promising edge.
720 auto &edge = forwardQueue_.top()->data;
721 auto parent = edge.getParent();
722 auto child = edge.getChild();
723
724 // Make sure the edge is sane
725 assert(child->hasReverseParent() || graph_.isGoal(child));
726 assert(parent->hasReverseParent() || graph_.isGoal(parent));
727
728 // Remove the edge from the incoming and outgoing lookups.
729 child->removeFromForwardQueueIncomingLookup(forwardQueue_.top());
730 parent->removeFromForwardQueueOutgoingLookup(forwardQueue_.top());
731
732 // Remove the edge from the queue.
733 forwardQueue_.pop();
734
735 // This counts as processing an edge.
736 ++numProcessedEdges_;
737
738 // Register that an outgoing edge of the parent has been popped from the queue. This means that the parent
739 // has optimal cost-to-come for the current approximation.
740 parent->registerPoppedOutgoingEdgeDuringForwardSearch();
741
742 // If this is edge can not possibly improve our solution, the search is done.
743 auto edgeCost = objective_->motionCostHeuristic(parent->getState(), child->getState());
744 auto parentCostToGoToGoal = objective_->combineCosts(edgeCost, child->getCostToGoToGoal());
745 auto pathThroughEdgeCost = objective_->combineCosts(parent->getCostToComeFromStart(), parentCostToGoToGoal);
746 if (!objective_->isCostBetterThan(pathThroughEdgeCost, solutionCost_))
747 {
748 if (objective_->isFinite(pathThroughEdgeCost) ||
749 !objective_->isFinite(computeBestCostToComeFromGoalOfAnyStart()))
750 {
751 std::vector<aitstar::Edge> edges;
752 forwardQueue_.getContent(edges);
753 for (const auto &edge : edges)
754 {
755 edge.getChild()->resetForwardQueueIncomingLookup();
756 edge.getParent()->resetForwardQueueOutgoingLookup();
757 }
758 forwardQueue_.clear();
759 }
760 else
761 {
762 performReverseSearchIteration_ = true;
763 }
764 } // This edge can improve the solution. Check if it's already in the reverse search tree.
765 else if (child->hasForwardParent() && child->getForwardParent()->getId() == parent->getId())
766 {
767 // This is a freebie, just insert the outgoing edges of the child.
768 auto edges = getOutgoingEdges(child);
769 if (haveAllVerticesBeenProcessed(edges))
770 {
771 for (const auto &edge : edges)
772 {
773 insertOrUpdateInForwardQueue(edge);
774 }
775 }
776 else
777 {
778 edgesToBeInserted_ = edges;
779 performReverseSearchIteration_ = true;
780 return;
781 }
782 } // This edge can improve the solution and is not already in the reverse search tree.
783 else if (objective_->isCostBetterThan(child->getCostToComeFromStart(),
784 objective_->combineCosts(parent->getCostToComeFromStart(),
785 objective_->motionCostHeuristic(
786 parent->getState(), child->getState()))))
787 {
788 // If the edge cannot improve the cost to come to the child, we're done processing it.
789 return;
790 } // The edge can possibly improve the solution and the path to the child. Let's check it for collision.
791 else if (parent->isWhitelistedAsChild(child) ||
792 motionValidator_->checkMotion(parent->getState(), child->getState()))
793 {
794 // Remember that this is a good edge.
795 if (!parent->isWhitelistedAsChild(child))
796 {
797 parent->whitelistAsChild(child);
798 numEdgeCollisionChecks_++;
799 }
800
801 // Compute the edge cost.
802 auto edgeCost = objective_->motionCost(parent->getState(), child->getState());
803
804 // Check if the edge can improve the cost to come to the child.
805 if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCost),
806 child->getCostToComeFromStart()))
807 {
808 // If the child has already been expanded during the current forward search, something's fishy.
809 assert(!child->hasHadOutgoingEdgePoppedDuringCurrentForwardSearch());
810
811 // Rewire the child.
812 child->setForwardParent(parent, edgeCost);
813
814 // Add it to the children of the parent.
815 parent->addToForwardChildren(child);
816
817 // Share the good news with the whole branch.
818 child->updateCostOfForwardBranch();
819
820 // Check if the solution can benefit from this.
821 updateExactSolution();
822
823 // If we don't have an exact solution but are tracking approximate solutions, see if the child is
824 // the best approximate solution so far.
825 if (!pdef_->hasExactSolution() && trackApproximateSolutions_)
826 {
827 updateApproximateSolution(child);
828 }
829
830 // Insert the child's outgoing edges into the queue.
831 auto edges = getOutgoingEdges(child);
832 if (haveAllVerticesBeenProcessed(edges))
833 {
834 for (const auto &edge : edges)
835 {
836 insertOrUpdateInForwardQueue(edge);
837 }
838 }
839 else
840 {
841 edgesToBeInserted_ = edges;
842 performReverseSearchIteration_ = true;
843 return;
844 }
845 }
846 }
847 else
848 {
849 // This child should be blacklisted.
850 parent->blacklistAsChild(child);
851
852 // If desired, now is the time to repair the reverse search.
853 if (repairReverseSearch_)
854 {
855 if (parent->hasReverseParent() && parent->getReverseParent()->getId() == child->getId())
856 {
857 // The parent was connected to the child through an invalid edge.
858 parent->setCostToComeFromGoal(objective_->infiniteCost());
859 parent->setExpandedCostToComeFromGoal(objective_->infiniteCost());
860 parent->resetReverseParent();
861 child->removeFromReverseChildren(parent->getId());
862
863 // This also affects all children of this vertex.
864 invalidateCostToComeFromGoalOfReverseBranch(parent);
865
866 // If any of these children are in the reverse queue, their sort key is outdated.
867 rebuildReverseQueue();
868
869 // The parent's cost-to-come needs to be updated. This places children in open.
870 reverseSearchUpdateVertex(parent);
871
872 // If the reverse queue is empty, this means we have to add new samples.
873 if (reverseQueue_.empty())
874 {
875 std::vector<aitstar::Edge> edges;
876 forwardQueue_.getContent(edges);
877 for (const auto &edge : edges)
878 {
879 edge.getChild()->resetForwardQueueIncomingLookup();
880 edge.getParent()->resetForwardQueueOutgoingLookup();
881 }
882 forwardQueue_.clear();
883 }
884 else
885 {
886 performReverseSearchIteration_ = true;
887 }
888 }
889 }
890 }
891 }
892
893 void AITstar::performReverseSearchIteration()
894 {
895 assert(!reverseQueue_.empty());
896
897 // Get the most promising vertex.
898 auto vertex = reverseQueue_.top()->data.second;
899
900 // Remove it from the queue.
901 reverseQueue_.pop();
902 vertex->resetReverseQueuePointer();
903
904 // The open queue should not contain consistent vertices.
905 assert((!objective_->isFinite(vertex->getCostToComeFromGoal()) &&
906 !objective_->isFinite(vertex->getExpandedCostToComeFromGoal())) ||
907 (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
908 vertex->getExpandedCostToComeFromGoal())));
909
910 // If any goal is underconsistent, we need to continue.
911 bool underconsistentStart{false};
912 for (const auto &start : graph_.getStartVertices())
913 {
914 if (objective_->isCostBetterThan(start->getExpandedCostToComeFromGoal(),
915 start->getCostToComeFromGoal()))
916 {
917 underconsistentStart = true;
918 break;
919 }
920 }
921
922 // If there is currently no reason to think this vertex can be on an optimal path, clear the queue.
923 if (edgesToBeInserted_.empty() &&
924 ((!underconsistentStart &&
925 !objective_->isCostBetterThan(objective_->combineCosts(vertex->getCostToComeFromGoal(),
926 computeCostToGoToStartHeuristic(vertex)),
927 solutionCost_)) ||
928 objective_->isCostBetterThan(
929 ompl::base::Cost(computeBestCostToComeFromGoalOfAnyStart().value() + 1e-6), solutionCost_)))
930 {
931 // This invalidates the cost-to-go estimate of the forward search.
932 performReverseSearchIteration_ = false;
933 forwardQueueMustBeRebuilt_ = true;
934 vertex->registerExpansionDuringReverseSearch();
935 return;
936 }
937
938 // Check if the vertex is overconsistent. g(s) < v(s).
939 if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
940 {
941 // Register the expansion of this vertex.
942 vertex->registerExpansionDuringReverseSearch();
943 }
944 else
945 {
946 // Register the expansion of this vertex.
947 vertex->registerExpansionDuringReverseSearch();
948 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
949 reverseSearchUpdateVertex(vertex);
950 }
951
952 // Update all successors. Start with the reverse search children, because if this vertex
953 // becomes the parent of a neighbor, that neighbor would be updated again as part of the
954 // reverse children.
955 for (const auto &child : vertex->getReverseChildren())
956 {
957 reverseSearchUpdateVertex(child);
958 }
959
960 // We can now process the neighbors.
961 for (const auto &neighbor : graph_.getNeighbors(vertex))
962 {
963 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
964 !vertex->isBlacklistedAsChild(neighbor))
965 {
966 reverseSearchUpdateVertex(neighbor);
967 }
968 }
969
970 // We also need to update the forward search children.
971 for (const auto &child : vertex->getForwardChildren())
972 {
973 reverseSearchUpdateVertex(child);
974 }
975
976 // We also need to update the forward search parent if it exists.
977 if (vertex->hasForwardParent())
978 {
979 reverseSearchUpdateVertex(vertex->getForwardParent());
980 }
981
982 if (!edgesToBeInserted_.empty())
983 {
984 if (haveAllVerticesBeenProcessed(edgesToBeInserted_))
985 {
986 for (std::size_t i = 0u; i < edgesToBeInserted_.size(); ++i)
987 {
988 auto &edge = edgesToBeInserted_.at(i);
989 edge.setSortKey(computeSortKey(edge.getParent(), edge.getChild()));
990 insertOrUpdateInForwardQueue(edge);
991 }
992 edgesToBeInserted_.clear();
993 }
994 }
995 }
996
997 void AITstar::reverseSearchUpdateVertex(const std::shared_ptr<aitstar::Vertex> &vertex)
998 {
999 if (!graph_.isGoal(vertex))
1000 {
1001 // Get the best parent for this vertex.
1002 auto bestParent = vertex->getReverseParent();
1003 auto bestCost =
1004 vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
1005
1006 // Check all neighbors as defined by the graph.
1007 for (const auto &neighbor : graph_.getNeighbors(vertex))
1008 {
1009 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
1010 !vertex->isBlacklistedAsChild(neighbor))
1011 {
1012 auto edgeCost = objective_->motionCostHeuristic(neighbor->getState(), vertex->getState());
1013 auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
1014 if (objective_->isCostBetterThan(parentCost, bestCost))
1015 {
1016 bestParent = neighbor;
1017 bestCost = parentCost;
1018 }
1019 }
1020 }
1021
1022 // Check all children this vertex holds in the forward search.
1023 for (const auto &forwardChild : vertex->getForwardChildren())
1024 {
1025 auto edgeCost = objective_->motionCostHeuristic(forwardChild->getState(), vertex->getState());
1026 auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
1027
1028 if (objective_->isCostBetterThan(parentCost, bestCost))
1029 {
1030 bestParent = forwardChild;
1031 bestCost = parentCost;
1032 }
1033 }
1034
1035 // Check the parent of this vertex in the forward search.
1036 if (vertex->hasForwardParent())
1037 {
1038 auto forwardParent = vertex->getForwardParent();
1039 auto edgeCost = objective_->motionCostHeuristic(forwardParent->getState(), vertex->getState());
1040 auto parentCost =
1041 objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
1042
1043 if (objective_->isCostBetterThan(parentCost, bestCost))
1044 {
1045 bestParent = forwardParent;
1046 bestCost = parentCost;
1047 }
1048 }
1049
1050 // Check the parent of this vertex in the reverse search.
1051 if (vertex->hasReverseParent())
1052 {
1053 auto reverseParent = vertex->getReverseParent();
1054 auto edgeCost = objective_->motionCostHeuristic(reverseParent->getState(), vertex->getState());
1055 auto parentCost =
1056 objective_->combineCosts(reverseParent->getExpandedCostToComeFromGoal(), edgeCost);
1057
1058 if (objective_->isCostBetterThan(parentCost, bestCost))
1059 {
1060 bestParent = reverseParent;
1061 bestCost = parentCost;
1062 }
1063 }
1064
1065 // If this vertex is now disconnected, take special care.
1066 if (!objective_->isFinite(bestCost))
1067 {
1068 // Reset the reverse parent if the vertex has one.
1069 if (vertex->hasReverseParent())
1070 {
1071 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
1072 vertex->resetReverseParent();
1073 }
1074
1075 // Invalidate the branch in the reverse search tree that is rooted at this vertex.
1076 vertex->setCostToComeFromGoal(objective_->infiniteCost());
1077 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1078 auto affectedVertices = vertex->invalidateReverseBranch();
1079
1080 // Remove the affected edges from the forward queue, placing them in the edge cache.
1081 for (const auto &affectedVertex : affectedVertices)
1082 {
1083 auto forwardQueueIncomingLookup = affectedVertex.lock()->getForwardQueueIncomingLookup();
1084 for (const auto &element : forwardQueueIncomingLookup)
1085 {
1086 edgesToBeInserted_.emplace_back(element->data);
1087 element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1088 forwardQueue_.remove(element);
1089 }
1090 affectedVertex.lock()->resetForwardQueueIncomingLookup();
1091
1092 auto forwardQueueOutgoingLookup = affectedVertex.lock()->getForwardQueueOutgoingLookup();
1093 for (const auto &element : forwardQueueOutgoingLookup)
1094 {
1095 edgesToBeInserted_.emplace_back(element->data);
1096 element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1097 forwardQueue_.remove(element);
1098 }
1099 affectedVertex.lock()->resetForwardQueueOutgoingLookup();
1100 }
1101
1102 // Remove appropriate edges from the forward queue that target the root of the branch.
1103 auto vertexForwardQueueIncomingLookup = vertex->getForwardQueueIncomingLookup();
1104 for (const auto &element : vertexForwardQueueIncomingLookup)
1105 {
1106 auto &edge = element->data;
1107 auto it = std::find_if(affectedVertices.begin(), affectedVertices.end(),
1108 [edge](const auto &affectedVertex) {
1109 return affectedVertex.lock()->getId() == edge.getParent()->getId();
1110 });
1111 if (it != affectedVertices.end())
1112 {
1113 edgesToBeInserted_.emplace_back(element->data);
1114 vertex->removeFromForwardQueueIncomingLookup(element);
1115 element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1116 forwardQueue_.remove(element);
1117 }
1118 }
1119
1120 // Remove appropriate edges from the forward queue that target the root of the branch.
1121 auto vertexForwardQueueOutgoingLookup = vertex->getForwardQueueOutgoingLookup();
1122 for (const auto &element : vertexForwardQueueOutgoingLookup)
1123 {
1124 edgesToBeInserted_.emplace_back(element->data);
1125 vertex->removeFromForwardQueueOutgoingLookup(element);
1126 element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1127 forwardQueue_.remove(element);
1128 }
1129
1130 // Check update the invalidated vertices and insert them in open if they become connected to the
1131 // tree.
1132 for (const auto &affectedVertex : affectedVertices)
1133 {
1134 auto affectedVertexPtr = affectedVertex.lock();
1135
1136 reverseSearchUpdateVertex(affectedVertexPtr);
1137 if (affectedVertex.lock()->hasReverseParent())
1138 {
1139 insertOrUpdateInReverseQueue(affectedVertexPtr);
1140 affectedVertexPtr->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1141 }
1142 }
1143
1144 return;
1145 }
1146
1147 // Update the reverse parent.
1148 vertex->setReverseParent(bestParent);
1149
1150 // Update the children of the parent.
1151 bestParent->addToReverseChildren(vertex);
1152
1153 // Set the cost to come from the goal.
1154 vertex->setCostToComeFromGoal(bestCost);
1155
1156 // If this has made the vertex inconsistent, insert or update it in the open queue.
1157 if (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
1158 vertex->getExpandedCostToComeFromGoal()))
1159 {
1160 insertOrUpdateInReverseQueue(vertex);
1161 }
1162 else
1163 {
1164 // Remove this vertex from the queue if it is in the queue.
1165 auto reverseQueuePointer = vertex->getReverseQueuePointer();
1166 if (reverseQueuePointer)
1167 {
1168 reverseQueue_.remove(reverseQueuePointer);
1169 vertex->resetReverseQueuePointer();
1170 }
1171 }
1172 }
1173 }
1174
1175 void AITstar::insertOrUpdateInReverseQueue(const std::shared_ptr<aitstar::Vertex> &vertex)
1176 {
1177 // Get the pointer to the element in the queue.
1178 auto element = vertex->getReverseQueuePointer();
1179
1180 // Update it if it is in the queue.
1181 if (element)
1182 {
1183 element->data.first = computeSortKey(vertex);
1184 reverseQueue_.update(element);
1185 }
1186 else // Insert it into the queue otherwise.
1187 {
1188 // Compute the sort key for the vertex queue.
1189 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
1190 computeSortKey(vertex), vertex);
1191
1192 // Insert the vertex into the queue, storing the corresponding pointer.
1193 auto reverseQueuePointer = reverseQueue_.insert(element);
1194 vertex->setReverseQueuePointer(reverseQueuePointer);
1195 }
1196 }
1197
1198 void AITstar::insertOrUpdateInForwardQueue(const aitstar::Edge &edge)
1199 {
1200 // Check if the edge is already in the queue and can be updated.
1201 auto lookup = edge.getChild()->getForwardQueueIncomingLookup();
1202 auto it = std::find_if(lookup.begin(), lookup.end(), [&edge](const auto element) {
1203 return element->data.getParent()->getId() == edge.getParent()->getId();
1204 });
1205
1206 if (it != lookup.end())
1207 {
1208 // We used the incoming lookup of the child. Assert that it is already in the outgoing lookup of the
1209 // parent.
1210 assert(std::find_if(edge.getParent()->getForwardQueueOutgoingLookup().begin(),
1211 edge.getParent()->getForwardQueueOutgoingLookup().end(),
1212 [&edge](const auto element) {
1213 return element->data.getChild()->getId() == edge.getChild()->getId();
1214 }) != edge.getParent()->getForwardQueueOutgoingLookup().end());
1215 (*it)->data.setSortKey(edge.getSortKey());
1216 forwardQueue_.update(*it);
1217 }
1218 else
1219 {
1220 auto element = forwardQueue_.insert(edge);
1221 edge.getParent()->addToForwardQueueOutgoingLookup(element);
1222 edge.getChild()->addToForwardQueueIncomingLookup(element);
1223 }
1224 }
1225
1226 std::shared_ptr<ompl::geometric::PathGeometric>
1227 AITstar::getPathToVertex(const std::shared_ptr<aitstar::Vertex> &vertex) const
1228 {
1229 // Create the reverse path by following the parents to the start.
1230 std::vector<std::shared_ptr<aitstar::Vertex>> reversePath;
1231 auto current = vertex;
1232 while (!graph_.isStart(current))
1233 {
1234 reversePath.emplace_back(current);
1235 current = current->getForwardParent();
1236 }
1237 reversePath.emplace_back(current);
1238
1239 // Reverse the reverse path to get the forward path.
1240 auto path = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
1241 for (const auto &vertex : boost::adaptors::reverse(reversePath))
1242 {
1243 path->append(vertex->getState());
1244 }
1245
1246 return path;
1247 }
1248
1249 std::array<ompl::base::Cost, 3u> AITstar::computeSortKey(const std::shared_ptr<aitstar::Vertex> &parent,
1250 const std::shared_ptr<aitstar::Vertex> &child) const
1251 {
1252 // Compute the sort key [g_T(start) + c_hat(start, neighbor) + h_hat(neighbor), g_T(start) +
1253 // c_hat(start, neighbor), g_T(start)].
1254 ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1255 return {
1256 objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1257 child->getCostToGoToGoal()),
1258 objective_->combineCosts(edgeCostHeuristic, child->getCostToGoToGoal()),
1259 parent->getCostToComeFromStart()};
1260 }
1261
1262 std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(const std::shared_ptr<aitstar::Vertex> &vertex) const
1263 {
1264 // LPA* sort key is [min(g(x), v(x)) + h(x); min(g(x), v(x))].
1265 return {objective_->combineCosts(objective_->betterCost(vertex->getCostToComeFromGoal(),
1266 vertex->getExpandedCostToComeFromGoal()),
1267 computeCostToGoToStartHeuristic(vertex)),
1268 objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1269 }
1270
1271 std::vector<aitstar::Edge> AITstar::getOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex) const
1272 {
1273 // Prepare the return variable.
1274 std::vector<aitstar::Edge> outgoingEdges;
1275
1276 // Insert the edges to the current children.
1277 for (const auto &child : vertex->getForwardChildren())
1278 {
1279 outgoingEdges.emplace_back(vertex, child, computeSortKey(vertex, child));
1280 }
1281
1282 // Insert the edges to the current neighbors.
1283 for (const auto &neighbor : graph_.getNeighbors(vertex))
1284 {
1285 // We do not want self loops.
1286 if (vertex->getId() == neighbor->getId())
1287 {
1288 continue;
1289 }
1290
1291 // If the neighbor is the reverse parent, it will explicitly be added later.
1292 if (vertex->hasReverseParent() && neighbor->getId() == vertex->getReverseParent()->getId())
1293 {
1294 continue;
1295 }
1296
1297 // We do not want blacklisted edges.
1298 if (neighbor->isBlacklistedAsChild(vertex) || vertex->isBlacklistedAsChild(neighbor))
1299 {
1300 continue;
1301 }
1302
1303 // If none of the above tests caught on, we can insert the edge.
1304 outgoingEdges.emplace_back(vertex, neighbor, computeSortKey(vertex, neighbor));
1305 }
1306
1307 // Insert the edge to the reverse search parent.
1308 if (vertex->hasReverseParent())
1309 {
1310 const auto &reverseParent = vertex->getReverseParent();
1311 outgoingEdges.emplace_back(vertex, reverseParent, computeSortKey(vertex, reverseParent));
1312 }
1313
1314 return outgoingEdges;
1315 }
1316
1317 bool AITstar::haveAllVerticesBeenProcessed(const std::vector<aitstar::Edge> &edges) const
1318 {
1319 for (const auto &edge : edges)
1320 {
1321 if (!haveAllVerticesBeenProcessed(edge))
1322 {
1323 return false;
1324 }
1325 }
1326
1327 return true;
1328 }
1329
1330 bool AITstar::haveAllVerticesBeenProcessed(const aitstar::Edge &edge) const
1331 {
1332 return edge.getParent()->hasBeenExpandedDuringCurrentReverseSearch() &&
1333 edge.getChild()->hasBeenExpandedDuringCurrentReverseSearch();
1334 }
1335
1336 void AITstar::updateExactSolution()
1337 {
1338 // Check if any of the goals have a cost to come less than the current solution cost.
1339 for (const auto &goal : graph_.getGoalVertices())
1340 {
1341 // We need to check whether the cost is better, or whether someone has removed the exact solution from
1342 // the problem definition.
1343 if (objective_->isCostBetterThan(goal->getCostToComeFromStart(), solutionCost_) ||
1344 (!pdef_->hasExactSolution() && objective_->isFinite(goal->getCostToComeFromStart())))
1345 {
1346 // Remember the incumbent cost.
1347 solutionCost_ = goal->getCostToComeFromStart();
1348
1349 // Create a solution.
1350 ompl::base::PlannerSolution solution(getPathToVertex(goal));
1351 solution.setPlannerName(name_);
1352
1353 // Set the optimized flag.
1354 solution.setOptimized(objective_, solutionCost_, objective_->isSatisfied(solutionCost_));
1355
1356 // Let the problem definition know that a new solution exists.
1357 pdef_->addSolutionPath(solution);
1358
1359 // Let the user know about the new solution.
1360 informAboutNewSolution();
1361 }
1362 }
1363 }
1364
1365 void AITstar::updateApproximateSolution(const std::shared_ptr<aitstar::Vertex> &vertex)
1366 {
1367 assert(trackApproximateSolutions_);
1368 if (vertex->hasForwardParent() || graph_.isStart(vertex))
1369 {
1370 auto costToGoal = computeCostToGoToGoal(vertex);
1371
1372 // We need to check whether this is better than the current approximate solution or whether someone has
1373 // removed all approximate solutions from the problem definition.
1374 if (objective_->isCostBetterThan(costToGoal, approximateSolutionCostToGoal_) ||
1375 !pdef_->hasApproximateSolution())
1376 {
1377 // Remember the incumbent approximate cost.
1378 approximateSolutionCost_ = vertex->getCostToComeFromStart();
1379 approximateSolutionCostToGoal_ = costToGoal;
1380 ompl::base::PlannerSolution solution(getPathToVertex(vertex));
1381 solution.setPlannerName(name_);
1382
1383 // Set the approximate flag.
1384 solution.setApproximate(costToGoal.value());
1385
1386 // This solution is approximate and can not satisfy the objective.
1387 solution.setOptimized(objective_, approximateSolutionCost_, false);
1388
1389 // Let the problem definition know that a new solution exists.
1390 pdef_->addSolutionPath(solution);
1391 }
1392 }
1393 };
1394
1395 ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const
1396 {
1397 // We need to loop over all start vertices and see which is the closest one.
1398 ompl::base::Cost bestCost = objective_->infiniteCost();
1399 for (const auto &start : graph_.getStartVertices())
1400 {
1401 bestCost = objective_->betterCost(
1402 bestCost, objective_->motionCostHeuristic(vertex->getState(), start->getState()));
1403 }
1404 return bestCost;
1405 }
1406
1407 ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const
1408 {
1409 // We need to loop over all goal vertices and see which is the closest one.
1410 ompl::base::Cost bestCost = objective_->infiniteCost();
1411 for (const auto &goal : graph_.getGoalVertices())
1412 {
1413 bestCost = objective_->betterCost(
1414 bestCost, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
1415 }
1416 return bestCost;
1417 }
1418
1419 ompl::base::Cost AITstar::computeCostToGoToGoal(const std::shared_ptr<aitstar::Vertex> &vertex) const
1420 {
1421 // We need to loop over all goal vertices and see which is the closest one.
1422 ompl::base::Cost bestCost = objective_->infiniteCost();
1423 for (const auto &goal : graph_.getGoalVertices())
1424 {
1425 bestCost =
1426 objective_->betterCost(bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1427 }
1428 return bestCost;
1429 }
1430
1431 ompl::base::Cost AITstar::computeBestCostToComeFromGoalOfAnyStart() const
1432 {
1433 // We need to loop over all start vertices and see which is the closest one.
1434 ompl::base::Cost bestCost = objective_->infiniteCost();
1435 for (const auto &start : graph_.getStartVertices())
1436 {
1437 bestCost = objective_->betterCost(bestCost, start->getCostToComeFromGoal());
1438 }
1439 return bestCost;
1440 }
1441
1442 std::size_t AITstar::countNumVerticesInForwardTree() const
1443 {
1444 std::size_t numVerticesInForwardTree = 0u;
1445 auto vertices = graph_.getVertices();
1446 for (const auto &vertex : vertices)
1447 {
1448 if (graph_.isStart(vertex) || vertex->hasForwardParent())
1449 {
1450 ++numVerticesInForwardTree;
1451 }
1452 }
1453 return numVerticesInForwardTree;
1454 }
1455
1456 std::size_t AITstar::countNumVerticesInReverseTree() const
1457 {
1458 std::size_t numVerticesInReverseTree = 0u;
1459 auto vertices = graph_.getVertices();
1460 for (const auto &vertex : vertices)
1461 {
1462 if (graph_.isGoal(vertex) || vertex->hasReverseParent())
1463 {
1464 ++numVerticesInReverseTree;
1465 }
1466 }
1467 return numVerticesInReverseTree;
1468 }
1469
1470 void AITstar::invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<aitstar::Vertex> &vertex)
1471 {
1472 vertex->unregisterExpansionDuringReverseSearch();
1473 // Update the cost of all reverse children and remove from open.
1474 for (const auto &child : vertex->getReverseChildren())
1475 {
1476 invalidateCostToComeFromGoalOfReverseBranch(child);
1477 child->setCostToComeFromGoal(objective_->infiniteCost());
1478 child->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1479 auto reverseQueuePointer = child->getReverseQueuePointer();
1480 if (reverseQueuePointer)
1481 {
1482 reverseQueue_.remove(reverseQueuePointer);
1483 child->resetReverseQueuePointer();
1484 }
1485 }
1486 }
1487
1488 } // namespace geometric
1489} // 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:348
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition Planner.cpp:355
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:423
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:420
std::string name_
The name of this planner.
Definition Planner.h:426
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:417
bool setup_
Flag indicating whether setup() has been called.
Definition Planner.h:440
A shared pointer wrapper for ompl::base::SpaceInformation.
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition AITstar.cpp:504
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
Definition AITstar.cpp:497
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition AITstar.cpp:339
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
Definition AITstar.cpp:519
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition AITstar.cpp:316
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition AITstar.cpp:344
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition AITstar.cpp:311
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition AITstar.cpp:539
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition AITstar.cpp:301
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition AITstar.cpp:251
AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
Constructs a AIT*.
Definition AITstar.cpp:53
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition AITstar.cpp:354
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition AITstar.cpp:256
void setRepairReverseSearch(bool repairReverseSearch)
Enable LPA* repair of reverse search.
Definition AITstar.cpp:359
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition AITstar.cpp:529
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition AITstar.cpp:104
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition AITstar.cpp:171
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition AITstar.cpp:321
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:349
void clear() override
Clears the algorithm's internal state.
Definition AITstar.cpp:155
std::size_t getBatchSize() const
Get the batch size.
Definition AITstar.cpp:306
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition AITstar.cpp:334
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.
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
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.
std::vector< std::shared_ptr< Vertex > > addSamples(std::size_t numNewSamples)
Adds a batch of samples and returns the samples it has added.
void clear()
Resets the graph to its construction state, without resetting options.
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.
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.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
#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.
@ CRASH
The planner crashed.