diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp
index 3464442b70cd4c65f3cc9e2b5b763e9b0e8e984d..e3e3190bffaa3ed3fb2e32c0f605197f8a30d6e3 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;
 }