From 4dfa91c84a47fb0f6890c04c69d42b12ee492b07 Mon Sep 17 00:00:00 2001 From: James Ward <james.ward@appliedev.com> Date: Sat, 7 Nov 2020 11:14:06 +0800 Subject: [PATCH] Loop fix (#2068) * Abort analytic expansion if crossing through already visited node * Check that we are not creating an infinite loop at the goal node * Mark nodes in analytic expansion as visited for the sake of completeness * Move checking of already visited nodes to final stage of analytic expansion --- smac_planner/src/a_star.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp index 3464442b..e3e3190b 100644 --- a/smac_planner/src/a_star.cpp +++ b/smac_planner/src/a_star.cpp @@ -411,10 +411,20 @@ AStarAlgorithm<NodeSE2>::NodePtr AStarAlgorithm<NodeSE2>::getAnalyticPath( prev = node; for (const auto & node_pose : possible_nodes) { const auto & n = node_pose.first; - n->parent = prev; - prev = n; + if (!n->wasVisited()) { + // Make sure this node has not been visited by the regular algorithm. + // If it has been, there is the (slight) chance that it is in the path we are expanding + // from, so we should skip it. + // Skipping to the next node will still create a kinematically feasible path. + n->parent = prev; + n->visited(); + prev = n; + } + } + if (_goal != prev) { + _goal->parent = prev; + _goal->visited(); } - _goal->parent = prev; return _goal; } -- GitLab