Fix bidirectional BFS path reconstruction for empty and meeting paths

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
 

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