fix(mobile): add opt-in four-connected path queries

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)

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