backporting to foxy checking goal index on backchecks (#2202)

ros-navigation · Feb 25, 2021 · 63b9510 · 63b9510
1 parent 77c1b74
commit 63b9510
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp
@@ -411,7 +411,7 @@ AStarAlgorithm<NodeSE2>::NodePtr AStarAlgorithm<NodeSE2>::getAnalyticPath(
   prev = node;
   for (const auto & node_pose : possible_nodes) {
     const auto & n = node_pose.first;
-    if (!n->wasVisited()) {
+    if (!n->wasVisited() && n->getIndex() != _goal->getIndex()) {
       // 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.