diff --git a/PathPlanning/FlowField/flowfield.py b/PathPlanning/FlowField/flowfield.py index e50430de3c..5dd5b80a2b 100644 --- a/PathPlanning/FlowField/flowfield.py +++ b/PathPlanning/FlowField/flowfield.py @@ -90,7 +90,7 @@ def create_integration_field(self): for i in range(-1, 2): for j in range(-1, 2): x, y = curr_x + i, curr_y + j - if self.obs_grid[(x, y)] == 'obs': + if (x, y) not in self.integration_field: continue if (i, j) in [(1, 0), (0, 1), (-1, 0), (0, -1)]: e_cost = 10 @@ -118,20 +118,26 @@ def assign_vectors(self): offset_list = [(i + a, j + b) for a in range(-1, 2) for b in range(-1, 2)] - neighbor_list = [{'loc': pt, - 'cost': self.integration_field[pt]} - for pt in offset_list - if self.obs_grid[pt] != 'obs'] - neighbor_list = sorted(neighbor_list, key=lambda x: x['cost']) - best_neighbor = neighbor_list[0]['loc'] - self.vector_field[(i, j)] = best_neighbor + # Only traversable, goal-connected cells provide directions. + neighbor_list = [ + (self.integration_field[pt], pt) + for pt in offset_list + if pt in self.integration_field + and np.isfinite(self.integration_field[pt]) + ] + if not neighbor_list: + self.vector_field[(i, j)] = (None, None) + continue + + self.vector_field[(i, j)] = min( + neighbor_list, key=lambda item: item[0])[1] def follow_vectors(self): curr_x, curr_y = self.start_pt while curr_x is not None and curr_y is not None: curr_x, curr_y = self.vector_field[(curr_x, curr_y)] - if show_animation: + if show_animation and curr_x is not None and curr_y is not None: plt.plot(curr_x, curr_y, "b*") plt.pause(0.001) diff --git a/tests/test_flow_field.py b/tests/test_flow_field.py index d049731fe5..10b5613ddb 100644 --- a/tests/test_flow_field.py +++ b/tests/test_flow_field.py @@ -1,4 +1,5 @@ import conftest +import numpy as np import PathPlanning.FlowField.flowfield as flow_field @@ -7,5 +8,80 @@ def test(): flow_field.main() +def test_free_grid_with_goal_at_boundary(): + flow_field.show_animation = False + grid = {(x, y): 'free' for x in range(3) for y in range(3)} + planner = flow_field.FlowField(grid, 2, 2, 0, 0, 3, 3) + planner.find_path() + + assert planner.integration_field[(2, 2)] == 0 + assert np.isfinite(planner.integration_field[(0, 0)]) + + # Every free cell has a finite cost and a valid descending direction. + for position, cost in planner.integration_field.items(): + assert np.isfinite(cost) + if position == (2, 2): + assert planner.vector_field[position] == (None, None) + continue + neighbor = planner.vector_field[position] + assert neighbor in grid + assert planner.integration_field[neighbor] < cost + + position = (0, 0) + visited = {position} + for _ in range(9): + next_position = planner.vector_field[position] + if next_position == (None, None): + break + assert next_position not in visited + visited.add(next_position) + position = next_position + + assert position == (2, 2) + + +def test_disconnected_start_has_no_navigation_direction(): + flow_field.show_animation = False + grid = { + (x, y): ('obs' if x == 1 else 'free') + for x in range(4) for y in range(4) + } + planner = flow_field.FlowField(grid, 3, 3, 0, 0, 4, 4) + planner.find_path() + + assert np.isinf(planner.integration_field[(0, 0)]) + assert planner.vector_field[(0, 0)] == (None, None) + assert planner.integration_field[(3, 3)] == 0 + + +def test_single_cell_grid_is_valid(): + flow_field.show_animation = False + planner = flow_field.FlowField({(0, 0): 'free'}, 0, 0, 0, 0, 1, 1) + planner.find_path() + + assert planner.integration_field[(0, 0)] == 0 + assert planner.vector_field[(0, 0)] == (None, None) + + +def test_animation_skips_termination_sentinel(monkeypatch): + """Do not send (None, None) to Matplotlib after reaching the goal.""" + monkeypatch.setattr(flow_field, 'show_animation', True) + plotted = [] + shown = [] + monkeypatch.setattr( + flow_field.plt, 'plot', + lambda x, y, *args: plotted.append((x, y)), + ) + monkeypatch.setattr(flow_field.plt, 'pause', lambda *_: None) + monkeypatch.setattr(flow_field.plt, 'show', lambda: shown.append(True)) + + grid = {(0, 0): 'free', (1, 0): 'free'} + planner = flow_field.FlowField(grid, 1, 0, 0, 0, 2, 1) + planner.find_path() + + assert plotted == [(1, 0)] + assert shown == [True] + + if __name__ == '__main__': conftest.run_this_test(__file__)