feat(mobile): animate range-bearing sensor beam and sensing region

src/roboticstoolbox/mobile/sensors.py

@@ -6,7 +6,11 @@
 from math import cos, pi, sin
 from typing import Any
 
+import matplotlib.pyplot as plt
 import numpy as np
+from matplotlib.axes import Axes
+from matplotlib.lines import Line2D
+from matplotlib.patches import Polygon
 from spatialmath import base
 
 from roboticstoolbox.tools.types import ArrayLike, NDArray
@@ -276,8 +280,9 @@
         from the robot to the landmark observed each time :meth:`reading` is
         called.
 
-        .. note:: ``line_style`` and ``poly_style`` are accepted but are not
-            currently used.
+        ``line_style`` styles the landmark beam and ``poly_style`` styles the
+        sensing region when ``plot=True``. The region is shown when both range
+        and angle limits are supplied.
 
         .. runblock:: pycon
 
@@ -339,6 +344,22 @@
         self._landmarklog: list[int] = []
 
         self._random = np.random.default_rng(seed)
+        self._plot_ax: Axes | None = None
+        self._plot_line: Line2D | None = None
+        self._plot_poly: Polygon | None = None
+        if self._animate:
+            self._init_plot()
+
+    def _init_plot(self) -> None:
+        """Create map axes and discard previous sensor artists."""
+        if self._plot_line is not None and self._plot_line.axes is not None:
+            self._plot_line.remove()
+        if self._plot_poly is not None and self._plot_poly.axes is not None:
+            self._plot_poly.remove()
+        self.map.plot()
+        self._plot_ax = plt.gca()
+        self._plot_line = None
+        self._plot_poly = None
 
     def __str__(self) -> str:
         s = super().__str__()
@@ -366,7 +387,7 @@
         self._landmarklog = []
 
         if self._animate:
-            self.map.plot()
+            self._init_plot()
 
     @property
     def W(self) -> NDArray:
@@ -432,38 +453,20 @@
         lm_id = -1
         # sample interval
         if self._count % self._every != 0:
+            if self._animate:
+                self.plot(None)
             self._landmarklog.append(lm_id)
             return (None, None)
 
         # simulated failure, fail is a list of 2-tuples giving (start,end) times
         # for a sensor failure
         if self._fail is not None:
             if any([start <= self._count < end for start, end in self._fail]):
+                if self._animate:
+                    self.plot(None)
                 self._landmarklog.append(lm_id)
                 return (None, None)
 
-        # create a polygon to indicate the active sensing area based on range+angle limits
-        # if self.animate && ~isempty(self.theta_range) && ~isempty(self.r_range)
-        #     h = findobj(gca, 'tag', 'sensor-area')
-        #     if isempty(h)
-
-        #         th=linspace(self.theta_range[0], self.theta_range[1], 20)
-        #         x = self.r_range[1] * cos(th)
-        #         y = self.r_range[1] * sin(th)
-        #         if self.r_range[0] > 0
-        #             th = flip(th)
-        #             x = [x self.r_range[0] * cos(th)]
-        #             y = [y self.r_range[0] * sin(th)]
-        #         else
-        #             x = [x 0]
-        #             y = [y 0]
-        #         end
-        #         # no sensor zone, create one
-        #         plot_poly([x; y], 'fillcolor', 'r', 'alpha', 0.1, 'edgecolor', 'none', 'animate', 'tag', 'sensor-area')
-        #     else
-        #         hg = get(h, 'Parent')
-        #         plot_poly(h, self.robot.x)
-
         zk = self.visible()
         if len(zk) > 1:
             # more than 1 visible landmark, pick a random one
@@ -481,6 +484,8 @@
         else:
             if self.verbose:
                 print("Sensor:: no features\n")
+            if self._animate:
+                self.plot(None)
             self._landmarklog.append(lm_id)
             return (None, None)
 
@@ -498,6 +503,56 @@
 
         return z, lm_id
 
