Fix Dijkstra handling of unreachable goals

Author: huxiaoyi-ovo

PathPlanning/Dijkstra/dijkstra.py

@@ -72,6 +72,10 @@ def planning(self, sx, sy, gx, gy):
         open_set[self.calc_index(start_node)] = start_node
 
         while True:
+            if len(open_set) == 0:
+                print("Open set is empty..")
+                return [], []
+
             c_id = min(open_set, key=lambda o: open_set[o].cost)
             current = open_set[c_id]
 

tests/test_dijkstra.py

@@ -7,5 +7,20 @@ def test_1():
     m.main()
 
 
+def test_unreachable_goal():
+    m.show_animation = False
+
+    ox, oy = [], []
+    for i in range(7):
+        ox.extend([0.0, 6.0, float(i), float(i), 3.0])
+        oy.extend([float(i), float(i), 0.0, 6.0, float(i)])
+
+    planner = m.DijkstraPlanner(ox, oy, 1.0, 0.0)
+    rx, ry = planner.planning(1.0, 3.0, 5.0, 3.0)
+
+    assert rx == []
+    assert ry == []
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)