From d73fd0b5e916673195f378111b90f9322879b35c Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 05:16:08 -0400 Subject: [PATCH 1/2] fix(bidirectional-bfs): handle unreachable goals and reconstruct meeting path once --- .../bidirectional_breadth_first_search.py | 36 +++++++++---------- 1 file changed, 16 insertions(+), 20 deletions(-) diff --git a/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py b/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py index 60a8c5e170..c18b657302 100644 --- a/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py +++ b/PathPlanning/BidirectionalBreadthFirstSearch/bidirectional_breadth_first_search.py @@ -155,32 +155,28 @@ def planning(self, sx, sy, gx, gy): node_B.parent = current_B open_set_B[n_id_B] = node_B - rx, ry = self.calc_final_path_bidir( - meet_point_A, meet_point_B, closed_set_A, closed_set_B) - return rx, ry + if meet_point_A is None: + return [], [] + + return self.calc_final_path_bidir(meet_point_A, meet_point_B) - # takes both set and meeting nodes and calculate optimal path - def calc_final_path_bidir(self, n1, n2, setA, setB): - rxA, ryA = self.calc_final_path(n1, setA) - rxB, ryB = self.calc_final_path(n2, setB) + def calc_final_path_bidir(self, n1, n2): + rxA, ryA = self.calc_final_path(n1) + rxB, ryB = self.calc_final_path(n2) rxA.reverse() ryA.reverse() - rx = rxA + rxB - ry = ryA + ryB - - return rx, ry + # Both halves contain the meeting cell; include it only once. + return rxA + rxB[1:], ryA + ryB[1:] - def calc_final_path(self, goal_node, closed_set): - # generate final course - rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ - self.calc_grid_position(goal_node.y, self.min_y)] - n = closed_set[goal_node.parent_index] - while n is not None: - rx.append(self.calc_grid_position(n.x, self.min_x)) - ry.append(self.calc_grid_position(n.y, self.min_y)) - n = n.parent + def calc_final_path(self, goal_node): + rx, ry = [], [] + node = goal_node + while node is not None: + rx.append(self.calc_grid_position(node.x, self.min_x)) + ry.append(self.calc_grid_position(node.y, self.min_y)) + node = node.parent return rx, ry From e9e6909faa53c1096b491fba9dd4a6bd89d62a6f Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Thu, 29 Oct 2026 05:16:29 -0400 Subject: [PATCH 2/2] test(bidirectional-bfs): cover unreachable, meeting, and coincident goals --- ...test_bidirectional_breadth_first_search.py | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 tests/test_bidirectional_breadth_first_search.py diff --git a/tests/test_bidirectional_breadth_first_search.py b/tests/test_bidirectional_breadth_first_search.py new file mode 100644 index 0000000000..12a19b72ec --- /dev/null +++ b/tests/test_bidirectional_breadth_first_search.py @@ -0,0 +1,64 @@ +import conftest +from PathPlanning.BidirectionalBreadthFirstSearch import ( + bidirectional_breadth_first_search as m, +) + + +def make_planner(blocked=False): + ox, oy = [], [] + for i in range(8): + ox.extend([i, i, 0, 7]) + oy.extend([0, 7, i, i]) + if blocked: + for y in range(1, 7): + ox.append(3) + oy.append(y) + return m.BidirectionalBreadthFirstSearchPlanner(ox, oy, 1.0, 0.0) + + +def test_unreachable_goal_returns_empty_path(): + m.show_animation = False + planner = make_planner(blocked=True) + + rx, ry = planner.planning(1.0, 2.0, 5.0, 2.0) + + assert rx == [] + assert ry == [] + + +def test_adjacent_goal_has_single_meeting_cell(): + m.show_animation = False + planner = make_planner() + + rx, ry = planner.planning(1.0, 2.0, 2.0, 2.0) + + assert list(zip(rx, ry)) == [(1.0, 2.0), (2.0, 2.0)] + + +def test_coincident_start_and_goal_has_one_waypoint(): + m.show_animation = False + planner = make_planner() + + rx, ry = planner.planning(1.0, 2.0, 1.0, 2.0) + + assert list(zip(rx, ry)) == [(1.0, 2.0)] + + +def test_reachable_path_is_contiguous_without_duplicate_meeting(): + m.show_animation = False + planner = make_planner() + + rx, ry = planner.planning(1.0, 1.0, 5.0, 5.0) + path = list(zip(rx, ry)) + + assert path[0] == (1.0, 1.0) + assert path[-1] == (5.0, 5.0) + assert len(path) > 2 + assert all( + max(abs(ax - bx), abs(ay - by)) == 1.0 + for (ax, ay), (bx, by) in zip(path, path[1:]) + ) + + +if __name__ == '__main__': + conftest.run_this_test(__file__)