Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 20 additions & 15 deletions src/roboticstoolbox/mobile/PoseGraph.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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}"
)

Expand Down Expand Up @@ -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

Expand Down
124 changes: 124 additions & 0 deletions tests/test_posegraph_lidar_records.py
Original file line number Diff line number Diff line change
@@ -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
Loading