From 095193193fcbed3000ef67b32108cefacd9edca6 Mon Sep 17 00:00:00 2001 From: Afloat <132809511+Afloat16@users.noreply.github.com> Date: Tue, 3 Nov 2026 08:12:52 -0500 Subject: [PATCH 1/2] fix(mobile): respect Manhattan connectivity in path queries --- .../mobile/DistanceTransformPlanner.py | 7 +++++-- tests/test_mobile_planners_a.py | 21 +++++++++++++++++++ 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/roboticstoolbox/mobile/DistanceTransformPlanner.py b/src/roboticstoolbox/mobile/DistanceTransformPlanner.py index 99c2b21f4..5b378b530 100644 --- a/src/roboticstoolbox/mobile/DistanceTransformPlanner.py +++ b/src/roboticstoolbox/mobile/DistanceTransformPlanner.py @@ -53,8 +53,9 @@ 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 4-way connected points for the Manhattan metric, or + 8-way connected points for the Euclidean 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. @@ -316,6 +317,8 @@ def next(self, position: ArrayLike2) -> NDArray | None: ], dtype=int, ) + if self.metric.lower() in ("manhattan", "cityblock"): + 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..0d501a97e 100644 --- a/tests/test_mobile_planners_a.py +++ b/tests/test_mobile_planners_a.py @@ -13,6 +13,7 @@ import matplotlib.pyplot as plt import numpy as np import numpy.testing as nt +import pytest from roboticstoolbox import ( CurvaturePolyPlanner, @@ -23,6 +24,26 @@ from roboticstoolbox.mobile.DistanceTransformPlanner import distancexform +@pytest.mark.parametrize("metric", ["manhattan", "cityblock", "MANHATTAN"]) +def test_manhattan_queries_use_four_connected_steps(metric: str) -> None: + planner = DistanceTransformPlanner(np.zeros((5, 5)), metric=metric) + 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, planner.distancemap[0, 0]) + + +def test_euclidean_queries_keep_diagonal_steps() -> None: + planner = DistanceTransformPlanner(np.zeros((5, 5)), metric="euclidean") + planner.plan((4, 4)) + + path = planner.query((0, 0)) + + nt.assert_array_equal(np.diff(path, axis=0), np.ones((4, 2))) + + def quiet(func, *args, **kwargs): """Call ``func`` returning what it returned and what it printed""" out = io.StringIO() From d282e72cd7e7af645030068718ca25a8f3ffb127 Mon Sep 17 00:00:00 2001 From: Afloat <132809511+Afloat16@users.noreply.github.com> Date: Tue, 3 Nov 2026 10:29:19 -0500 Subject: [PATCH 2/2] fix(mobile): preserve default paths with opt-in connectivity --- .../mobile/DistanceTransformPlanner.py | 32 ++++++++++++++---- tests/test_mobile_planners_a.py | 33 ++++++++++++++++--- 2 files changed, 54 insertions(+), 11 deletions(-) diff --git a/src/roboticstoolbox/mobile/DistanceTransformPlanner.py b/src/roboticstoolbox/mobile/DistanceTransformPlanner.py index 5b378b530..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,9 +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-way connected points for the Manhattan metric, or - 8-way connected points for the Euclidean metric. 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. @@ -91,6 +95,7 @@ def __init__( metric: str = "euclidean", *, distance: str | None = None, + connectivity: int = 8, **kwargs: Any, ) -> None: if distance is not None: @@ -106,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 @@ -120,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: """ @@ -189,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, ) @@ -317,7 +337,7 @@ def next(self, position: ArrayLike2) -> NDArray | None: ], dtype=int, ) - if self.metric.lower() in ("manhattan", "cityblock"): + 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 diff --git a/tests/test_mobile_planners_a.py b/tests/test_mobile_planners_a.py index 0d501a97e..635141c19 100644 --- a/tests/test_mobile_planners_a.py +++ b/tests/test_mobile_planners_a.py @@ -18,30 +18,53 @@ from roboticstoolbox import ( CurvaturePolyPlanner, DistanceTransformPlanner, + NoPathError, PRMPlanner, QuinticPolyPlanner, ) from roboticstoolbox.mobile.DistanceTransformPlanner import distancexform -@pytest.mark.parametrize("metric", ["manhattan", "cityblock", "MANHATTAN"]) -def test_manhattan_queries_use_four_connected_steps(metric: str) -> None: - planner = DistanceTransformPlanner(np.zeros((5, 5)), metric=metric) +@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) -def test_euclidean_queries_keep_diagonal_steps() -> None: - planner = DistanceTransformPlanner(np.zeros((5, 5)), metric="euclidean") +@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):