diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index f5a4703910..0a19ad990e 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/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] diff --git a/tests/test_dijkstra.py b/tests/test_dijkstra.py index 40404153d8..a797e8ada7 100644 --- a/tests/test_dijkstra.py +++ b/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__)