Fix FlowField bounds and unreachable navigation vectors

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)
 

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