Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions PathPlanning/BreadthFirstSearch/breadth_first_search.py
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand All @@ -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])

Expand Down Expand Up @@ -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))
Expand Down
32 changes: 32 additions & 0 deletions tests/test_breadth_first_search.py
Original file line number Diff line number Diff line change
Expand Up @@ -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__)