diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 6d20350432..dcf08da610 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -61,6 +61,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. """ start_node = self.Node(self.calc_xy_index(sx, self.min_x), @@ -74,7 +75,7 @@ def planning(self, sx, sy, gx, gy): while True: if len(open_set) == 0: print("Open set is empty..") - break + return [], [] c_id = min( open_set, diff --git a/tests/test_a_star_unreachable_goal.py b/tests/test_a_star_unreachable_goal.py new file mode 100644 index 0000000000..98b87b6423 --- /dev/null +++ b/tests/test_a_star_unreachable_goal.py @@ -0,0 +1,35 @@ +import conftest +from PathPlanning.AStar import a_star as m + + +def test_unreachable_goal_returns_empty_path(): + m.show_animation = False + # 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)]) + planner = m.AStarPlanner(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 = [], [] + for i in range(11): + ox.extend([0.0, 10.0, float(i), float(i)]) + oy.extend([float(i), float(i), 0.0, 10.0]) + planner = m.AStarPlanner(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__)