diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index ccc2989001..7a35207aeb 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -200,6 +200,10 @@ def valid_position(self, position: Position, t: int) -> bool: if not self.inside_grid_bounds(position): return False + # Times outside the simulation window are not valid + if t < 0 or t >= self.time_limit: + return False + # Check if position is not occupied at time t return self.reservation_matrix[position.x, position.y, t] == 0 diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py index 1194c61d2e..8380b22e11 100644 --- a/tests/test_space_time_astar.py +++ b/tests/test_space_time_astar.py @@ -5,6 +5,7 @@ ) from PathPlanning.TimeBasedPathPlanning import SpaceTimeAStar as m import numpy as np +import pytest import conftest @@ -28,5 +29,21 @@ def test_1(): assert path.expanded_node_count < 1000 +def test_no_path_within_time_limit(): + # Goal is farther away than the time limit allows, so planning should + # fail cleanly instead of indexing past the reservation matrix + start = Position(1, 1) + goal = Position(9, 9) + grid = Grid( + np.array([11, 11]), + num_obstacles=0, + obstacle_avoid_points=[start, goal], + time_limit=5, + ) + + m.show_animation = False + with pytest.raises(Exception, match="No path found"): + m.SpaceTimeAStar.plan(grid, start, goal) + if __name__ == "__main__": conftest.run_this_test(__file__)