from ctypes import Structure, c_byte, c_char, c_int16, c_ubyte, c_uint32, c_uint16, sizeof, string_at from sys import argv import os UDP_STATE_IDLE = 0 UDP_STATE_HAND_SHAKE = 1 UDP_STATE_DATA = 2 UDP_STATE_FOOTER = 3 HAND_SHAKE_MAGIC = b"\x01\x09\x08\x09\x01\x00\x02\x02" HEADER_MAGIC = b"\x02\x01\x04\x03\x06\x05\x08\x07" class Handshake(Structure): _pack_ = 1 _fields_=[ ('Magic', c_ubyte*8), ('FrameDataLength', c_uint32), ('Reserved', c_ubyte * 12) ] class HeaderBlock(Structure): _pack_ = 1 _fields_ = [ ('Magic', c_ubyte * 8), # [2 1 4 3 6 5 8 7] ('FrameNumber', c_uint32), # frame number ('VersionNumber', c_uint32), # Version number, uint32_t format: MMddhhmm ('NumberOfDetection', c_uint16), # Number of Detection in the frame ('NumberOfTrack', c_uint16), # Number of Detection in the frame ('HostSpeed', c_uint16), # divide by 100 to get ego speed in m/s (e.g. 6453 = 64.53 m/s, e.g. -2456 = -24.56 m/s) ('HostAngle', c_uint16), # divide by 100 to get host angle in degree. Clockwise rotation corresponds to positive rotation.(e.g. 1400 = 14 deg) ('Reserved', c_ubyte * 8), ('RangeAccuracyIdx', c_uint16), # divide by 10000 to get value in m ('DopplerAccuracyIdx', c_uint16), # divide by 10000 to get value in m/s ('AzimuthAccuracyIdx', c_uint16), # divide by 10000 to get value in degree ('ElevationAccuracyIdx', c_uint16), # divide by 10000 to get value in degree ('DspWorkload', c_ubyte), ('ARMWorkload', c_ubyte), ('Reserved2', c_ubyte * 6), ] class FooterBlock(Structure): _pack_ = 1 _fields_ = [ ('Reserved1', c_ubyte * 8), ('RangeAccuracyIdx', c_uint16), # divide by 10000 to get value in m ('DopplerAccuracyIdx', c_uint16), # divide by 10000 to get value in m ('AzimuthAccuracyIdx', c_uint16), # divide by 10000 to get value in degree ('ElevationAccuracyIdx', c_uint16), # divide by 10000 to get value in degree ('Reserved2', c_ubyte * 16), ] class DetectionBlock(Structure): _pack_ = 1 _fields_ = [ ('data', c_ubyte * 8), # ('Reserved1', c_ubyte, 1), # ('Flag', c_ubyte, 1), # used for decoding. Source code provided in the SDK. # ('Reserved2', c_ubyte, 6), # ('PowerValue', c_uint16, 16), # 0 to 65535, Power value is in dB scale and represents the, signal to noise ratio of the detection. Divide by 100 to decode power. (e.g. 208 = 2.08dB) # ('BetaIndex', c_int16, 10), # multiply with Elevation Accuracy value to get Elevation (beta) value in degree. # ('AzimuthIndex', c_int16, 10), # multiply with Doppler Accuracy value to get Doppler value in m/s. # ('DropplerIndex', c_int16, 10), # multiply with Doppler Accuracy value to get Doppler value in m/s. # ('RangeIndex', c_uint16, 10), # multiply with Range Accuracy value to get Range value in m. ] # XYZ: The XYZ coordinates can be retrieved using the following equation: # X = Range * Sin(Alpha) * Cos(Beta) # Y = Range * Sin(Beta) # Z = Range * Cos(Alpha) * Cos(Beta) class TrackerBlock(Structure): _pack_ = 1 _fields_ = [ ('TrackID', c_uint32), # 0 to 4294967296 Track ID ('XPos', c_int16), # -32768 to 32767, divide by 100 to get x in m (e.g. 6453 = 64.53 m, e.g.: -2456 = -24.56 m) ('YPos', c_int16), # -32768 to 32767, divide by 100 to get y in m (e.g. 6453 = 64.53 m, e.g. -2456 = -24.56 m) ('ZPos', c_uint16), # 0 to 65535, divide by 100 to get z in m (e.g. 12654 = 126.54 m) ('XDot', c_int16), # -32768 to 32767, divide by 100 to get speed in the x direction in m/s (e.g. 6453 = 64.53 m/s, e.g. -2456 = -24.56 m/s) ('YDot', c_int16), # -32768 to 32767, divide by 100 to get speed in the y direction in m/s (e.g. 6453 = 64.53 m/s, e.g. -2456 = -24.56 m/s) ('ZDot', c_int16), # -32768 to 32767, divide by 100 to get speed in the z direction in m/s (e.g. 6453 = 64.53 m/s, e.g. -2456 = -24.56 m/s) ('Res1', c_uint16), ('Res2', c_uint16), ('Res3', c_uint16), ('Flag', c_uint16), # 0 to 65535. 16 1-bit flags, the flag definitions are internal to Oculii. # Bit 3 – Bit 15 : Reserved # Bit 0, Bit 1, Bit 2 : Track Quality. Currently only values 1 and 2 are used. Filter to use only value ‘2’. ('Class', c_uint16), # 0 to 5. Reserved for future. # 0: Unknown Class # 1: Pedestrian # 2: Motorcycle/Bike # 3: Vehicle and SUV # 4: Bus and Truck # 5: Background ('Conf', c_uint16), ('Res4', c_uint16), ('Res5', c_uint16), ] def bytes_hex(bytes): return ' '.join(["%02x" % b for b in bytes]) class MicroWaveParser: def __init__(self): self.state = UDP_STATE_IDLE self.N = 0 self.Nt = 0 self.PointCount = 0 self.TrackCount = 0 self.PointCloudReady = False self.PointCloud = bytearray() def _receive_data(self, data) -> None: self.data_buf.extend(data) self.ReceivedDataLength += len(data) if self.ReceivedDataLength == self.DataLength: self.state = UDP_STATE_FOOTER return def _receive_footer(self, data) -> None: self.footer_buf.extend(data) self.ReceivedFooterLength += len(data) if self.ReceivedFooterLength == sizeof(FooterBlock): footer = FooterBlock.from_buffer_copy(self.footer_buf) self.PointCloudReady = True self.state = UDP_STATE_IDLE return def AddBuf(self, buf: bytes) -> None: buf_size = len(buf) if self.state == UDP_STATE_IDLE: if buf_size == sizeof(Handshake): handshake = Handshake.from_buffer_copy(buf) if HAND_SHAKE_MAGIC == string_at(handshake.Magic, 8): self.PointCloudReady = False self.state = UDP_STATE_HAND_SHAKE self.PointCloud = bytearray(buf) elif self.state == UDP_STATE_HAND_SHAKE: self.PointCloud.extend(buf) header = HeaderBlock.from_buffer_copy(buf[:sizeof(HeaderBlock)]) if HEADER_MAGIC == string_at(header.Magic, 8): self.N = header.NumberOfDetection self.Nt = header.NumberOfTrack self.PointCount = 0 self.TrackCount = 0 self.DataLength = self.N * 8 + self.Nt * sizeof(TrackerBlock) self.RemainLength = self.DataLength + sizeof(FooterBlock) self.data_buf = bytearray() self.footer_buf = bytearray() self.Points = [] self.ReceivedDataLength = 0 self.ReceivedFooterLength = 0 self.state = UDP_STATE_DATA header_size = sizeof(HeaderBlock) # 处理第一个数据包中的数据 data_size = min(buf_size - header_size, self.DataLength) self._receive_data(buf[header_size: header_size + data_size]) # 判断第一个数据包中是否包含Footer Block if (header_size + data_size) < buf_size: self._receive_footer(buf[header_size + data_size]) elif self.state == UDP_STATE_DATA: self.PointCloud.extend(buf) remain_data_size = self.DataLength - self.ReceivedDataLength data_size = min(buf_size, remain_data_size) self._receive_data(buf[: data_size]) if data_size < buf_size: self._receive_footer(buf[data_size: ]) elif self.state == UDP_STATE_FOOTER: self.PointCloud.extend(buf) self._receive_footer(buf) if __name__ == '__main__': if len(argv) != 2: print('Usage: python parser.py [filename]') exit(1) print(sizeof(Handshake)) filename = argv[1] file_size = os.path.getsize(filename) with open(filename, "rb") as udp_file: if file_size == 24: handshake = Handshake.from_buffer_copy(udp_file.read(sizeof(Handshake))) udp_file.close() print('Magic: ', bytes_hex(handshake.Magic)) print('FrameDataLength: ', handshake.FrameDataLength) print('Reserved: ', bytes_hex(handshake.Reserved)) elif file_size == 256: header = HeaderBlock.from_buffer_copy(udp_file.read(sizeof(HeaderBlock))) udp_file.close() print('Magic: ', bytes_hex(header.Magic)) print('FrameNumber: ', header.FrameNumber) print('VersionNumber: ', header.VersionNumber) print('NumberOfDetection: ', header.NumberOfDetection) print('NumberOfTrack: ', header.NumberOfTrack) print('HostSpeed: ', header.HostSpeed) print('HostAngle: ', header.HostAngle) print('Reserved: ', bytes_hex(header.Reserved)) print('RangeAccuracyIdx: ', header.RangeAccuracyIdx) print('DopplerAccuracyIdx: ', header.DopplerAccuracyIdx) print('AzimuthAccuracyIdx: ', header.AzimuthAccuracyIdx) print('ElevationAccuracyIdx: ', header.ElevationAccuracyIdx) print('DspWorkload: ', header.DspWorkload) print('ARMWorkload: ', header.ARMWorkload) print('Reserved2: ', bytes_hex(header.Reserved2))