+    def plot(self, id: int | None) -> None:
+        """Update the sensing region and the beam to the observed landmark.
+
+        :param id: landmark index, or None if no landmark is observed
+        """
+        if self._plot_ax is None:
+            self._init_plot()
+        ax = self._plot_ax
+        if ax is None:
+            return  # pragma: no cover
+
+        pose = self.robot.x
+        if self._r_range is not None and self._theta_range is not None:
+            theta = np.linspace(
+                float(self._theta_range[0]), float(self._theta_range[1]), 40
+            ) + pose[2]
+            direction = np.column_stack((np.cos(theta), np.sin(theta)))
+            outer = self._r_range[1] * direction
+            if self._r_range[0] > 0:
+                inner = self._r_range[0] * direction[::-1]
+            else:
+                inner = np.zeros((1, 2))
+            vertices = np.vstack((outer, inner)) + pose[:2]
+            if self._plot_poly is None:
+                poly_style: dict[str, Any] = {
+                    "facecolor": "red",
+                    "alpha": 0.1,
+                    "edgecolor": "none",
+                }
+                poly_style.update(self._poly_style or {})
+                self._plot_poly = Polygon(vertices, closed=True, **poly_style)
+                ax.add_patch(self._plot_poly)
+            else:
+                self._plot_poly.set_xy(vertices)
+
+        if self._plot_line is None:
+            line_style: dict[str, Any] = {"color": "red", "linewidth": 1}
+            line_style.update(self._line_style or {})
+            (self._plot_line,) = ax.plot([], [], **line_style)
+
+        if id is None:
+            self._plot_line.set_visible(False)
+        else:
+            landmark = self.map[id]
+            self._plot_line.set_data(
+                [pose[0], landmark[0]], [pose[1], landmark[1]]
+            )
+            self._plot_line.set_visible(True)
+        ax.figure.canvas.draw_idle()
+
     def visible(self) -> list[tuple[NDArray, int]]:
         """
         List of all visible landmarks

tests/test_sensor_plot.py

@@ -0,0 +1,109 @@
+"""Headless regression tests for range-bearing sensor visualization."""
+
+import matplotlib
+
+matplotlib.use("Agg")
+import matplotlib.pyplot as plt
+import numpy as np
+import numpy.testing as nt
+
+from roboticstoolbox import Bicycle, LandmarkMap, RangeBearingSensor
+
+
+def test_sensor_beam_and_sector_move_without_duplicate_artists() -> None:
+    robot = Bicycle(x0=[0.0, 0.0, 0.0])
+    landmarks = LandmarkMap(np.array([[2.0], [0.0]]))
+    sensor = RangeBearingSensor(
+        robot,
+        landmarks,
+        range=(0.5, 3.0),
+        angle=np.pi / 4,
+        plot=True,
+        line_style={"color": "blue", "linewidth": 2},
+        poly_style={"facecolor": "green", "alpha": 0.25},
+    )
+    try:
+        z, idx = sensor.reading()
+        assert idx == 0
+        assert z is not None
+        ax = sensor._plot_ax
+        line, poly = sensor._plot_line, sensor._plot_poly
+        assert ax is not None
+        assert line in ax.lines
+        assert poly in ax.patches
+        assert line.get_color() == "blue"
+        assert line.get_linewidth() == 2
+        assert poly.get_alpha() == 0.25
+        original_vertices = poly.get_xy().copy()
+        nt.assert_allclose(line.get_xdata(), [0, 2])
+        nt.assert_allclose(line.get_ydata(), [0, 0])
+
+        robot._x = np.array([0.4, -0.2, 0.3])
+        sensor.plot(0)
+        assert sensor._plot_line is line
+        assert sensor._plot_poly is poly
+        assert len(ax.patches) == 1
+        assert line.get_visible()
+        nt.assert_allclose(line.get_xdata(), [0.4, 2.0])
+        nt.assert_allclose(line.get_ydata(), [-0.2, 0.0])
+        assert not np.allclose(poly.get_xy(), original_vertices)
+
+        sensor.plot(None)
+        assert not line.get_visible()
+        assert len(ax.patches) == 1
+    finally:
+        plt.close("all")
+
+
+def test_no_observation_hides_beam_and_keeps_sector() -> None:
+    robot = Bicycle(x0=[0.0, 0.0, 0.0])
+    landmarks = LandmarkMap(np.array([[2.0], [0.0]]))
+    sensor = RangeBearingSensor(
+        robot, landmarks, range=3.0, angle=np.pi / 4, plot=True, every=2
+    )
+    try:
+        assert sensor.reading() == (None, None)
+        assert sensor._plot_line is not None
+        assert not sensor._plot_line.get_visible()
+        assert sensor._plot_poly is not None
+        z, idx = sensor.reading()
+        assert z is not None and idx == 0
+        assert sensor._plot_line.get_visible()
+        robot._x = np.array([0.0, 0.0, np.pi])
+        assert sensor.reading() == (None, None)
+        assert not sensor._plot_line.get_visible()
+    finally:
+        plt.close("all")
+
+
+def test_sensor_init_recreates_artists() -> None:
+    robot = Bicycle(x0=[0, 0, 0])
+    sensor = RangeBearingSensor(
+        robot, LandmarkMap(np.array([[2.0], [0.0]])), range=3, angle=1, plot=True
+    )
+    try:
+        sensor.plot(0)
+        old_line, old_poly = sensor._plot_line, sensor._plot_poly
+        ax = sensor._plot_ax
+        sensor.init()
+        assert old_line not in ax.lines
+        assert old_poly not in ax.patches
+        sensor.plot(0)
+        assert sensor._plot_line is not old_line
+        assert sensor._plot_poly is not old_poly
+    finally:
+        plt.close("all")
+
+
+def test_sensor_without_limits_draws_beam_only() -> None:
+    robot = Bicycle(x0=[0, 0, 0])
+    sensor = RangeBearingSensor(
+        robot, LandmarkMap(np.array([[2.0], [0.0]])), plot=True
+    )
+    try:
+        z, idx = sensor.reading()
+        assert z is not None and idx == 0
+        assert sensor._plot_line is not None
+        assert sensor._plot_poly is None
+    finally:
+        plt.close("all")