diff --git a/src/roboticstoolbox/mobile/DistanceTransformPlanner.py b/src/roboticstoolbox/mobile/DistanceTransformPlanner.py index 99c2b21f4..133c867c0 100644 --- a/src/roboticstoolbox/mobile/DistanceTransformPlanner.py +++ b/src/roboticstoolbox/mobile/DistanceTransformPlanner.py @@ -34,9 +34,11 @@ class DistanceTransformPlanner(PlannerBase): :param metric: distance metric, one of: "euclidean" [default], "manhattan" ("cityblock" is a synonym) :param distance: deprecated, use ``metric`` + :param connectivity: number of path neighbours, 4 or 8, defaults to 8 :param kwargs: common planner options, see :class:`PlannerBase` :raises ValueError: ``metric`` is not recognised, or ``metric`` and - ``distance`` are both given and are different + ``distance`` are both given and are different, or ``connectivity`` + is not an integer equal to 4 or 8 .. deprecated:: 1.5.0 ``distance`` is deprecated, use ``metric``. It was never an argument @@ -53,8 +55,11 @@ class DistanceTransformPlanner(PlannerBase): Creates a planner that finds the path between two points in the 2D grid using omnidirectional motion and avoiding occupied cells. The path - comprises a set of 4- or 8-way connected points in adjacent cells. Also - known as the wavefront, grassfire or brushfire planning algorithm. + comprises a set of 8-way connected points by default, preserving the + paths in the book for both distance metrics. Set ``connectivity=4`` to + restrict both the distance transform and the path to horizontal and + vertical moves. These moves have the same cost for either metric. + Also known as the wavefront, grassfire or brushfire planning algorithm. The map is described by a 2D occupancy ``occgrid`` whose elements are zero if traversable or nonzero if untraversable, ie. an obstacle. @@ -90,6 +95,7 @@ def __init__( metric: str = "euclidean", *, distance: str | None = None, + connectivity: int = 8, **kwargs: Any, ) -> None: if distance is not None: @@ -105,9 +111,15 @@ def __init__( metric = distance if metric.lower() not in METRICS: raise ValueError(f"unknown metric {metric!r}, must be one of {METRICS}") + if not isinstance(connectivity, (int, np.integer)) or connectivity not in ( + 4, + 8, + ): + raise ValueError("connectivity must be an integer equal to 4 or 8") super().__init__(occgrid=occgrid, ndims=2, **kwargs) self._metric = metric + self._connectivity = int(connectivity) self._distancemap: NDArray | None = None @property @@ -119,6 +131,15 @@ def metric(self) -> str: """ return self._metric + @property + def connectivity(self) -> int: + """ + Get the number of path neighbours + + :return: 4 for axial moves, or 8 to include diagonal moves + """ + return self._connectivity + @property def distancemap(self) -> NDArray | None: """ @@ -188,7 +209,7 @@ def plan( distancexform( self.occgrid.grid, goal=self.occgrid.w2g(self._goal), - metric=self._metric, + metric="manhattan" if self._connectivity == 4 else self._metric, animate=animate, summary=summary, ) @@ -316,6 +337,8 @@ def next(self, position: ArrayLike2) -> NDArray | None: ], dtype=int, ) + if self._connectivity == 4: + directions = directions[np.abs(directions).sum(axis=1) == 1] assert self.occgrid is not None and self._goal is not None # nosec B101: narrows the type for mypy self.validate_endpoint(position) diff --git a/tests/test_mobile_planners_a.py b/tests/test_mobile_planners_a.py index 66781aced..635141c19 100644 --- a/tests/test_mobile_planners_a.py +++ b/tests/test_mobile_planners_a.py @@ -13,16 +13,60 @@ import matplotlib.pyplot as plt import numpy as np import numpy.testing as nt +import pytest from roboticstoolbox import ( CurvaturePolyPlanner, DistanceTransformPlanner, + NoPathError, PRMPlanner, QuinticPolyPlanner, ) from roboticstoolbox.mobile.DistanceTransformPlanner import distancexform +@pytest.mark.parametrize("metric", ["euclidean", "manhattan", "cityblock", "MANHATTAN"]) +def test_four_connected_queries_use_axial_steps(metric: str) -> None: + planner = DistanceTransformPlanner(np.zeros((5, 5)), metric=metric, connectivity=4) + planner.plan((4, 3)) + + path = planner.query((0, 0)) + + nt.assert_array_equal(np.abs(np.diff(path, axis=0)).sum(axis=1), 1) + nt.assert_equal(len(path) - 1, 7) + nt.assert_equal(len(path) - 1, planner.distancemap[0, 0]) + nt.assert_equal(planner.connectivity, 4) + + +@pytest.mark.parametrize("metric", ["euclidean", "manhattan", "cityblock"]) +def test_default_queries_keep_diagonal_steps(metric: str) -> None: + planner = DistanceTransformPlanner(np.zeros((5, 5)), metric=metric) + planner.plan((4, 4)) + + path = planner.query((0, 0)) + + nt.assert_array_equal(np.diff(path, axis=0), np.ones((4, 2))) + nt.assert_equal(planner.connectivity, 8) + + +@pytest.mark.parametrize("metric", ["euclidean", "manhattan"]) +def test_four_connected_transform_does_not_cross_diagonal_gaps(metric: str) -> None: + grid = np.ones((3, 3)) + np.fill_diagonal(grid, 0) + planner = DistanceTransformPlanner(grid, metric=metric, connectivity=4) + planner.plan((2, 2)) + + nt.assert_equal(planner.distancemap[0, 0], np.inf) + with pytest.raises(NoPathError): + planner.query((0, 0)) + + +@pytest.mark.parametrize("connectivity", [True, 0, 3, 4.0, "4", None]) +def test_invalid_connectivity_is_rejected(connectivity: object) -> None: + with pytest.raises(ValueError, match="connectivity"): + DistanceTransformPlanner(np.zeros((5, 5)), connectivity=connectivity) + + def quiet(func, *args, **kwargs): """Call ``func`` returning what it returned and what it printed""" out = io.StringIO()