41 #include <boost/range/adaptor/reversed.hpp>
43 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
44 #include "ompl/geometric/planners/informedtrees/AITstar.h"
47 using namespace std::string_literals;
54 :
ompl::base::Planner(spaceInformation,
"AITstar")
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(),
61 return objective_->isCostBetterThan(a, b);
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(),
70 return objective_->isCostBetterThan(a, b);
76 specs_.multithreaded =
false;
77 specs_.approximateSolutions =
true;
78 specs_.optimizingPaths =
true;
79 specs_.directed =
true;
80 specs_.provingSolutionNonExistence =
false;
81 specs_.canReportIntermediateSolutions =
true;
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()); });
110 if (
static_cast<bool>(Planner::pdef_))
113 if (!
pdef_->hasOptimizationObjective())
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_));
121 if (
static_cast<bool>(
pdef_->getGoal()))
126 OMPL_ERROR(
"AIT* is currently only implemented for goals that can be cast to "
127 "ompl::base::GOAL_SAMPLEABLE_GOAL_REGION.");
134 objective_ =
pdef_->getOptimizationObjective();
137 solutionCost_ = objective_->infiniteCost();
138 approximateSolutionCost_ = objective_->infiniteCost();
139 approximateSolutionCostToGoal_ = objective_->infiniteCost();
142 motionValidator_ =
si_->getMotionValidator();
151 OMPL_WARN(
"AIT*: Unable to setup without a problem definition.");
158 forwardQueue_.
clear();
159 reverseQueue_.
clear();
160 solutionCost_ = objective_->infiniteCost();
161 approximateSolutionCost_ = objective_->infiniteCost();
162 approximateSolutionCostToGoal_ = objective_->infiniteCost();
163 edgesToBeInserted_.clear();
165 performReverseSearchIteration_ =
true;
166 isForwardSearchStartedOnBatch_ =
false;
167 forwardQueueMustBeRebuilt_ =
false;
174 auto status = ompl::base::PlannerStatus::StatusType::UNKNOWN;
177 Planner::checkValidity();
178 if (!Planner::setup_)
180 OMPL_WARN(
"%s: Failed to setup and thus solve can not do anything meaningful.",
name_.c_str());
181 status = ompl::base::PlannerStatus::StatusType::ABORT;
182 informAboutPlannerStatus(status);
194 OMPL_WARN(
"%s: No solution can be found as no start states are available",
name_.c_str());
195 status = ompl::base::PlannerStatus::StatusType::INVALID_START;
196 informAboutPlannerStatus(status);
203 OMPL_WARN(
"%s: No solution can be found as no goal states are available",
name_.c_str());
204 status = ompl::base::PlannerStatus::StatusType::INVALID_GOAL;
205 informAboutPlannerStatus(status);
209 OMPL_INFORM(
"%s: Searching for a solution to the given planning problem. The current best solution cost is "
214 while (!terminationCondition && !objective_->isSatisfied(solutionCost_))
221 updateExactSolution();
225 if (!
pdef_->hasExactSolution() && trackApproximateSolutions_)
229 updateApproximateSolution(vertex);
234 if (objective_->isFinite(solutionCost_))
236 status = ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION;
238 else if (trackApproximateSolutions_ && objective_->isFinite(approximateSolutionCost_))
240 status = ompl::base::PlannerStatus::StatusType::APPROXIMATE_SOLUTION;
244 status = ompl::base::PlannerStatus::StatusType::TIMEOUT;
247 informAboutPlannerStatus(status);
253 return solutionCost_;
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(); });
266 Planner::getPlannerData(data);
272 for (
const auto &vertex : vertices)
275 liveStates.insert(vertex);
282 else if (graph_.
isGoal(vertex))
292 if (vertex->hasForwardParent())
296 vertex->getForwardParent()->getId()));
303 batchSize_ = batchSize;
323 trackApproximateSolutions_ = track;
324 if (!trackApproximateSolutions_)
326 if (
static_cast<bool>(objective_))
328 approximateSolutionCost_ = objective_->infiniteCost();
329 approximateSolutionCostToGoal_ = objective_->infiniteCost();
336 return trackApproximateSolutions_;
341 isPruningEnabled_ = prune;
346 return isPruningEnabled_;
361 repairReverseSearch_ = repairReverseSearch;
364 void AITstar::rebuildForwardQueue()
367 std::vector<aitstar::Edge> edges;
371 for (
const auto &edge : edges)
373 edge.getChild()->resetForwardQueueIncomingLookup();
374 edge.getParent()->resetForwardQueueOutgoingLookup();
378 forwardQueue_.
clear();
382 if (haveAllVerticesBeenProcessed(edges))
384 for (
auto &edge : edges)
386 insertOrUpdateInForwardQueue(aitstar::Edge(edge.getParent(), edge.getChild(),
387 computeSortKey(edge.getParent(), edge.getChild())));
392 edgesToBeInserted_ = edges;
393 performReverseSearchIteration_ =
true;
397 void AITstar::rebuildReverseQueue()
400 std::vector<KeyVertexPair> content;
402 for (
auto &element : content)
404 element.second->resetReverseQueuePointer();
406 reverseQueue_.
clear();
408 for (
auto &vertex : content)
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);
418 void AITstar::informAboutNewSolution()
const
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.",
429 numProcessedEdges_, numEdgeCollisionChecks_,
430 numProcessedEdges_ == 0u ? 0.0 :
431 100.0 * (
static_cast<float>(numEdgeCollisionChecks_) /
432 static_cast<float>(numProcessedEdges_)),
433 countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
440 case ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION:
442 OMPL_INFORM(
"%s (%u iterations): Found an exact solution of cost %.4f.",
name_.c_str(),
443 numIterations_, solutionCost_.
value());
446 case ompl::base::PlannerStatus::StatusType::APPROXIMATE_SOLUTION:
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());
454 case ompl::base::PlannerStatus::StatusType::TIMEOUT:
456 if (trackApproximateSolutions_)
458 OMPL_INFORM(
"%s (%u iterations): Did not find any solution.",
name_.c_str(), numIterations_);
462 OMPL_INFORM(
"%s (%u iterations): Did not find an exact solution, and tracking approximate "
463 "solutions is disabled.",
464 name_.c_str(), numIterations_);
468 case ompl::base::PlannerStatus::StatusType::UNKNOWN:
469 case ompl::base::PlannerStatus::StatusType::INVALID_START:
470 case ompl::base::PlannerStatus::StatusType::INVALID_GOAL:
471 case ompl::base::PlannerStatus::StatusType::UNRECOGNIZED_GOAL_TYPE:
472 case ompl::base::PlannerStatus::StatusType::CRASH:
473 case ompl::base::PlannerStatus::StatusType::ABORT:
474 case ompl::base::PlannerStatus::StatusType::TYPE_COUNT:
476 OMPL_INFORM(
"%s (%u iterations): Unable to solve the given planning problem.",
name_.c_str(),
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.",
490 numProcessedEdges_, numEdgeCollisionChecks_,
491 numProcessedEdges_ == 0u ?
493 100.0 * (
static_cast<float>(numEdgeCollisionChecks_) /
static_cast<float>(numProcessedEdges_)),
494 countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
499 std::vector<aitstar::Edge> edges;
507 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>> content;
511 std::vector<std::shared_ptr<aitstar::Vertex>> vertices;
512 for (
const auto &pair : content)
514 vertices.emplace_back(pair.second);
521 if (!forwardQueue_.
empty())
523 return forwardQueue_.
top()->data;
531 if (!reverseQueue_.
empty())
533 return reverseQueue_.
top()->data.second;
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();
553 void AITstar::iterate()
556 if (numIterations_ == 0u)
561 goal->setCostToComeFromGoal(objective_->identityCost());
564 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
565 std::array<ompl::base::Cost, 2u>(
570 auto reverseQueuePointer = reverseQueue_.
insert(element);
571 goal->setReverseQueuePointer(reverseQueuePointer);
579 if (performReverseSearchIteration_)
582 if (!reverseQueue_.
empty())
584 performReverseSearchIteration();
593 for (
const auto &edge : edgesToBeInserted_)
595 if (haveAllVerticesBeenProcessed(edge))
597 insertOrUpdateInForwardQueue(aitstar::Edge(
598 edge.getParent(), edge.getChild(), computeSortKey(edge.getParent(), edge.getChild())));
601 edgesToBeInserted_.clear();
602 performReverseSearchIteration_ =
false;
603 forwardQueueMustBeRebuilt_ =
true;
608 if (!isForwardSearchStartedOnBatch_)
611 isForwardSearchStartedOnBatch_ =
true;
615 std::vector<aitstar::Edge> outgoingStartEdges;
618 if (objective_->isFinite(start->getCostToComeFromGoal()))
623 const auto outgoingEdges = getOutgoingEdges(start);
624 outgoingStartEdges.insert(outgoingStartEdges.end(), outgoingEdges.begin(),
625 outgoingEdges.end());
631 if (haveAllVerticesBeenProcessed(outgoingStartEdges))
633 for (
const auto &edge : outgoingStartEdges)
635 insertOrUpdateInForwardQueue(edge);
640 assert(edgesToBeInserted_.empty());
641 edgesToBeInserted_ = outgoingStartEdges;
642 performReverseSearchIteration_ =
true;
645 else if (forwardQueueMustBeRebuilt_)
648 rebuildForwardQueue();
649 forwardQueueMustBeRebuilt_ =
false;
651 else if (!forwardQueue_.
empty())
654 performForwardSearchIteration();
660 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>>
663 for (
const auto &element : reverseQueue)
665 element.second->resetReverseQueuePointer();
667 reverseQueue_.
clear();
670 std::vector<aitstar::Edge> forwardQueue;
672 for (
const auto &element : forwardQueue)
674 element.getChild()->resetForwardQueueIncomingLookup();
675 element.getParent()->resetForwardQueueOutgoingLookup();
677 forwardQueue_.
clear();
680 edgesToBeInserted_.clear();
689 if (isPruningEnabled_)
700 auto reverseQueuePointer = reverseQueue_.
insert(std::make_pair(computeSortKey(goal), goal));
701 goal->setReverseQueuePointer(reverseQueuePointer);
702 goal->setCostToComeFromGoal(objective_->identityCost());
706 isForwardSearchStartedOnBatch_ =
false;
709 performReverseSearchIteration_ =
true;
714 void AITstar::performForwardSearchIteration()
717 assert(edgesToBeInserted_.empty());
720 auto &edge = forwardQueue_.
top()->data;
721 auto parent = edge.getParent();
722 auto child = edge.getChild();
725 assert(child->hasReverseParent() || graph_.
isGoal(child));
726 assert(parent->hasReverseParent() || graph_.
isGoal(parent));
729 child->removeFromForwardQueueIncomingLookup(forwardQueue_.
top());
730 parent->removeFromForwardQueueOutgoingLookup(forwardQueue_.
top());
736 ++numProcessedEdges_;
740 parent->registerPoppedOutgoingEdgeDuringForwardSearch();
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_))
748 if (objective_->isFinite(pathThroughEdgeCost) ||
749 !objective_->isFinite(computeBestCostToComeFromGoalOfAnyStart()))
751 std::vector<aitstar::Edge> edges;
753 for (
const auto &edge : edges)
755 edge.getChild()->resetForwardQueueIncomingLookup();
756 edge.getParent()->resetForwardQueueOutgoingLookup();
758 forwardQueue_.
clear();
762 performReverseSearchIteration_ =
true;
765 else if (child->hasForwardParent() && child->getForwardParent()->getId() == parent->getId())
768 auto edges = getOutgoingEdges(child);
769 if (haveAllVerticesBeenProcessed(edges))
771 for (
const auto &edge : edges)
773 insertOrUpdateInForwardQueue(edge);
778 edgesToBeInserted_ = edges;
779 performReverseSearchIteration_ =
true;
783 else if (objective_->isCostBetterThan(child->getCostToComeFromStart(),
784 objective_->combineCosts(parent->getCostToComeFromStart(),
785 objective_->motionCostHeuristic(
786 parent->getState(), child->getState()))))
791 else if (parent->isWhitelistedAsChild(child) ||
792 motionValidator_->checkMotion(parent->getState(), child->getState()))
795 if (!parent->isWhitelistedAsChild(child))
797 parent->whitelistAsChild(child);
798 numEdgeCollisionChecks_++;
802 auto edgeCost = objective_->motionCost(parent->getState(), child->getState());
805 if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCost),
806 child->getCostToComeFromStart()))
809 assert(!child->hasHadOutgoingEdgePoppedDuringCurrentForwardSearch());
812 child->setForwardParent(parent, edgeCost);
815 parent->addToForwardChildren(child);
818 child->updateCostOfForwardBranch();
821 updateExactSolution();
825 if (!
pdef_->hasExactSolution() && trackApproximateSolutions_)
827 updateApproximateSolution(child);
831 auto edges = getOutgoingEdges(child);
832 if (haveAllVerticesBeenProcessed(edges))
834 for (
const auto &edge : edges)
836 insertOrUpdateInForwardQueue(edge);
841 edgesToBeInserted_ = edges;
842 performReverseSearchIteration_ =
true;
850 parent->blacklistAsChild(child);
853 if (repairReverseSearch_)
855 if (parent->hasReverseParent() && parent->getReverseParent()->getId() == child->getId())
858 parent->setCostToComeFromGoal(objective_->infiniteCost());
859 parent->setExpandedCostToComeFromGoal(objective_->infiniteCost());
860 parent->resetReverseParent();
861 child->removeFromReverseChildren(parent->getId());
864 invalidateCostToComeFromGoalOfReverseBranch(parent);
867 rebuildReverseQueue();
870 reverseSearchUpdateVertex(parent);
873 if (reverseQueue_.
empty())
875 std::vector<aitstar::Edge> edges;
877 for (
const auto &edge : edges)
879 edge.getChild()->resetForwardQueueIncomingLookup();
880 edge.getParent()->resetForwardQueueOutgoingLookup();
882 forwardQueue_.
clear();
886 performReverseSearchIteration_ =
true;
893 void AITstar::performReverseSearchIteration()
895 assert(!reverseQueue_.
empty());
898 auto vertex = reverseQueue_.
top()->data.second;
902 vertex->resetReverseQueuePointer();
905 assert((!objective_->isFinite(vertex->getCostToComeFromGoal()) &&
906 !objective_->isFinite(vertex->getExpandedCostToComeFromGoal())) ||
907 (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
908 vertex->getExpandedCostToComeFromGoal())));
911 bool underconsistentStart{
false};
914 if (objective_->isCostBetterThan(start->getExpandedCostToComeFromGoal(),
915 start->getCostToComeFromGoal()))
917 underconsistentStart =
true;
923 if (edgesToBeInserted_.empty() &&
924 ((!underconsistentStart &&
925 !objective_->isCostBetterThan(objective_->combineCosts(vertex->getCostToComeFromGoal(),
926 computeCostToGoToStartHeuristic(vertex)),
928 objective_->isCostBetterThan(
929 ompl::base::Cost(computeBestCostToComeFromGoalOfAnyStart().value() + 1e-6), solutionCost_)))
932 performReverseSearchIteration_ =
false;
933 forwardQueueMustBeRebuilt_ =
true;
934 vertex->registerExpansionDuringReverseSearch();
939 if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
942 vertex->registerExpansionDuringReverseSearch();
947 vertex->registerExpansionDuringReverseSearch();
948 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
949 reverseSearchUpdateVertex(vertex);
955 for (
const auto &child : vertex->getReverseChildren())
957 reverseSearchUpdateVertex(child);
961 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
963 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
964 !vertex->isBlacklistedAsChild(neighbor))
966 reverseSearchUpdateVertex(neighbor);
971 for (
const auto &child : vertex->getForwardChildren())
973 reverseSearchUpdateVertex(child);
977 if (vertex->hasForwardParent())
979 reverseSearchUpdateVertex(vertex->getForwardParent());
982 if (!edgesToBeInserted_.empty())
984 if (haveAllVerticesBeenProcessed(edgesToBeInserted_))
986 for (std::size_t i = 0u; i < edgesToBeInserted_.size(); ++i)
988 auto &edge = edgesToBeInserted_.at(i);
989 edge.setSortKey(computeSortKey(edge.getParent(), edge.getChild()));
990 insertOrUpdateInForwardQueue(edge);
992 edgesToBeInserted_.clear();
997 void AITstar::reverseSearchUpdateVertex(
const std::shared_ptr<aitstar::Vertex> &vertex)
999 if (!graph_.
isGoal(vertex))
1002 auto bestParent = vertex->getReverseParent();
1004 vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
1007 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
1009 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
1010 !vertex->isBlacklistedAsChild(neighbor))
1012 auto edgeCost = objective_->motionCostHeuristic(neighbor->getState(), vertex->getState());
1013 auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
1014 if (objective_->isCostBetterThan(parentCost,
bestCost))
1016 bestParent = neighbor;
1023 for (
const auto &forwardChild : vertex->getForwardChildren())
1025 auto edgeCost = objective_->motionCostHeuristic(forwardChild->getState(), vertex->getState());
1026 auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
1028 if (objective_->isCostBetterThan(parentCost,
bestCost))
1030 bestParent = forwardChild;
1036 if (vertex->hasForwardParent())
1038 auto forwardParent = vertex->getForwardParent();
1039 auto edgeCost = objective_->motionCostHeuristic(forwardParent->getState(), vertex->getState());
1041 objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
1043 if (objective_->isCostBetterThan(parentCost,
bestCost))
1045 bestParent = forwardParent;
1051 if (vertex->hasReverseParent())
1053 auto reverseParent = vertex->getReverseParent();
1054 auto edgeCost = objective_->motionCostHeuristic(reverseParent->getState(), vertex->getState());
1056 objective_->combineCosts(reverseParent->getExpandedCostToComeFromGoal(), edgeCost);
1058 if (objective_->isCostBetterThan(parentCost,
bestCost))
1060 bestParent = reverseParent;
1066 if (!objective_->isFinite(
bestCost))
1069 if (vertex->hasReverseParent())
1071 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
1072 vertex->resetReverseParent();
1076 vertex->setCostToComeFromGoal(objective_->infiniteCost());
1077 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1078 auto affectedVertices = vertex->invalidateReverseBranch();
1081 for (
const auto &affectedVertex : affectedVertices)
1083 auto forwardQueueIncomingLookup = affectedVertex.lock()->getForwardQueueIncomingLookup();
1084 for (
const auto &element : forwardQueueIncomingLookup)
1086 edgesToBeInserted_.emplace_back(element->data);
1087 element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1088 forwardQueue_.
remove(element);
1090 affectedVertex.lock()->resetForwardQueueIncomingLookup();
1092 auto forwardQueueOutgoingLookup = affectedVertex.lock()->getForwardQueueOutgoingLookup();
1093 for (
const auto &element : forwardQueueOutgoingLookup)
1095 edgesToBeInserted_.emplace_back(element->data);
1096 element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1097 forwardQueue_.
remove(element);
1099 affectedVertex.lock()->resetForwardQueueOutgoingLookup();
1103 auto vertexForwardQueueIncomingLookup = vertex->getForwardQueueIncomingLookup();
1104 for (
const auto &element : vertexForwardQueueIncomingLookup)
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();
1111 if (it != affectedVertices.end())
1113 edgesToBeInserted_.emplace_back(element->data);
1114 vertex->removeFromForwardQueueIncomingLookup(element);
1115 element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1116 forwardQueue_.
remove(element);
1121 auto vertexForwardQueueOutgoingLookup = vertex->getForwardQueueOutgoingLookup();
1122 for (
const auto &element : vertexForwardQueueOutgoingLookup)
1124 edgesToBeInserted_.emplace_back(element->data);
1125 vertex->removeFromForwardQueueOutgoingLookup(element);
1126 element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1127 forwardQueue_.
remove(element);
1132 for (
const auto &affectedVertex : affectedVertices)
1134 auto affectedVertexPtr = affectedVertex.lock();
1136 reverseSearchUpdateVertex(affectedVertexPtr);
1137 if (affectedVertex.lock()->hasReverseParent())
1139 insertOrUpdateInReverseQueue(affectedVertexPtr);
1140 affectedVertexPtr->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1148 vertex->setReverseParent(bestParent);
1151 bestParent->addToReverseChildren(vertex);
1154 vertex->setCostToComeFromGoal(
bestCost);
1157 if (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
1158 vertex->getExpandedCostToComeFromGoal()))
1160 insertOrUpdateInReverseQueue(vertex);
1165 auto reverseQueuePointer = vertex->getReverseQueuePointer();
1166 if (reverseQueuePointer)
1168 reverseQueue_.
remove(reverseQueuePointer);
1169 vertex->resetReverseQueuePointer();
1175 void AITstar::insertOrUpdateInReverseQueue(
const std::shared_ptr<aitstar::Vertex> &vertex)
1178 auto element = vertex->getReverseQueuePointer();
1183 element->data.first = computeSortKey(vertex);
1184 reverseQueue_.
update(element);
1189 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
1190 computeSortKey(vertex), vertex);
1193 auto reverseQueuePointer = reverseQueue_.
insert(element);
1194 vertex->setReverseQueuePointer(reverseQueuePointer);
1198 void AITstar::insertOrUpdateInForwardQueue(
const aitstar::Edge &edge)
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();
1206 if (it != lookup.end())
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);
1220 auto element = forwardQueue_.
insert(edge);
1221 edge.getParent()->addToForwardQueueOutgoingLookup(element);
1222 edge.getChild()->addToForwardQueueIncomingLookup(element);
1226 std::shared_ptr<ompl::geometric::PathGeometric>
1227 AITstar::getPathToVertex(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1230 std::vector<std::shared_ptr<aitstar::Vertex>> reversePath;
1231 auto current = vertex;
1232 while (!graph_.
isStart(current))
1234 reversePath.emplace_back(current);
1235 current = current->getForwardParent();
1237 reversePath.emplace_back(current);
1240 auto path = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
1241 for (
const auto &vertex : boost::adaptors::reverse(reversePath))
1243 path->append(vertex->getState());
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
1254 ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1256 objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1257 child->getCostToGoToGoal()),
1258 objective_->combineCosts(edgeCostHeuristic, child->getCostToGoToGoal()),
1259 parent->getCostToComeFromStart()};
1262 std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1265 return {objective_->combineCosts(objective_->betterCost(vertex->getCostToComeFromGoal(),
1266 vertex->getExpandedCostToComeFromGoal()),
1267 computeCostToGoToStartHeuristic(vertex)),
1268 objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1271 std::vector<aitstar::Edge> AITstar::getOutgoingEdges(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1274 std::vector<aitstar::Edge> outgoingEdges;
1277 for (
const auto &child : vertex->getForwardChildren())
1279 outgoingEdges.emplace_back(vertex, child, computeSortKey(vertex, child));
1283 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
1286 if (vertex->getId() == neighbor->getId())
1292 if (vertex->hasReverseParent() && neighbor->getId() == vertex->getReverseParent()->getId())
1298 if (neighbor->isBlacklistedAsChild(vertex) || vertex->isBlacklistedAsChild(neighbor))
1304 outgoingEdges.emplace_back(vertex, neighbor, computeSortKey(vertex, neighbor));
1308 if (vertex->hasReverseParent())
1310 const auto &reverseParent = vertex->getReverseParent();
1311 outgoingEdges.emplace_back(vertex, reverseParent, computeSortKey(vertex, reverseParent));
1314 return outgoingEdges;
1317 bool AITstar::haveAllVerticesBeenProcessed(
const std::vector<aitstar::Edge> &edges)
const
1319 for (
const auto &edge : edges)
1321 if (!haveAllVerticesBeenProcessed(edge))
1330 bool AITstar::haveAllVerticesBeenProcessed(
const aitstar::Edge &edge)
const
1332 return edge.getParent()->hasBeenExpandedDuringCurrentReverseSearch() &&
1333 edge.getChild()->hasBeenExpandedDuringCurrentReverseSearch();
1336 void AITstar::updateExactSolution()
1343 if (objective_->isCostBetterThan(goal->getCostToComeFromStart(), solutionCost_) ||
1344 (!
pdef_->hasExactSolution() && objective_->isFinite(goal->getCostToComeFromStart())))
1347 solutionCost_ = goal->getCostToComeFromStart();
1351 solution.setPlannerName(
name_);
1354 solution.setOptimized(objective_, solutionCost_, objective_->isSatisfied(solutionCost_));
1357 pdef_->addSolutionPath(solution);
1360 informAboutNewSolution();
1365 void AITstar::updateApproximateSolution(
const std::shared_ptr<aitstar::Vertex> &vertex)
1367 assert(trackApproximateSolutions_);
1368 if (vertex->hasForwardParent() || graph_.
isStart(vertex))
1370 auto costToGoal = computeCostToGoToGoal(vertex);
1374 if (objective_->isCostBetterThan(costToGoal, approximateSolutionCostToGoal_) ||
1375 !
pdef_->hasApproximateSolution())
1378 approximateSolutionCost_ = vertex->getCostToComeFromStart();
1379 approximateSolutionCostToGoal_ = costToGoal;
1381 solution.setPlannerName(
name_);
1384 solution.setApproximate(costToGoal.value());
1387 solution.setOptimized(objective_, approximateSolutionCost_,
false);
1390 pdef_->addSolutionPath(solution);
1395 ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1402 bestCost, objective_->motionCostHeuristic(vertex->getState(), start->getState()));
1407 ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1414 bestCost, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
1419 ompl::base::Cost AITstar::computeCostToGoToGoal(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1426 objective_->betterCost(
bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1437 bestCost = objective_->betterCost(
bestCost, start->getCostToComeFromGoal());
1442 std::size_t AITstar::countNumVerticesInForwardTree()
const
1444 std::size_t numVerticesInForwardTree = 0u;
1446 for (
const auto &vertex : vertices)
1448 if (graph_.
isStart(vertex) || vertex->hasForwardParent())
1450 ++numVerticesInForwardTree;
1453 return numVerticesInForwardTree;
1456 std::size_t AITstar::countNumVerticesInReverseTree()
const
1458 std::size_t numVerticesInReverseTree = 0u;
1460 for (
const auto &vertex : vertices)
1462 if (graph_.
isGoal(vertex) || vertex->hasReverseParent())
1464 ++numVerticesInReverseTree;
1467 return numVerticesInReverseTree;
1470 void AITstar::invalidateCostToComeFromGoalOfReverseBranch(
const std::shared_ptr<aitstar::Vertex> &vertex)
1472 vertex->unregisterExpansionDuringReverseSearch();
1474 for (
const auto &child : vertex->getReverseChildren())
1476 invalidateCostToComeFromGoalOfReverseBranch(child);
1477 child->setCostToComeFromGoal(objective_->infiniteCost());
1478 child->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1479 auto reverseQueuePointer = child->getReverseQueuePointer();
1480 if (reverseQueuePointer)
1482 reverseQueue_.
remove(reverseQueuePointer);
1483 child->resetReverseQueuePointer();