diff --git a/src/roboticstoolbox/mobile/PRMPlanner.py b/src/roboticstoolbox/mobile/PRMPlanner.py index c9fcec047..13b579622 100644 --- a/src/roboticstoolbox/mobile/PRMPlanner.py +++ b/src/roboticstoolbox/mobile/PRMPlanner.py @@ -165,6 +165,8 @@ def _create_roadmap(self, npoints: int, dist_thresh: float | None) -> None: graph = self._graph if occgrid is None or graph is None: raise ValueError("no occupancy grid or graph") + if npoints > 0 and np.all(occgrid.grid): + raise NoPathError("occupancy grid has no free cells") self.progress_start(npoints) @@ -246,6 +248,8 @@ def plan( :param dist_thresh: distance threshold, defaults to ``dist_thresh`` given to constructor :param animate: deprecated, animating the planning is not implemented + :raises NoPathError: points were requested but the occupancy grid has + no free cells Create a probabilistic roadmap. This is a graph connecting points randomly selected from the free space of the occupancy grid. Edges are diff --git a/tests/test_prm_free_space.py b/tests/test_prm_free_space.py new file mode 100644 index 000000000..ca7f2e0ee --- /dev/null +++ b/tests/test_prm_free_space.py @@ -0,0 +1,29 @@ +"""PRM planning on maps where rejection sampling cannot find free space.""" + +import numpy as np +import pytest + +from roboticstoolbox import NoPathError, PRMPlanner + + +@pytest.mark.timeout(2) +@pytest.mark.parametrize("inflate", [0, 1]) +def test_plan_rejects_maps_without_free_cells(inflate: int) -> None: + grid = np.ones((5, 7)) + if inflate: + grid[2, 3] = 0 + planner = PRMPlanner(grid, inflate=inflate, progress=False, seed=0) + + with pytest.raises(NoPathError, match="no free cells"): + planner.plan(npoints=1) + + +def test_plan_with_free_cells_builds_a_roadmap() -> None: + grid = np.ones((5, 7)) + grid[1:4, 2:5] = 0 + planner = PRMPlanner(grid, progress=False, seed=0) + + planner.plan(npoints=5) + + assert planner.graph.n == 5 + assert all(not planner.occgrid.isoccupied(vertex.coord) for vertex in planner.graph)