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'))