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__)