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