def test_scans_first_packet(packet: client.LidarPacket,
packets: client.PacketSource) -> None:
"""Check that data in the first packet survives batching to a scan."""
scans = iter(client.Scans(packets))
scan = next(scans)
h = packet._pf.pixels_per_column
w = packet._pf.columns_per_packet
assert np.array_equal(packet.field(ChanField.RANGE),
scan.field(ChanField.RANGE)[:h, :w])
assert np.array_equal(packet.field(ChanField.REFLECTIVITY),
scan.field(ChanField.REFLECTIVITY)[:h, :w])
assert np.array_equal(packet.field(client.ChanField.SIGNAL),
scan.field(client.ChanField.SIGNAL)[:h, :w])
assert np.array_equal(packet.field(client.ChanField.NEAR_IR),
scan.field(client.ChanField.NEAR_IR)[:h, :w])
assert packet.frame_id == scan.frame_id
assert np.array_equal(packet.timestamp, scan.timestamp[:w])
assert np.array_equal(packet.header(ColHeader.ENCODER_COUNT),
scan.header(ColHeader.ENCODER_COUNT)[:w])
assert np.array_equal(packet.status, scan.status[:w])
def __iter__(self) -> Iterator[Packet]:
buf = bytearray(2**16)
packet_info = _pcap.packet_info()
real_start_ts = time.monotonic()
pcap_start_ts = None
while True:
with self._lock:
if self._handle is None:
break
if not _pcap.next_packet_info(self._handle, packet_info):
break
n = _pcap.read_packet(self._handle, buf)
if self._rate:
if not pcap_start_ts:
pcap_start_ts = packet_info.timestamp
real_delta = time.monotonic() - real_start_ts
pcap_delta = (packet_info.timestamp -
pcap_start_ts) / self._rate
delta = max(0, pcap_delta.total_seconds() - real_delta)
time.sleep(delta)
if packet_info.dst_port == self._lidar_port and n != 0:
yield LidarPacket(buf[0:n], self._metadata)
elif packet_info.dst_port == self._imu_port and n != 0:
yield ImuPacket(buf[0:n], self._metadata)
def __iter__(self) -> Iterator[Packet]:
with self._lock:
if self._handle is None:
raise ValueError("I/O operation on closed packet source")
buf = bytearray(2**16)
packet_info = _pcap.packet_info()
real_start_ts = time.monotonic()
pcap_start_ts = None
metadata = self._metadata
while True:
with self._lock:
if (self._handle is None or
not _pcap.next_packet_info(self._handle, packet_info)):
break
timestamp = packet_info.timestamp
n = _pcap.read_packet(self._handle, buf)
if self._rate:
if not pcap_start_ts:
pcap_start_ts = packet_info.timestamp
real_delta = time.monotonic() - real_start_ts
pcap_delta = (packet_info.timestamp -
pcap_start_ts) / self._rate
delta = max(0, pcap_delta - real_delta)
time.sleep(delta)
if packet_info.dst_port == self._lidar_port and n != 0:
yield LidarPacket(buf[0:n], metadata, timestamp)
elif packet_info.dst_port == self._imu_port and n != 0:
yield ImuPacket(buf[0:n], metadata, timestamp)
def test_read_real_packet(packet: client.LidarPacket) -> None:
"""Read some arbitrary values from a packet and check header invariants."""
assert packet.field(client.ChanField.RANGE)[0, 0] == 1723
assert packet.field(client.ChanField.REFLECTIVITY)[0, 0] == 196
assert packet.field(client.ChanField.SIGNAL)[0, 0] == 66
assert packet.field(client.ChanField.NEAR_IR)[0, 0] == 1768
assert np.all(np.diff(packet.header(client.ColHeader.FRAME_ID)) == 0)
assert np.all(np.diff(packet.header(client.ColHeader.MEASUREMENT_ID)) == 1)
# in 512xN mode, the angle between measurements is exactly 176 encoder ticks
assert np.all(
np.diff(packet.header(client.ColHeader.ENCODER_COUNT)) == 176)
assert np.all(packet.header(client.ColHeader.STATUS) == 0xffffffff)
def test_read_legacy_packet(packet: client.LidarPacket) -> None:
"""Read some arbitrary values from a packet and check header invariants."""
assert packet.field(client.ChanField.RANGE)[-1, 0] == 12099
assert packet.field(client.ChanField.REFLECTIVITY)[-1, 0] == 1017
assert packet.field(client.ChanField.SIGNAL)[-1, 0] == 6
assert packet.field(client.ChanField.NEAR_IR)[-1, 0] == 13
assert np.all(np.diff(packet.header(client.ColHeader.FRAME_ID)) == 0)
assert np.all(np.diff(packet.header(client.ColHeader.MEASUREMENT_ID)) == 1)
assert np.all(np.diff(packet.timestamp) > 0)
assert np.all(np.diff(packet.measurement_id) == 1)
assert packet.packet_type == 0
assert packet.frame_id == 5424
assert packet.init_id == 0
assert packet.prod_sn == 0
# in 1024xN mode, the angle between measurements is exactly 88 encoder ticks
assert np.all(np.diff(packet.header(client.ColHeader.ENCODER_COUNT)) == 88)
assert np.all(packet.status == 0xffffffff)
def __iter__(self) -> Iterator[Packet]:
with self._lock:
if self._handle is None:
raise ValueError("I/O operation on closed packet source")
buf = bytearray(2**16)
packet_info = _pcap.packet_info()
real_start_ts = time.monotonic()
pcap_start_ts = None
while True:
with self._lock:
if not (self._handle
and _pcap.next_packet_info(self._handle, packet_info)):
break
n = _pcap.read_packet(self._handle, buf)
# if rate is set, read in 'real time' simulating UDP stream
# TODO: factor out into separate packet iterator utility
timestamp = packet_info.timestamp
if self._rate:
if not pcap_start_ts:
pcap_start_ts = timestamp
real_delta = time.monotonic() - real_start_ts
pcap_delta = (timestamp - pcap_start_ts) / self._rate
delta = max(0, pcap_delta - real_delta)
time.sleep(delta)
try:
if (packet_info.dst_port == self._metadata.udp_port_lidar):
yield LidarPacket(buf[0:n], self._metadata, timestamp)
elif (packet_info.dst_port == self._metadata.udp_port_imu):
yield ImuPacket(buf[0:n], self._metadata, timestamp)
except ValueError:
# TODO: bad packet size or init_id here, use specific exceptions
pass
def _patch_frame_id(packet: client.LidarPacket, fid: int) -> None:
"""Rewrite the frame id of a non-legacy format lidar packet."""
packet._data[2:4] = memoryview(fid.to_bytes(2, byteorder='little'))