def test_incompatible_profile(lidar_stream: client.PacketSource) -> None:
"""Test batching of a LidarScan with custom fields set."""
info = lidar_stream.metadata
assert info.format.udp_profile_lidar == client.UDPProfileLidar.PROFILE_LIDAR_LEGACY
packets_per_frame = (info.format.columns_per_frame //
info.format.columns_per_packet)
batch = ScanBatcher(lidar_stream.metadata)
ls = client.LidarScan(info.format.pixels_per_column,
info.format.columns_per_frame,
client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL)
# Try to decode a legacy packet into a dual returns scan
# TODO change exception thrown on the cpp side
with pytest.raises(IndexError):
for p in take(packets_per_frame, lidar_stream):
batch(p._data, ls)
batch = ScanBatcher(lidar_stream.metadata)
fields = {
client.ChanField.RANGE: np.uint8,
}
ls = client.LidarScan(info.format.pixels_per_column,
info.format.columns_per_frame,
fields)
# Test for decoding scans to a bad dest buffer type
with pytest.raises(ValueError):
for p in take(packets_per_frame, lidar_stream):
batch(p._data, ls)
def test_xyz_lut_scan_dims(stream_digest: digest.StreamDigest, meta: client.SensorInfo) -> None:
"""Check that (in)valid lidar scan dimensions are handled by xyzlut."""
w = meta.format.columns_per_frame
h = meta.format.pixels_per_column
xyzlut = client.XYZLut(meta)
assert xyzlut(client.LidarScan(h, w)).shape == (h, w, 3)
with pytest.raises(ValueError):
xyzlut(client.LidarScan(h + 1, w))
with pytest.raises(ValueError):
xyzlut(client.LidarScan(h, w - 1))
def test_scan_writeable() -> None:
"""Check that a native scan is a writeable view of data."""
ls = client.LidarScan(1024, 32)
assert not ls.field(client.ChanField.RANGE).flags.owndata
assert not ls.status.flags.owndata
assert ls.field(client.ChanField.SIGNAL).flags.aligned
assert ls.measurement_id.flags.aligned
assert ls.field(client.ChanField.NEAR_IR).flags.aligned
assert ls.timestamp.flags.aligned
ls.field(client.ChanField.RANGE)[0, 0] = 42
assert ls.field(client.ChanField.RANGE)[0, 0] == 42
ls.field(client.ChanField.RANGE)[:] = 7
assert np.all(ls.field(client.ChanField.RANGE) == 7)
ls.status[-1] = 0xffff
assert ls.status[-1] == 0xffff
assert ls.header(client.ColHeader.STATUS)[-1] == 0xffff
ls.header(client.ColHeader.STATUS)[-2] = 0xffff
assert ls.status[-2] == 0xffff
assert ls.header(client.ColHeader.STATUS)[-2] == 0xffff
ls.status[:] = 0x1
assert np.all(ls.status == 0x1)
assert np.all(ls.header(client.ColHeader.STATUS) == 0x1)
def test_batch_custom_fields(lidar_stream: client.PacketSource) -> None:
"""Test batching of a LidarScan with custom fields set."""
info = lidar_stream.metadata
packets_per_frame = (info.format.columns_per_frame //
info.format.columns_per_packet)
batch = ScanBatcher(lidar_stream.metadata)
# create LidarScan with only 2 fields
fields: Dict[client.ChanField, client.FieldDType] = {
client.ChanField.RANGE: np.uint32,
client.ChanField.SIGNAL: np.uint16
}
ls = client.LidarScan(info.format.pixels_per_column,
info.format.columns_per_frame, fields)
# we expect zero initialized fields
for f in ls.fields:
assert np.count_nonzero(ls.field(f)) == 0
# do batching into ls with a fields subset
for p in take(packets_per_frame, lidar_stream):
batch(p._data, ls)
# it should contain the same num fields as we've added
assert len(list(ls.fields)) == len(fields)
# and the content shouldn't be zero after batching
for f in ls.fields:
assert np.count_nonzero(ls.field(f)) > 0
def test_scan_copy_eq() -> None:
"""Test equality with a copy."""
ls0 = client.LidarScan(32, 512)
ls0.status[:] = 0x1
ls0.field(client.ChanField.SIGNAL)[:] = 100
ls1 = deepcopy(ls0)
assert ls0 is not ls1
assert ls0 == ls1
ls0.frame_id = 9
assert ls0 != ls1
ls1.frame_id = 9
assert ls0 == ls1
ls0.measurement_id[0] = 1
assert ls0 != ls1
ls1.measurement_id[0] = 1
assert ls0 == ls1
ls0.field(client.ChanField.RANGE)[0, 0] = 42
assert ls0 != ls1
ls1.field(client.ChanField.RANGE)[0, 0] = 42
assert ls0 == ls1
def test_scan_empty() -> None:
"""Sanity check scan with no fields."""
ls = client.LidarScan(32, 1024, {})
assert set(ls.fields) == set()
for f in client.ChanField.values:
with pytest.raises(ValueError):
ls.field(f)
def test_scan_fields_ref() -> None:
"""Make sure ref to fields keeps scan alive."""
fields = client.LidarScan(32, 1024).fields
# should fail (or trip asan) if the field iterator doesn't keep scan alive
assert set(fields) == {
client.ChanField.RANGE,
client.ChanField.REFLECTIVITY,
client.ChanField.SIGNAL,
client.ChanField.NEAR_IR,
}
def _simple_scans(source: client.PacketSource) -> Iterator[client.LidarScan]:
"""Batch packets to scans without zeroing between frames."""
batch = ScanBatcher(source.metadata)
info = source.metadata
ls = client.LidarScan(info.format.pixels_per_column,
info.format.columns_per_frame,
info.format.udp_profile_lidar)
for p in source:
if batch(p._data, ls):
yield deepcopy(ls)
def test_scan_field_ref() -> None:
"""Test that field references keep scans alive."""
ls = client.LidarScan(512, 16)
range = ls.field(client.ChanField.RANGE)
range[:] = 42
del ls
assert np.all(range == 42)
range[:] = 43
assert np.all(range == 43)
def test_scan_header_ref() -> None:
"""Test that header references keep scans alive."""
ls = client.LidarScan(512, 16)
status = ls.status
status[:] = 0x11
del ls
assert np.all(status == 0x11)
status[:] = 0x01
assert np.all(status == 0x01)
def test_scan_custom() -> None:
"""Sanity check scan with a custom set of fields."""
ls = client.LidarScan(32, 1024, {
client.ChanField.SIGNAL: np.uint16,
client.ChanField.FLAGS: np.uint8
})
assert set(ls.fields) == {client.ChanField.SIGNAL, client.ChanField.FLAGS}
assert ls.field(client.ChanField.SIGNAL).dtype == np.uint16
with pytest.raises(ValueError):
ls.field(client.ChanField.RANGE)
def test_scan_to_native() -> None:
"""Check that converting to a native scan copies data."""
ls = client.LidarScan(32, 1024)
ls._data[:] = np.arange(ls._data.size).reshape(N_FIELDS, -1)
native = ls.to_native()
assert ls._data.base is not native.data.base
assert np.array_equal(ls._data, native.data)
ls._data[0, 0] = 42
native.data[:] = 1
assert ls._data[0, 0] == 42
def test_scan_default_fields() -> None:
"""Default scan has the expected fields for the LEGACY profile."""
ls = client.LidarScan(32, 1024)
assert set(ls.fields) == {
client.ChanField.RANGE,
client.ChanField.REFLECTIVITY,
client.ChanField.SIGNAL,
client.ChanField.NEAR_IR,
}
for f in ls.fields:
assert ls.field(f).dtype == np.uint32
def test_scan_complete(w, win_start, win_end) -> None:
"""Set the status headers to the specified window and check _complete()."""
ls = client.LidarScan(32, w)
status = ls.status
if win_start <= win_end:
status[win_start:win_end + 1] = 0xFFFFFFFF
else:
status[0:win_end + 1] = 0xFFFFFFFF
status[win_start:] = 0xFFFFFFFF
assert ls._complete((win_start, win_end))
def test_scan_zero_init() -> None:
"""Test that scan fields and headers are zero initialized."""
ls = client.LidarScan(
64, 1024,
client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL)
assert ls.frame_id == -1
assert np.count_nonzero(ls.timestamp) == 0
assert np.count_nonzero(ls.measurement_id) == 0
assert np.count_nonzero(ls.status) == 0
for f in ls.fields:
assert np.count_nonzero(ls.field(f)) == 0
def test_scan_dual_profile() -> None:
"""Dual returns scan has the expected fields."""
ls = client.LidarScan(
32, 1024,
client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL)
assert set(ls.fields) == {
client.ChanField.RANGE,
client.ChanField.RANGE2,
client.ChanField.REFLECTIVITY,
client.ChanField.REFLECTIVITY2,
client.ChanField.SIGNAL,
client.ChanField.SIGNAL2,
client.ChanField.NEAR_IR,
}
def test_batch_missing_zeroed(lidar_stream: client.PacketSource) -> None:
"""Check that missing data is zeroed out when batching."""
info = lidar_stream.metadata
packets_per_frame = (info.format.columns_per_frame //
info.format.columns_per_packet)
batch = ScanBatcher(lidar_stream.metadata)
ls = client.LidarScan(info.format.pixels_per_column,
info.format.columns_per_frame,
info.format.udp_profile_lidar)
# initialize fields and headers with nonzero values
ls.timestamp[:] = 1
ls.measurement_id[:] = 1
ls.status[:] = 1
for f in ls.fields:
ls.field(f)[:] = 1
# packet indices to drop
drop_inds = [10, 20]
# drop some packets
packets = list(take(packets_per_frame, lidar_stream))
for i in drop_inds:
del packets[i]
# parse the packets into a scan
for p in packets:
batch(p._data, ls)
# check that data associated with the dropped packets is zeroed
n = info.format.columns_per_packet
assert np.count_nonzero(
ls.status) == info.format.columns_per_frame - n * len(drop_inds)
for i in drop_inds:
assert (ls.timestamp[i * n:n] == 0).all()
assert (ls.measurement_id[i * n:n] == 0).all()
assert (ls.status[i * n:n] == 0).all()
for f in ls.fields:
assert (ls.field(f)[:, i * n:n] == 0).all()
def test_scan_not_complete() -> None:
"""Test that not all scans are considered complete."""
ls = client.LidarScan(32, 1024)
status = ls.status
assert not ls._complete()
status[0] = 0x02
assert not ls._complete()
assert not ls._complete((0, 0))
status[1:] = 0xFFFFFFFF
assert not ls._complete()
status[:] = 0xFFFFFFFF
status[-1] = 0x02
assert not ls._complete()
# windows are inclusive but python slicing is not
status[:] = 0x00
status[:10] = 0xFFFFFFFF
assert not ls._complete((0, 10))
status[:] = 0x00
status[11:21] = 0xFFFFFFFF
assert not ls._complete((10, 20))
# window [i, i]
status[:] = 0x00
status[0] = 0xFFFFFFFF
assert not ls._complete()
assert not ls._complete((0, 1))
assert ls._complete((0, 0))
status[:] = 0x00
status[128] = 0xFFFFFFFF
assert not ls._complete()
assert not ls._complete((127, 128))
assert ls._complete((128, 128))
def test_scan_eq_fields() -> None:
"""Test equality between scans with different fields."""
ls0 = client.LidarScan(32, 1024)
ls1 = client.LidarScan(32, 1024,
client.UDPProfileLidar.PROFILE_LIDAR_LEGACY)
ls2 = client.LidarScan(
32, 1024,
client.UDPProfileLidar.PROFILE_LIDAR_RNG19_RFL8_SIG16_NIR16_DUAL)
ls3 = client.LidarScan(32, 1024, {client.ChanField.SIGNAL: np.uint32})
ls4 = client.LidarScan(32, 1024, {client.ChanField.SIGNAL: np.uint16})
ls5 = client.LidarScan(32, 1024, {})
assert ls0 == ls1
assert not (ls0 != ls1) # should be implemented using __eq__
assert ls1 != ls2
assert ls3 != ls4
assert ls5 != ls0
assert ls5 != ls2
assert ls5 != ls4
def test_scan_from_native() -> None:
ls = client.LidarScan(1024, 32)
ls2 = client.LidarScan.from_native(ls)
assert ls is ls2
def test_scan_to_native() -> None:
ls = client.LidarScan(1024, 32)
ls2 = ls.to_native()
assert ls is ls2
