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_))
216 iterate(terminationCondition);
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();
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 if (graph_.
addSamples(batchSize_, terminationCondition))
663 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>>
666 for (
const auto &element : reverseQueue)
668 element.second->resetReverseQueuePointer();
670 reverseQueue_.
clear();
673 std::vector<aitstar::Edge> forwardQueue;
675 for (
const auto &element : forwardQueue)
677 element.getChild()->resetForwardQueueIncomingLookup();
678 element.getParent()->resetForwardQueueOutgoingLookup();
680 forwardQueue_.
clear();
683 edgesToBeInserted_.clear();
686 if (isPruningEnabled_)
700 goal->setCostToComeFromGoal(objective_->identityCost());
701 auto reverseQueuePointer = reverseQueue_.
insert(std::make_pair(computeSortKey(goal), goal));
702 goal->setReverseQueuePointer(reverseQueuePointer);
706 isForwardSearchStartedOnBatch_ =
false;
709 performReverseSearchIteration_ =
true;
715 void AITstar::performForwardSearchIteration()
718 assert(edgesToBeInserted_.empty());
721 auto &edge = forwardQueue_.
top()->data;
722 auto parent = edge.getParent();
723 auto child = edge.getChild();
726 assert(child->hasReverseParent() || graph_.
isGoal(child));
727 assert(parent->hasReverseParent() || graph_.
isGoal(parent));
730 child->removeFromForwardQueueIncomingLookup(forwardQueue_.
top());
731 parent->removeFromForwardQueueOutgoingLookup(forwardQueue_.
top());
737 ++numProcessedEdges_;
741 parent->registerPoppedOutgoingEdgeDuringForwardSearch();
744 auto edgeCost = objective_->motionCostHeuristic(parent->getState(), child->getState());
745 auto parentCostToGoToGoal = objective_->combineCosts(edgeCost, child->getCostToGoToGoal());
746 auto pathThroughEdgeCost = objective_->combineCosts(parent->getCostToComeFromStart(), parentCostToGoToGoal);
747 if (!objective_->isCostBetterThan(pathThroughEdgeCost, solutionCost_))
749 if (objective_->isFinite(pathThroughEdgeCost) ||
750 !objective_->isFinite(computeBestCostToComeFromGoalOfAnyStart()))
752 std::vector<aitstar::Edge> edges;
754 for (
const auto &edge : edges)
756 edge.getChild()->resetForwardQueueIncomingLookup();
757 edge.getParent()->resetForwardQueueOutgoingLookup();
759 forwardQueue_.
clear();
763 performReverseSearchIteration_ =
true;
766 else if (child->hasForwardParent() && child->getForwardParent()->getId() == parent->getId())
769 auto edges = getOutgoingEdges(child);
770 if (haveAllVerticesBeenProcessed(edges))
772 for (
const auto &edge : edges)
774 insertOrUpdateInForwardQueue(edge);
779 edgesToBeInserted_ = edges;
780 performReverseSearchIteration_ =
true;
784 else if (objective_->isCostBetterThan(child->getCostToComeFromStart(),
785 objective_->combineCosts(parent->getCostToComeFromStart(),
786 objective_->motionCostHeuristic(
787 parent->getState(), child->getState()))))
792 else if (parent->isWhitelistedAsChild(child) ||
793 motionValidator_->checkMotion(parent->getState(), child->getState()))
796 if (!parent->isWhitelistedAsChild(child))
798 parent->whitelistAsChild(child);
799 numEdgeCollisionChecks_++;
803 auto edgeCost = objective_->motionCost(parent->getState(), child->getState());
806 if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCost),
807 child->getCostToComeFromStart()))
810 assert(!child->hasHadOutgoingEdgePoppedDuringCurrentForwardSearch());
813 child->setForwardParent(parent, edgeCost);
816 parent->addToForwardChildren(child);
819 child->updateCostOfForwardBranch();
822 updateExactSolution();
826 if (!
pdef_->hasExactSolution() && trackApproximateSolutions_)
828 updateApproximateSolution(child);
832 auto edges = getOutgoingEdges(child);
833 if (haveAllVerticesBeenProcessed(edges))
835 for (
const auto &edge : edges)
837 insertOrUpdateInForwardQueue(edge);
842 edgesToBeInserted_ = edges;
843 performReverseSearchIteration_ =
true;
851 parent->blacklistAsChild(child);
854 if (repairReverseSearch_)
856 if (parent->hasReverseParent() && parent->getReverseParent()->getId() == child->getId())
859 parent->setCostToComeFromGoal(objective_->infiniteCost());
860 parent->setExpandedCostToComeFromGoal(objective_->infiniteCost());
861 parent->resetReverseParent();
862 child->removeFromReverseChildren(parent->getId());
865 invalidateCostToComeFromGoalOfReverseBranch(parent);
868 rebuildReverseQueue();
871 reverseSearchUpdateVertex(parent);
874 if (reverseQueue_.
empty())
876 std::vector<aitstar::Edge> edges;
878 for (
const auto &edge : edges)
880 edge.getChild()->resetForwardQueueIncomingLookup();
881 edge.getParent()->resetForwardQueueOutgoingLookup();
883 forwardQueue_.
clear();
887 performReverseSearchIteration_ =
true;
894 void AITstar::performReverseSearchIteration()
896 assert(!reverseQueue_.
empty());
899 auto vertex = reverseQueue_.
top()->data.second;
903 vertex->resetReverseQueuePointer();
906 assert((!objective_->isFinite(vertex->getCostToComeFromGoal()) &&
907 !objective_->isFinite(vertex->getExpandedCostToComeFromGoal())) ||
908 (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
909 vertex->getExpandedCostToComeFromGoal())));
912 bool underconsistentStart{
false};
915 if (objective_->isCostBetterThan(start->getExpandedCostToComeFromGoal(),
916 start->getCostToComeFromGoal()))
918 underconsistentStart =
true;
924 if (edgesToBeInserted_.empty() &&
925 ((!underconsistentStart &&
926 !objective_->isCostBetterThan(objective_->combineCosts(vertex->getCostToComeFromGoal(),
927 computeCostToGoToStartHeuristic(vertex)),
929 objective_->isCostBetterThan(
930 ompl::base::Cost(computeBestCostToComeFromGoalOfAnyStart().value() + 1e-6), solutionCost_)))
933 performReverseSearchIteration_ =
false;
934 forwardQueueMustBeRebuilt_ =
true;
935 vertex->registerExpansionDuringReverseSearch();
940 if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
943 vertex->registerExpansionDuringReverseSearch();
948 vertex->registerExpansionDuringReverseSearch();
949 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
950 reverseSearchUpdateVertex(vertex);
956 for (
const auto &child : vertex->getReverseChildren())
958 reverseSearchUpdateVertex(child);
962 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
964 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
965 !vertex->isBlacklistedAsChild(neighbor))
967 reverseSearchUpdateVertex(neighbor);
972 for (
const auto &child : vertex->getForwardChildren())
974 reverseSearchUpdateVertex(child);
978 if (vertex->hasForwardParent())
980 reverseSearchUpdateVertex(vertex->getForwardParent());
983 if (!edgesToBeInserted_.empty())
985 if (haveAllVerticesBeenProcessed(edgesToBeInserted_))
987 for (std::size_t i = 0u; i < edgesToBeInserted_.size(); ++i)
989 auto &edge = edgesToBeInserted_.at(i);
990 edge.setSortKey(computeSortKey(edge.getParent(), edge.getChild()));
991 insertOrUpdateInForwardQueue(edge);
993 edgesToBeInserted_.clear();
998 void AITstar::reverseSearchUpdateVertex(
const std::shared_ptr<aitstar::Vertex> &vertex)
1000 if (!graph_.
isGoal(vertex))
1003 auto bestParent = vertex->getReverseParent();
1005 vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
1008 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
1010 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
1011 !vertex->isBlacklistedAsChild(neighbor))
1013 auto edgeCost = objective_->motionCostHeuristic(neighbor->getState(), vertex->getState());
1014 auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
1015 if (objective_->isCostBetterThan(parentCost,
bestCost))
1017 bestParent = neighbor;
1024 for (
const auto &forwardChild : vertex->getForwardChildren())
1026 auto edgeCost = objective_->motionCostHeuristic(forwardChild->getState(), vertex->getState());
1027 auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
1029 if (objective_->isCostBetterThan(parentCost,
bestCost))
1031 bestParent = forwardChild;
1037 if (vertex->hasForwardParent())
1039 auto forwardParent = vertex->getForwardParent();
1040 auto edgeCost = objective_->motionCostHeuristic(forwardParent->getState(), vertex->getState());
1042 objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
1044 if (objective_->isCostBetterThan(parentCost,
bestCost))
1046 bestParent = forwardParent;
1052 if (vertex->hasReverseParent())
1054 auto reverseParent = vertex->getReverseParent();
1055 auto edgeCost = objective_->motionCostHeuristic(reverseParent->getState(), vertex->getState());
1057 objective_->combineCosts(reverseParent->getExpandedCostToComeFromGoal(), edgeCost);
1059 if (objective_->isCostBetterThan(parentCost,
bestCost))
1061 bestParent = reverseParent;
1067 if (!objective_->isFinite(
bestCost))
1070 if (vertex->hasReverseParent())
1072 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
1073 vertex->resetReverseParent();
1077 vertex->setCostToComeFromGoal(objective_->infiniteCost());
1078 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1079 auto affectedVertices = vertex->invalidateReverseBranch();
1082 for (
const auto &affectedVertex : affectedVertices)
1084 auto forwardQueueIncomingLookup = affectedVertex.lock()->getForwardQueueIncomingLookup();
1085 for (
const auto &element : forwardQueueIncomingLookup)
1087 edgesToBeInserted_.emplace_back(element->data);
1088 element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1089 forwardQueue_.
remove(element);
1091 affectedVertex.lock()->resetForwardQueueIncomingLookup();
1093 auto forwardQueueOutgoingLookup = affectedVertex.lock()->getForwardQueueOutgoingLookup();
1094 for (
const auto &element : forwardQueueOutgoingLookup)
1096 edgesToBeInserted_.emplace_back(element->data);
1097 element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1098 forwardQueue_.
remove(element);
1100 affectedVertex.lock()->resetForwardQueueOutgoingLookup();
1104 auto vertexForwardQueueIncomingLookup = vertex->getForwardQueueIncomingLookup();
1105 for (
const auto &element : vertexForwardQueueIncomingLookup)
1107 auto &edge = element->data;
1108 auto it = std::find_if(affectedVertices.begin(), affectedVertices.end(),
1109 [edge](
const auto &affectedVertex) {
1110 return affectedVertex.lock()->getId() == edge.getParent()->getId();
1112 if (it != affectedVertices.end())
1114 edgesToBeInserted_.emplace_back(element->data);
1115 vertex->removeFromForwardQueueIncomingLookup(element);
1116 element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1117 forwardQueue_.
remove(element);
1122 auto vertexForwardQueueOutgoingLookup = vertex->getForwardQueueOutgoingLookup();
1123 for (
const auto &element : vertexForwardQueueOutgoingLookup)
1125 edgesToBeInserted_.emplace_back(element->data);
1126 vertex->removeFromForwardQueueOutgoingLookup(element);
1127 element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1128 forwardQueue_.
remove(element);
1133 for (
const auto &affectedVertex : affectedVertices)
1135 auto affectedVertexPtr = affectedVertex.lock();
1137 reverseSearchUpdateVertex(affectedVertexPtr);
1138 if (affectedVertex.lock()->hasReverseParent())
1140 insertOrUpdateInReverseQueue(affectedVertexPtr);
1141 affectedVertexPtr->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1149 vertex->setReverseParent(bestParent);
1152 bestParent->addToReverseChildren(vertex);
1155 vertex->setCostToComeFromGoal(
bestCost);
1158 if (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
1159 vertex->getExpandedCostToComeFromGoal()))
1161 insertOrUpdateInReverseQueue(vertex);
1166 auto reverseQueuePointer = vertex->getReverseQueuePointer();
1167 if (reverseQueuePointer)
1169 reverseQueue_.
remove(reverseQueuePointer);
1170 vertex->resetReverseQueuePointer();
1176 void AITstar::insertOrUpdateInReverseQueue(
const std::shared_ptr<aitstar::Vertex> &vertex)
1179 auto element = vertex->getReverseQueuePointer();
1184 element->data.first = computeSortKey(vertex);
1185 reverseQueue_.
update(element);
1190 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
1191 computeSortKey(vertex), vertex);
1194 auto reverseQueuePointer = reverseQueue_.
insert(element);
1195 vertex->setReverseQueuePointer(reverseQueuePointer);
1199 void AITstar::insertOrUpdateInForwardQueue(
const aitstar::Edge &edge)
1202 auto lookup = edge.getChild()->getForwardQueueIncomingLookup();
1203 auto it = std::find_if(lookup.begin(), lookup.end(), [&edge](
const auto element) {
1204 return element->data.getParent()->getId() == edge.getParent()->getId();
1207 if (it != lookup.end())
1211 assert(std::find_if(edge.getParent()->getForwardQueueOutgoingLookup().begin(),
1212 edge.getParent()->getForwardQueueOutgoingLookup().end(),
1213 [&edge](
const auto element) {
1214 return element->data.getChild()->getId() == edge.getChild()->getId();
1215 }) != edge.getParent()->getForwardQueueOutgoingLookup().end());
1216 (*it)->data.setSortKey(edge.getSortKey());
1217 forwardQueue_.
update(*it);
1221 auto element = forwardQueue_.
insert(edge);
1222 edge.getParent()->addToForwardQueueOutgoingLookup(element);
1223 edge.getChild()->addToForwardQueueIncomingLookup(element);
1227 std::shared_ptr<ompl::geometric::PathGeometric>
1228 AITstar::getPathToVertex(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1231 std::vector<std::shared_ptr<aitstar::Vertex>> reversePath;
1232 auto current = vertex;
1233 while (!graph_.
isStart(current))
1235 reversePath.emplace_back(current);
1236 current = current->getForwardParent();
1238 reversePath.emplace_back(current);
1241 auto path = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
1242 for (
const auto &vertex : boost::adaptors::reverse(reversePath))
1244 path->append(vertex->getState());
1250 std::array<ompl::base::Cost, 3u> AITstar::computeSortKey(
const std::shared_ptr<aitstar::Vertex> &parent,
1251 const std::shared_ptr<aitstar::Vertex> &child)
const
1255 ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1257 objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1258 child->getCostToGoToGoal()),
1259 objective_->combineCosts(edgeCostHeuristic, child->getCostToGoToGoal()),
1260 parent->getCostToComeFromStart()};
1263 std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1266 return {objective_->combineCosts(objective_->betterCost(vertex->getCostToComeFromGoal(),
1267 vertex->getExpandedCostToComeFromGoal()),
1268 computeCostToGoToStartHeuristic(vertex)),
1269 objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1272 std::vector<aitstar::Edge> AITstar::getOutgoingEdges(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1275 std::vector<aitstar::Edge> outgoingEdges;
1278 for (
const auto &child : vertex->getForwardChildren())
1280 outgoingEdges.emplace_back(vertex, child, computeSortKey(vertex, child));
1284 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
1287 if (vertex->getId() == neighbor->getId())
1293 if (vertex->hasReverseParent() && neighbor->getId() == vertex->getReverseParent()->getId())
1299 if (neighbor->isBlacklistedAsChild(vertex) || vertex->isBlacklistedAsChild(neighbor))
1305 outgoingEdges.emplace_back(vertex, neighbor, computeSortKey(vertex, neighbor));
1309 if (vertex->hasReverseParent())
1311 const auto &reverseParent = vertex->getReverseParent();
1312 outgoingEdges.emplace_back(vertex, reverseParent, computeSortKey(vertex, reverseParent));
1315 return outgoingEdges;
1318 bool AITstar::haveAllVerticesBeenProcessed(
const std::vector<aitstar::Edge> &edges)
const
1320 for (
const auto &edge : edges)
1322 if (!haveAllVerticesBeenProcessed(edge))
1331 bool AITstar::haveAllVerticesBeenProcessed(
const aitstar::Edge &edge)
const
1333 return edge.getParent()->hasBeenExpandedDuringCurrentReverseSearch() &&
1334 edge.getChild()->hasBeenExpandedDuringCurrentReverseSearch();
1337 void AITstar::updateExactSolution()
1344 if (objective_->isCostBetterThan(goal->getCostToComeFromStart(), solutionCost_) ||
1345 (!
pdef_->hasExactSolution() && objective_->isFinite(goal->getCostToComeFromStart())))
1348 solutionCost_ = goal->getCostToComeFromStart();
1352 solution.setPlannerName(
name_);
1355 solution.setOptimized(objective_, solutionCost_, objective_->isSatisfied(solutionCost_));
1358 pdef_->addSolutionPath(solution);
1361 informAboutNewSolution();
1366 void AITstar::updateApproximateSolution(
const std::shared_ptr<aitstar::Vertex> &vertex)
1368 assert(trackApproximateSolutions_);
1369 if (vertex->hasForwardParent() || graph_.
isStart(vertex))
1371 auto costToGoal = computeCostToGoToGoal(vertex);
1375 if (objective_->isCostBetterThan(costToGoal, approximateSolutionCostToGoal_) ||
1376 !
pdef_->hasApproximateSolution())
1379 approximateSolutionCost_ = vertex->getCostToComeFromStart();
1380 approximateSolutionCostToGoal_ = costToGoal;
1382 solution.setPlannerName(
name_);
1385 solution.setApproximate(costToGoal.value());
1388 solution.setOptimized(objective_, approximateSolutionCost_,
false);
1391 pdef_->addSolutionPath(solution);
1396 ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1403 bestCost, objective_->motionCostHeuristic(vertex->getState(), start->getState()));
1408 ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1415 bestCost, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
1420 ompl::base::Cost AITstar::computeCostToGoToGoal(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1427 objective_->betterCost(
bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1438 bestCost = objective_->betterCost(
bestCost, start->getCostToComeFromGoal());
1443 std::size_t AITstar::countNumVerticesInForwardTree()
const
1445 std::size_t numVerticesInForwardTree = 0u;
1447 for (
const auto &vertex : vertices)
1449 if (graph_.
isStart(vertex) || vertex->hasForwardParent())
1451 ++numVerticesInForwardTree;
1454 return numVerticesInForwardTree;
1457 std::size_t AITstar::countNumVerticesInReverseTree()
const
1459 std::size_t numVerticesInReverseTree = 0u;
1461 for (
const auto &vertex : vertices)
1463 if (graph_.
isGoal(vertex) || vertex->hasReverseParent())
1465 ++numVerticesInReverseTree;
1468 return numVerticesInReverseTree;
1471 void AITstar::invalidateCostToComeFromGoalOfReverseBranch(
const std::shared_ptr<aitstar::Vertex> &vertex)
1473 vertex->unregisterExpansionDuringReverseSearch();
1475 for (
const auto &child : vertex->getReverseChildren())
1477 invalidateCostToComeFromGoalOfReverseBranch(child);
1478 child->setCostToComeFromGoal(objective_->infiniteCost());
1479 child->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1480 auto reverseQueuePointer = child->getReverseQueuePointer();
1481 if (reverseQueuePointer)
1483 reverseQueue_.
remove(reverseQueuePointer);
1484 child->resetReverseQueuePointer();