Fix Greedy Best-First path reconstruction for reached and unreachable goals

PathPlanning/GreedyBestFirstSearch/greedy_best_first_search.py

@@ -99,9 +99,7 @@ def planning(self, sx, sy, gx, gy):
 
             if current.x == ngoal.x and current.y == ngoal.y:
                 print("Found goal")
-                ngoal.pind = current.pind
-                ngoal.cost = current.cost
-                break
+                return self.calc_final_path(current)
 
             # expand_grid search grid based on motion model
             for i, _ in enumerate(self.motion):
@@ -124,19 +122,16 @@ def planning(self, sx, sy, gx, gy):
                 else:
                     if open_set[n_id].cost > node.cost:
                         open_set[n_id] = node
-        closed_set[ngoal.pind] = current
-        rx, ry = self.calc_final_path(ngoal, closed_set)
-        return rx, ry
-
-    def calc_final_path(self, ngoal, closedset):
-        # generate final course
-        rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
-            self.calc_grid_position(ngoal.y, self.miny)]
-        n = closedset[ngoal.pind]
-        while n is not None:
-            rx.append(self.calc_grid_position(n.x, self.minx))
-            ry.append(self.calc_grid_position(n.y, self.miny))
-            n = n.parent
+        return [], []
+
+    def calc_final_path(self, goal_node):
+        # Follow parent pointers directly, from goal back to start.
+        rx, ry = [], []
+        node = goal_node
+        while node is not None:
+            rx.append(self.calc_grid_position(node.x, self.minx))
+            ry.append(self.calc_grid_position(node.y, self.miny))
+            node = node.parent
 
         return rx, ry
 

tests/test_greedy_best_first_search.py

@@ -7,5 +7,48 @@ def test_1():
     m.main()
 
 
+def _make_bounded_map(block_center=False):
+    ox, oy = [], []
+    for i in range(8):
+        ox.extend([i, i, 0, 7])
+        oy.extend([0, 7, i, i])
+    if block_center:
+        for y in range(1, 7):
+            ox.append(3)
+            oy.append(y)
+    return ox, oy
+
+
+def test_unreachable_goal_returns_empty_path():
+    m.show_animation = False
+    ox, oy = _make_bounded_map(block_center=True)
+    planner = m.BestFirstSearchPlanner(ox, oy, 1.0, 0.0)
+
+    rx, ry = planner.planning(1.0, 2.0, 5.0, 2.0)
+
+    assert rx == []
+    assert ry == []
+
+
+def test_adjacent_goal_appears_only_once():
+    m.show_animation = False
+    ox, oy = _make_bounded_map()
+    planner = m.BestFirstSearchPlanner(ox, oy, 1.0, 0.0)
+
+    rx, ry = planner.planning(1.0, 2.0, 2.0, 2.0)
+
+    assert list(zip(rx, ry)) == [(2.0, 2.0), (1.0, 2.0)]
+
+
+def test_start_equals_goal_returns_single_point():
+    m.show_animation = False
+    ox, oy = _make_bounded_map()
+    planner = m.BestFirstSearchPlanner(ox, oy, 1.0, 0.0)
+
+    rx, ry = planner.planning(1.0, 2.0, 1.0, 2.0)
+
+    assert list(zip(rx, ry)) == [(1.0, 2.0)]
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)