From c20f1d4cb675f03e6ce6485020d4e46099cd7257 Mon Sep 17 00:00:00 2001 From: blessedefe4real <234967906+blessedefe4real@users.noreply.github.com> Date: Thu, 5 Nov 2026 20:55:54 +0000 Subject: [PATCH] Fix BFS crash for unreachable goal and start equal to goal BreadthFirstSearchPlanner.planning raised KeyError(-1) in two cases: - the goal cannot be reached: the loop broke out on an empty open set and calc_final_path looked up the goal's parent index -1; - the start is the goal: the goal is found immediately with parent index -1, causing the same lookup. Return empty lists when the open set is exhausted, and treat a missing parent as the start of the path. Add regression tests for both cases. --- .../breadth_first_search.py | 6 ++-- tests/test_breadth_first_search.py | 32 +++++++++++++++++++ 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/PathPlanning/BreadthFirstSearch/breadth_first_search.py b/PathPlanning/BreadthFirstSearch/breadth_first_search.py index ad994732a5..9dda6505d8 100644 --- a/PathPlanning/BreadthFirstSearch/breadth_first_search.py +++ b/PathPlanning/BreadthFirstSearch/breadth_first_search.py @@ -57,6 +57,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. """ nstart = self.Node(self.calc_xyindex(sx, self.minx), @@ -70,7 +71,7 @@ def planning(self, sx, sy, gx, gy): while True: if len(open_set) == 0: print("Open set is empty..") - break + return [], [] current = open_set.pop(list(open_set.keys())[0]) @@ -118,7 +119,8 @@ 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.parent_index] + # The goal has no parent when the start is the goal + n = closedset.get(ngoal.parent_index) 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)) diff --git a/tests/test_breadth_first_search.py b/tests/test_breadth_first_search.py index bfc63e39d6..0d087fa590 100644 --- a/tests/test_breadth_first_search.py +++ b/tests/test_breadth_first_search.py @@ -7,5 +7,37 @@ def test_1(): m.main() + +def make_walled_map(): + """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)]) + return ox, oy + + +def test_unreachable_goal_returns_empty_path(): + m.show_animation = False + ox, oy = make_walled_map() + planner = m.BreadthFirstSearchPlanner(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 = make_walled_map() + planner = m.BreadthFirstSearchPlanner(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__)