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