adding looping fix to foxy (ros-navigation#2075)

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;
 }