ensure the index also isn't the goal's (#2201)

ros-navigation · Feb 25, 2021 · 4ffc701 · 4ffc701
1 parent 565c294
commit 4ffc701
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_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.