diff --git a/src/roboticstoolbox/mobile/PoseGraph.py b/src/roboticstoolbox/mobile/PoseGraph.py index 3114a9820..b3b934276 100644 --- a/src/roboticstoolbox/mobile/PoseGraph.py +++ b/src/roboticstoolbox/mobile/PoseGraph.py @@ -218,6 +218,7 @@ def __init__( # all the vertices in order of index self._vertices: list[PGVertex] = [] firstlidar = True + first_nbeams = 0 v: PGVertex | None = None for line_number, line in enumerate(f, start=1): # for zip file, we get data as bytes not str @@ -321,28 +322,30 @@ def __init__( # lidar records are associated with the immediately # preceding VERTEX record # - # not quite sure what all the fields are - # 1 ? + # 1 laser type # 2 min scan angle # 3 scan range # 4 angular increment # 5 maximum range possible - # ? + # 6 range accuracy, 7 remission mode # 8 N = number of beams - # 9 to 9+N lidar range data - # ? - # 9+N+12 timestamp (*nix timestamp) - # 9+N+13 lidar type (str) + # 9 to 9+N-1 lidar range data + # 9+N M = number of remission values, followed by M values + # 10+N+M two poses, velocities and safety fields (11 values) + # 21+N+M acquisition timestamp, then host and logger timestamp + nbeams = int(tokens[8]) + nremissions = int(tokens[9 + nbeams]) if firstlidar: - nbeams = int(tokens[8]) lidarmeta = tokens[2:6] + first_nbeams = nbeams firstlidar = False # add attributes to the most recent vertex created v.theta = np.arange(0, nbeams) * float(tokens[4]) + float(tokens[2]) v.range = np.array([float(x) for x in tokens[9 : nbeams + 9]]) - v.time = float(tokens[21 + nbeams]) + v.maxrange = float(tokens[5]) + v.time = float(tokens[21 + nbeams + nremissions]) self.vindex[v.index] = v nlidar += 1 else: @@ -368,7 +371,8 @@ def __init__( if verbose: fov = np.degrees([self._angmin, self._angmax]) print( - f" {nlidar} lidar scans: {nbeams} beams, fov {fov[0]:.1f}° to" + f" {nlidar} lidar scans; first scan: {first_nbeams} beams," + f" fov {fov[0]:.1f}° to" f" {fov[1]:.1f}°, max range {self._maxrange}" ) @@ -416,15 +420,16 @@ def scanxy(self, i: int) -> NDArray: :raises IndexError: there is no such vertex :raises ValueError: the vertex has no lidar scan :return: points, one per column, NaN for beams that returned no - range less than the maximum range of the sensor + range less than the sensor's maximum range for that scan :rtype: ndarray(2,N) .. seealso:: :meth:`scan` """ - range, theta = self.scan(i) - range = np.where(range < self._maxrange, range, np.nan) - x = range * np.cos(theta) - y = range * np.sin(theta) + vertex = self._scan_vertex(i) + ranges, theta = vertex.range, vertex.theta + ranges = np.where(ranges < vertex.maxrange, ranges, np.nan) + x = ranges * np.cos(theta) + y = ranges * np.sin(theta) return np.c_[x, y].T diff --git a/tests/test_posegraph_lidar_records.py b/tests/test_posegraph_lidar_records.py new file mode 100644 index 000000000..5fb5501a3 --- /dev/null +++ b/tests/test_posegraph_lidar_records.py @@ -0,0 +1,124 @@ +"""Lidar fields belong to their own ROBOTLASER1 record.""" + +import zipfile +from pathlib import Path + +import numpy as np +import pytest + +from roboticstoolbox import PoseGraph + + +def laser_record( + ranges: list[float], + timestamp: float, + *, + maxrange: float = 10, + angle: float = -1, + step: float = 0.5, + remissions: tuple[float, ...] = (), +) -> str: + # g2o RobotLaser: configuration, ranges, remissions, two poses, + # velocities/safety fields, acquisition timestamp, host, logger timestamp. + fields = [ + "ROBOTLASER1", + 0, + angle, + step * (len(ranges) - 1), + step, + maxrange, + 0.01, + int(bool(remissions)), + len(ranges), + *ranges, + len(remissions), + *remissions, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + timestamp, + "sensor", + timestamp + 1, + ] + return " ".join(map(str, fields)) + "\n" + + +def load_records(tmp_path: Path, records: list[str], zipped: bool = False) -> PoseGraph: + contents = "".join( + f"VERTEX_SE2 {i} {i} 0 0\n{record}" for i, record in enumerate(records) + ) + path = tmp_path / "records.g2o" + if zipped: + path = path.with_suffix(".g2o.zip") + with zipfile.ZipFile(path, "w") as archive: + archive.writestr("records.g2o", contents) + else: + path.write_text(contents) + return PoseGraph(str(path), lidar=True) + + +@pytest.mark.parametrize("zipped", [False, True]) +@pytest.mark.parametrize("counts", [(2, 4), (4, 2), (2, 0)]) +def test_each_scan_uses_its_declared_beam_count( + tmp_path: Path, zipped: bool, counts: tuple[int, int] +) -> None: + ranges = [[float(i + 1) for i in range(n)] for n in counts] + graph = load_records( + tmp_path, + [laser_record(r, 100 + i, angle=0.1 * i) for i, r in enumerate(ranges)], + zipped, + ) + + for i, expected in enumerate(ranges): + values, angles = graph.scan(i) + np.testing.assert_array_equal(values, expected) + np.testing.assert_allclose(angles, 0.1 * i + 0.5 * np.arange(len(expected))) + assert graph.scanxy(i).shape == (2, len(expected)) + assert graph.time(i) == 100 + i + + +@pytest.mark.parametrize("remissions", [(), (0.2, 0.4, 0.6)]) +def test_timestamp_follows_the_remission_array( + tmp_path: Path, remissions: tuple[float, ...] +) -> None: + graph = load_records( + tmp_path, [laser_record([1, 2, 3], 123.5, remissions=remissions)] + ) + + np.testing.assert_array_equal(graph.scan(0)[0], [1, 2, 3]) + assert graph.time(0) == 123.5 + + +@pytest.mark.parametrize("limits", [(5, 10), (10, 5)]) +def test_scanxy_uses_the_maximum_range_from_its_record( + tmp_path: Path, limits: tuple[int, int] +) -> None: + graph = load_records( + tmp_path, + [laser_record([4, 5, 7, 10], i, maxrange=m) for i, m in enumerate(limits)], + ) + + for i, maximum in enumerate(limits): + ranges, angles = graph.scan(i) + expected = np.array([ranges * np.cos(angles), ranges * np.sin(angles)]) + expected[:, ranges >= maximum] = np.nan + np.testing.assert_allclose(graph.scanxy(i), expected, equal_nan=True) + + +def test_uniform_records_keep_the_existing_results(tmp_path: Path) -> None: + graph = load_records( + tmp_path, [laser_record([3, 4], 100), laser_record([3, 4], 101)] + ) + + for i in range(2): + np.testing.assert_array_equal(graph.scan(i)[0], [3, 4]) + np.testing.assert_array_equal(graph.scan(i)[1], [-1, -0.5]) + assert graph.time(i) == 100 + i