Return an empty path from A* when the goal is unreachable (fixes #1509)

PathPlanning/AStar/a_star.py

@@ -61,6 +61,7 @@ def planning(self, sx, sy, gx, gy):
         output:
             rx: x position list of the final path
             ry: y position list of the final path
+            Both lists are empty when the goal cannot be reached.
         """
 
         start_node = self.Node(self.calc_xy_index(sx, self.min_x),
@@ -74,7 +75,7 @@ def planning(self, sx, sy, gx, gy):
         while True:
             if len(open_set) == 0:
                 print("Open set is empty..")
-                break
+                return [], []
 
             c_id = min(
                 open_set,

tests/test_a_star_unreachable_goal.py

@@ -0,0 +1,35 @@
+import conftest
+from PathPlanning.AStar import a_star as m
+
+
+def test_unreachable_goal_returns_empty_path():
+    m.show_animation = False
+    # Square boundary from 0 to 10 with an inner wall at x = 5.
+    ox, oy = [], []
+    for i in range(11):
+        ox.extend([0.0, 10.0, float(i), float(i), 5.0])
+        oy.extend([float(i), float(i), 0.0, 10.0, float(i)])
+    planner = m.AStarPlanner(ox, oy, 1.0, 0.5)
+
+    rx, ry = planner.planning(2.0, 5.0, 8.0, 5.0)
+
+    assert rx == []
+    assert ry == []
+
+
+def test_start_equal_to_goal_returns_single_point_path():
+    m.show_animation = False
+    ox, oy = [], []
+    for i in range(11):
+        ox.extend([0.0, 10.0, float(i), float(i)])
+        oy.extend([float(i), float(i), 0.0, 10.0])
+    planner = m.AStarPlanner(ox, oy, 1.0, 0.5)
+
+    rx, ry = planner.planning(2.0, 5.0, 2.0, 5.0)
+
+    assert rx == [2.0]
+    assert ry == [5.0]
+
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)