Merge commit 'f79b30a55db543bd1c7dd494a8c896df8841aa28'
| New file |
| | |
|---|
| | | @echo off |
|---|
| | | |
|---|
| | | @REM change to the filepath |
|---|
| | | cd %~dp0 |
|---|
| | | echo change to path: %cd% |
|---|
| | | |
|---|
| | | set aerialpython=%cd%/env/aerial_deploy/python.exe |
|---|
| | | set envsetting=%cd%/env/SettingEnv.bat |
|---|
| | | |
|---|
| | | @REM Setting the env |
|---|
| | | if not exist %aerialpython% ( |
|---|
| | | call %envsetting% |
|---|
| | | ) |
|---|
| | | cd %~dp0 |
|---|
| | | echo change to path: %cd% |
|---|
| | | @REM please the folder path of the bin files. |
|---|
| | | @REM set BINfolder=./Manual_Microwave/pod1-2012-12-12-12-12-12/ |
|---|
| | | |
|---|
| | | @REM ---------------Usage: bin2ply.py [BINfolder] |
|---|
| | | %aerialpython% ./src/utils/bin2ply.py %BINfolder% |
|---|
| | | pause |
|---|
| New file |
| | |
|---|
| | | from .components import * |
|---|
| | | from .constants import * |
|---|
| | | from .parsers import * |
|---|
| | | from .servers import * |
|---|
| New file |
| | |
|---|
| | | from ctypes import Structure, c_ubyte, c_uint16, c_uint32, c_int16 |
|---|
| | | from bitarray import bitarray, util |
|---|
| | | import math |
|---|
| | | |
|---|
| | | """ |
|---|
| | | UDP packets 0 --> ... --> UDP packets n |
|---|
| | | Handshake --> HeaderBlock --> DetectionBlock --> TrackerBlock --> FooterBlock |
|---|
| | | |
|---|
| | | """ |
|---|
| | | |
|---|
| | | |
|---|
| | | class Handshake(Structure): |
|---|
| | | """ |
|---|
| | | Handshake structure, 24 bytes |
|---|
| | | """ |
|---|
| | | _pack_ = 1 |
|---|
| | | _fields_ = [ |
|---|
| | | ('Magic', c_ubyte*8), |
|---|
| | | ('FrameDataLength', c_uint32), |
|---|
| | | ('Reserved', c_ubyte * 12) |
|---|
| | | ] |
|---|
| | | |
|---|
| | | |
|---|
| | | class HeaderBlock(Structure): |
|---|
| | | """ |
|---|
| | | Header structure, 48 bytes |
|---|
| | | """ |
|---|
| | | _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), |
|---|
| | | ] |
|---|
| | | def __new__(cls, buf): |
|---|
| | | # new instance from buf |
|---|
| | | return cls.from_buffer_copy(buf) |
|---|
| | | |
|---|
| | | def __init__(self, data): |
|---|
| | | # claculate parameters |
|---|
| | | self.RangeAccuracy = self.RangeAccuracyIdx / 10000.0 |
|---|
| | | self.DopplerAccuracy = self.DopplerAccuracyIdx / 10000.0 |
|---|
| | | self.AzimuthAccuracy = self.AzimuthAccuracyIdx / 10000.0 |
|---|
| | | self.ElevationAccuracy = self.ElevationAccuracyIdx / 10000.0 |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | | class FooterBlock(Structure): |
|---|
| | | """ |
|---|
| | | Footer structure, 32 bytes |
|---|
| | | """ |
|---|
| | | _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), |
|---|
| | | ] |
|---|
| | | def __new__(cls, buf): |
|---|
| | | # new instance from buf |
|---|
| | | return cls.from_buffer_copy(buf) |
|---|
| | | |
|---|
| | | def __init__(self, data): |
|---|
| | | # claculate parameters |
|---|
| | | self.RangeAccuracy = self.RangeAccuracyIdx / 10000.0 |
|---|
| | | self.DopplerAccuracy = self.DopplerAccuracyIdx / 10000.0 |
|---|
| | | self.AzimuthAccuracy = self.AzimuthAccuracyIdx / 10000.0 |
|---|
| | | self.ElevationAccuracy = self.ElevationAccuracyIdx / 10000.0 |
|---|
| | | |
|---|
| | | |
|---|
| | | class DetectionBlock: |
|---|
| | | """ |
|---|
| | | Detection structure |
|---|
| | | using bitarray parse structure because of ctypes can't parse compact bitfields |
|---|
| | | """ |
|---|
| | | |
|---|
| | | def __init__(self, header: HeaderBlock, footer: FooterBlock, data): |
|---|
| | | buf = bytearray(data) |
|---|
| | | buf.reverse() |
|---|
| | | bits = bitarray() |
|---|
| | | bits.frombytes(bytes(buf)) |
|---|
| | | # print(bits) |
|---|
| | | # parse data structure |
|---|
| | | self.Flag = bits[1] |
|---|
| | | self.RangeIndex = util.ba2int(bits[54:64]) |
|---|
| | | self.DropplerIndex = util.ba2int(bits[45:54]) |
|---|
| | | if bits[44]: |
|---|
| | | self.DropplerIndex -= 512 |
|---|
| | | self.AzimuthIndex = util.ba2int(bits[35:44]) |
|---|
| | | if bits[34]: |
|---|
| | | self.AzimuthIndex -= 512 |
|---|
| | | self.BetaIndex = util.ba2int(bits[25:34]) |
|---|
| | | if bits[24]: |
|---|
| | | self.BetaIndex -= 512 |
|---|
| | | self.PowerValue = util.ba2int(bits[8:24]) |
|---|
| | | |
|---|
| | | if not self.Flag: |
|---|
| | | # using header's parameters |
|---|
| | | RangeAccuracy = header.RangeAccuracy |
|---|
| | | DopplerAccuracy = header.DopplerAccuracy |
|---|
| | | AzimuthAccuracy = header.AzimuthAccuracy |
|---|
| | | ElevationAccuracy = header.ElevationAccuracy |
|---|
| | | else: |
|---|
| | | # using footer's parameters |
|---|
| | | RangeAccuracy = footer.RangeAccuracy |
|---|
| | | DopplerAccuracy = footer.DopplerAccuracy |
|---|
| | | AzimuthAccuracy = footer.AzimuthAccuracy |
|---|
| | | ElevationAccuracy = footer.ElevationAccuracy |
|---|
| | | |
|---|
| | | # claculation |
|---|
| | | self.Power = self.PowerValue / 100.0 |
|---|
| | | self.Range = self.RangeIndex * RangeAccuracy |
|---|
| | | self.Doppler = self.DropplerIndex * DopplerAccuracy |
|---|
| | | self.Beta = self.BetaIndex * ElevationAccuracy |
|---|
| | | self.Azimuth = self.AzimuthIndex * AzimuthAccuracy |
|---|
| | | # degrees to radians |
|---|
| | | alpha = math.radians(self.Azimuth) |
|---|
| | | beta = math.radians(self.Beta) |
|---|
| | | # 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) |
|---|
| | | # or |
|---|
| | | # Z = sqrt(Range^2 - X^2 - Y^2) |
|---|
| | | self.X = self.Range * math.sin(alpha) * math.cos(beta) |
|---|
| | | self.Y = self.Range * math.sin(beta) |
|---|
| | | self.Z = math.sqrt(self.Range * self.Range - |
|---|
| | | self.X * self.X - self.Y * self.Y) |
|---|
| | | |
|---|
| | | class TrackerBlock(Structure): |
|---|
| | | """ |
|---|
| | | Tracker 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), |
|---|
| | | ] |
|---|
| New file |
| | |
|---|
| | | HAND_SHAKE_MAGIC = b"\x01\x09\x08\x09\x01\x00\x02\x02" |
|---|
| | | HEADER_MAGIC = b"\x02\x01\x04\x03\x06\x05\x08\x07" |
|---|
| New file |
| | |
|---|
| | | @echo off |
|---|
| | | |
|---|
| | | @REM change to the filepath |
|---|
| | | cd %~dp0 |
|---|
| | | echo change to path: %cd% |
|---|
| | | |
|---|
| | | set aerialpython=%cd%/../../../env/aerial_deploy/python.exe |
|---|
| | | set envsetting=%cd%/env/SettingEnv.bat |
|---|
| | | |
|---|
| | | @REM Setting the env |
|---|
| | | if not exist %aerialpython% ( |
|---|
| | | call %envsetting% |
|---|
| | | ) |
|---|
| | | |
|---|
| | | echo All data will be save in '%cd%/saveUDPData/' folder |
|---|
| | | |
|---|
| | | %aerialpython% udp_test_get.py microwave |
|---|
| | | |
|---|
| | | echo =========== ALL DONE ================ |
|---|
| | | |
|---|
| | | echo Press any key to exit this windiow. |
|---|
| | | pause |
|---|
| New file |
| | |
|---|
| | | @echo off |
|---|
| | | |
|---|
| | | @REM change to the filepath |
|---|
| | | cd %~dp0 |
|---|
| | | echo change to path: %cd% |
|---|
| | | |
|---|
| | | set aerialpython=%cd%/../../../env/aerial_deploy/python.exe |
|---|
| | | set envsetting=%cd%/env/SettingEnv.bat |
|---|
| | | |
|---|
| | | @REM Setting the env |
|---|
| | | if not exist %aerialpython% ( |
|---|
| | | call %envsetting% |
|---|
| | | ) |
|---|
| | | |
|---|
| | | echo All data will be saved in '%cd%/saveUDPData/' folder |
|---|
| | | |
|---|
| | | %aerialpython% udp_test_get.py test |
|---|
| | | |
|---|
| | | echo =========== ALL GET DONE ================ |
|---|
| | | |
|---|
| | | echo Press any key to exit this windiow. |
|---|
| | | pause |
|---|
| New file |
| | |
|---|
| | | @echo off |
|---|
| | | |
|---|
| | | @REM change to the filepath |
|---|
| | | cd %~dp0 |
|---|
| | | echo change to path: %cd% |
|---|
| | | |
|---|
| | | set aerialpython=%cd%/../../../env/aerial_deploy/python.exe |
|---|
| | | set envsetting=%cd%/env/SettingEnv.bat |
|---|
| | | |
|---|
| | | @REM Setting the env |
|---|
| | | if not exist %aerialpython% ( |
|---|
| | | call %envsetting% |
|---|
| | | ) |
|---|
| | | |
|---|
| | | start AutoGetUDPtest.bat |
|---|
| | | start AutoSendUDPtest.bat |
|---|
| New file |
| | |
|---|
| | | @echo off |
|---|
| | | |
|---|
| | | @REM change to the filepath |
|---|
| | | cd %~dp0 |
|---|
| | | echo change to path: %cd% |
|---|
| | | |
|---|
| | | set aerialpython=%cd%/../../../env/aerial_deploy/python.exe |
|---|
| | | set envsetting=%cd%/env/SettingEnv.bat |
|---|
| | | |
|---|
| | | @REM Setting the env |
|---|
| | | if not exist %aerialpython% ( |
|---|
| | | call %envsetting% |
|---|
| | | ) |
|---|
| | | |
|---|
| | | echo All data saved in '%cd%/demoUDPData/' folder will be sent to 127.0.0.1:9911 |
|---|
| | | |
|---|
| | | %aerialpython% udp_test_send.py |
|---|
| | | |
|---|
| | | echo =========== ALL SEND DONE ================ |
|---|
| | | |
|---|
| | | echo Press any key to exit this windiow. |
|---|
| | | pause |
|---|
| New file |
| | |
|---|
| | | import os |
|---|
| | | import sys |
|---|
| | | |
|---|
| | | # add python path of microwave to sys.path |
|---|
| | | microwave_path = os.path.join(__file__, *(['..'] * 3)) |
|---|
| | | microwave_path = os.path.abspath(microwave_path) |
|---|
| | | sys.path.insert(0, microwave_path) |
|---|
| | | |
|---|
| | | from microwave.parsers import MicroWaveParser |
|---|
| | | |
|---|
| | | if __name__ == '__main__': |
|---|
| | | if len(sys.argv) != 2: |
|---|
| | | print('Usage: python MicroWave2Ply.py [filename.bin]') |
|---|
| | | exit(1) |
|---|
| | | |
|---|
| | | filename = sys.argv[1] |
|---|
| | | (fn, ext) = os.path.splitext(filename) |
|---|
| | | with open(filename, "rb") as udp_file: |
|---|
| | | buf = udp_file.read() |
|---|
| | | point_cloud = MicroWaveParser.ParserFrame(buf) |
|---|
| | | with open(fn + ".ply", "w") as f: |
|---|
| | | f.write("ply\n") |
|---|
| | | f.write("format ascii 1.0\n") |
|---|
| | | f.write("comment MicroWave2Ply generated\n") |
|---|
| | | f.write("element vertex %d\n" % len(point_cloud)) |
|---|
| | | f.write("property float x\n") |
|---|
| | | f.write("property float y\n") |
|---|
| | | f.write("property float z\n") |
|---|
| | | f.write("element face 0\n") |
|---|
| | | f.write("property list uchar int vertex_indices\n") |
|---|
| | | f.write("end_header\n") |
|---|
| | | for p in point_cloud: |
|---|
| | | x, y, z = p |
|---|
| | | f.write("{:.5f} {:.5f} {:.5f}\n".format(x, y, z)) |
|---|
| | | print("Done!") |
|---|
| New file |
| | |
|---|
| | | import os |
|---|
| | | from sys import argv |
|---|
| | | import sys |
|---|
| | | |
|---|
| | | # add python path of microwave to sys.path |
|---|
| | | microwave_path = os.path.join(__file__, *(['..'] * 3)) |
|---|
| | | microwave_path = os.path.abspath(microwave_path) |
|---|
| | | sys.path.insert(0, microwave_path) |
|---|
| | | |
|---|
| | | from microwave.servers import MicroWaveReceiver |
|---|
| | | |
|---|
| | | |
|---|
| | | if __name__ == '__main__': |
|---|
| | | if len(sys.argv) != 2: |
|---|
| | | print('Usage: python MicroWaveReceiverTest.py [folder]') |
|---|
| | | exit(1) |
|---|
| | | folder = sys.argv[1] |
|---|
| | | files = ["packet{}.udp".format(i) for i in range(100)] |
|---|
| | | receiver = MicroWaveReceiver() |
|---|
| | | for file in files: |
|---|
| | | filename = os.path.join(folder, file) |
|---|
| | | with open(filename, "rb") as f: |
|---|
| | | data = f.read() |
|---|
| | | receiver.AddBuf(data) |
|---|
| | | if receiver.DataFrameReady: |
|---|
| | | frame_number = receiver.Header.FrameNumber |
|---|
| | | bin_name = os.path.join(folder, "{}.bin".format(frame_number)) |
|---|
| | | print("Frame {} ready".format(frame_number)) |
|---|
| | | with open(bin_name, "wb") as bin_file: |
|---|
| | | bin_file.write(receiver.DataFrame) |
|---|
| | | bin_file.close() |
|---|
| New file |
| | |
|---|
| | | import sys |
|---|
| | | import time |
|---|
| | | import os |
|---|
| | | from socketserver import BaseRequestHandler, UDPServer |
|---|
| | | |
|---|
| | | # add python path of microwave to sys.path |
|---|
| | | microwave_path = os.path.join(__file__, *(['..'] * 3)) |
|---|
| | | microwave_path = os.path.abspath(microwave_path) |
|---|
| | | sys.path.insert(0, microwave_path) |
|---|
| | | |
|---|
| | | from microwave.servers import MicroWaveReceiver |
|---|
| | | |
|---|
| | | |
|---|
| | | class MicroWaveRequestHandlertest(BaseRequestHandler): |
|---|
| | | def handle(self): |
|---|
| | | # Get message and client socket |
|---|
| | | buf, _ = self.request |
|---|
| | | receiver = self.server.Receiver |
|---|
| | | receiver.AddBuf(buf) |
|---|
| | | if receiver.DataFrameReady: |
|---|
| | | # data frame ready |
|---|
| | | frame_number = receiver.Header.FrameNumber |
|---|
| | | bin_name = os.path.join(self.server.OutputFolder, "{}.bin".format(frame_number)) |
|---|
| | | print("Frame {} ready".format(frame_number)) |
|---|
| | | with open(bin_name, "wb") as bin_file: |
|---|
| | | bin_file.write(receiver.DataFrame) |
|---|
| | | |
|---|
| | | |
|---|
| | | class MicroWaveUDPServertest(UDPServer): |
|---|
| | | def __init__(self, server_address, output_folder): |
|---|
| | | super().__init__(server_address, MicroWaveRequestHandlertest) |
|---|
| | | # create receiver instance |
|---|
| | | self.Receiver = MicroWaveReceiver() |
|---|
| | | self.OutputFolder = output_folder |
|---|
| | | |
|---|
| | | if __name__ == '__main__': |
|---|
| | | if len(sys.argv) == 2: |
|---|
| | | output_folder = sys.argv[1] |
|---|
| | | else: |
|---|
| | | output_folder = '.' |
|---|
| | | |
|---|
| | | # host = '192.168.2.65' |
|---|
| | | host = '' |
|---|
| | | port = 9911 |
|---|
| | | server = MicroWaveUDPServertest((host, port), output_folder) |
|---|
| | | server.packed_count = 0 |
|---|
| | | print(time.asctime(),'Server started on port', port) |
|---|
| | | print('....') |
|---|
| | | print('ctrl-c to quit server.') |
|---|
| | | try: |
|---|
| | | server.serve_forever() |
|---|
| | | except KeyboardInterrupt: |
|---|
| | | server.server_close() |
|---|
| | | print(time.asctime(),"Server Stopped") |
|---|
| | | except: |
|---|
| | | server.server_close() |
|---|
| | | print(time.asctime(),"Server Stopped") |
|---|
| New file |
| | |
|---|
| | | { |
|---|
| | | "cells": [ |
|---|
| | | { |
|---|
| | | "cell_type": "code", |
|---|
| | | "execution_count": 3, |
|---|
| | | "metadata": {}, |
|---|
| | | "outputs": [ |
|---|
| | | { |
|---|
| | | "name": "stdout", |
|---|
| | | "output_type": "stream", |
|---|
| | | "text": [ |
|---|
| | | "[Open3D WARNING] [ViewControl] SetViewPoint() failed because window height and width are not set.\n" |
|---|
| | | ] |
|---|
| | | } |
|---|
| | | ], |
|---|
| | | "source": [ |
|---|
| | | "import open3d as o3d\n", |
|---|
| | | "# visualization of point clouds.\n", |
|---|
| | | "# pcd = o3d.io.read_point_cloud('dense2_without_color.ply')\n", |
|---|
| | | "# show = [pcd]\n", |
|---|
| | | "pcd2522 = o3d.io.read_point_cloud('demoData/2522.ply')\n", |
|---|
| | | "pcd2523 = o3d.io.read_point_cloud('demoData2523.ply')\n", |
|---|
| | | "pcd2524 = o3d.io.read_point_cloud('demoData2524.ply')\n", |
|---|
| | | "pcd2525 = o3d.io.read_point_cloud('demoData2525.ply')\n", |
|---|
| | | "pcd2526 = o3d.io.read_point_cloud('demoData2526.ply')\n", |
|---|
| | | "pcd2527 = o3d.io.read_point_cloud('demoData2527.ply')\n", |
|---|
| | | "show = [pcd2522,pcd2523,pcd2524,pcd2525,pcd2526,pcd2527]\n", |
|---|
| | | "o3d.visualization.draw_geometries(show)\n" |
|---|
| | | ] |
|---|
| | | }, |
|---|
| | | { |
|---|
| | | "cell_type": "code", |
|---|
| | | "execution_count": null, |
|---|
| | | "metadata": {}, |
|---|
| | | "outputs": [], |
|---|
| | | "source": [] |
|---|
| | | } |
|---|
| | | ], |
|---|
| | | "metadata": { |
|---|
| | | "interpreter": { |
|---|
| | | "hash": "35e65690039548a4ed71bb11d5d15cea8607c9411f67738c48033f1deba4df50" |
|---|
| | | }, |
|---|
| | | "kernelspec": { |
|---|
| | | "display_name": "Python 3.9.10 ('huaneng_server')", |
|---|
| | | "language": "python", |
|---|
| | | "name": "python3" |
|---|
| | | }, |
|---|
| | | "language_info": { |
|---|
| | | "codemirror_mode": { |
|---|
| | | "name": "ipython", |
|---|
| | | "version": 3 |
|---|
| | | }, |
|---|
| | | "file_extension": ".py", |
|---|
| | | "mimetype": "text/x-python", |
|---|
| | | "name": "python", |
|---|
| | | "nbconvert_exporter": "python", |
|---|
| | | "pygments_lexer": "ipython3", |
|---|
| | | "version": "3.9.10" |
|---|
| | | }, |
|---|
| | | "orig_nbformat": 4 |
|---|
| | | }, |
|---|
| | | "nbformat": 4, |
|---|
| | | "nbformat_minor": 2 |
|---|
| | | } |
|---|
| New file |
| | |
|---|
| | | from socketserver import BaseRequestHandler, UDPServer |
|---|
| | | import time |
|---|
| | | import os, sys |
|---|
| | | |
|---|
| | | |
|---|
| | | cudir,_ = os.path.split(__file__) |
|---|
| | | file_folder = os.path.join(cudir,"saveUDPData") |
|---|
| | | |
|---|
| | | if not os.path.exists(file_folder): |
|---|
| | | os.makedirs(file_folder) |
|---|
| | | print(f"make a new folder: {file_folder}") |
|---|
| | | |
|---|
| | | packed_count = 0 |
|---|
| | | |
|---|
| | | class PointCloudUDPRequestHandler(BaseRequestHandler): |
|---|
| | | |
|---|
| | | def handle(self): |
|---|
| | | |
|---|
| | | global packed_count |
|---|
| | | |
|---|
| | | print(time.asctime(),' Got connection from {}, {}'.format(self.client_address, packed_count)) |
|---|
| | | # Get message and client socket |
|---|
| | | msg, sock = self.request |
|---|
| | | f = open('{}/packet{}.udp'.format(file_folder,packed_count), 'wb') |
|---|
| | | f.write(msg) |
|---|
| | | f.close() |
|---|
| | | packed_count += 1 |
|---|
| | | if packed_count >= 1000000: |
|---|
| | | UDPServer.server_close(server) |
|---|
| | | |
|---|
| | | |
|---|
| | | if __name__ == '__main__': |
|---|
| | | |
|---|
| | | def help(): |
|---|
| | | print("Usage: udp_test_get.py [host]. host can be 'test' or 'microwave' ") |
|---|
| | | exit(1) |
|---|
| | | |
|---|
| | | if len(sys.argv) == 2: |
|---|
| | | if sys.argv[1] == 'test': |
|---|
| | | host = '' |
|---|
| | | elif sys.argv[1] == 'microwave': |
|---|
| | | host = '192.168.2.65' |
|---|
| | | else: |
|---|
| | | help() |
|---|
| | | else: |
|---|
| | | help() |
|---|
| | | |
|---|
| | | global server |
|---|
| | | # host = '' |
|---|
| | | # host = '192.168.2.65' # use this host to get the udp data from microwave device |
|---|
| | | port = 9911 |
|---|
| | | server =UDPServer((host, port), PointCloudUDPRequestHandler) |
|---|
| | | print(time.asctime(),'Server started on port', port) |
|---|
| | | print('....') |
|---|
| | | print('ctrl-c to quit server.') |
|---|
| | | try: |
|---|
| | | server.serve_forever() |
|---|
| | | except KeyboardInterrupt: |
|---|
| | | server.server_close() |
|---|
| | | print(time.asctime(),"Server Stopped") |
|---|
| | | except: |
|---|
| | | server.server_close() |
|---|
| | | print(time.asctime(),"Server Stopped") |
|---|
| New file |
| | |
|---|
| | | import time |
|---|
| | | import socket |
|---|
| | | import os, sys |
|---|
| | | |
|---|
| | | cudir,_ = os.path.split(__file__) |
|---|
| | | file_folder = os.path.join(cudir,"demoData") |
|---|
| | | |
|---|
| | | packed_count = 0 |
|---|
| | | if __name__ == '__main__': |
|---|
| | | |
|---|
| | | files = ["packet{}.udp".format(i) for i in range(0, 100)] |
|---|
| | | for file in files: |
|---|
| | | with open(os.path.join(file_folder,file), "rb") as f: |
|---|
| | | message = f.read() |
|---|
| | | client_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) |
|---|
| | | client_socket.settimeout(1.0) |
|---|
| | | addr = ("127.0.0.1", 9911) |
|---|
| | | client_socket.sendto(message, addr) |
|---|
| | | print (time.asctime(), f' Sending {file} to {addr} ...') |
|---|
| | | time.sleep(0.1) |
|---|
| | | print('Done!') |
|---|
| New file |
| | |
|---|
| | | from .components import * |
|---|
| | | from .constants import * |
|---|
| | | from ctypes import sizeof, string_at |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | | class MicroWaveParser: |
|---|
| | | @classmethod |
|---|
| | | def ParserFrame(cls, buffer: bytes) -> list: |
|---|
| | | """ |
|---|
| | | 解析完整的一帧数据，数据包括数据头，数据体，数据尾，返回点云数组 |
|---|
| | | """ |
|---|
| | | header = HeaderBlock(buffer[:sizeof(HeaderBlock)]) |
|---|
| | | if HEADER_MAGIC == string_at(header.Magic, 8): |
|---|
| | | footer = FooterBlock(buffer[-sizeof(FooterBlock):]) |
|---|
| | | point_cloud = [] |
|---|
| | | for i in range(header.NumberOfDetection): |
|---|
| | | detection = DetectionBlock(header, footer, buffer[i * 8 + sizeof(HeaderBlock): (i + 1) * 8 + sizeof(HeaderBlock)]) |
|---|
| | | point = (detection.X, detection.Y, detection.Z) |
|---|
| | | point_cloud.append(point) |
|---|
| | | return point_cloud |
|---|
| | | else: |
|---|
| | | return [] |
|---|
| New file |
| | |
|---|
| | | import os, sys |
|---|
| | | from ctypes import sizeof, string_at |
|---|
| | | from socketserver import BaseRequestHandler, UDPServer |
|---|
| | | from .components import * |
|---|
| | | from .constants import * |
|---|
| | | |
|---|
| | | # add python path of src to sys.path |
|---|
| | | src_path = os.path.join(__file__, *(['..'] * 2)) |
|---|
| | | src_path = os.path.abspath(src_path) |
|---|
| | | sys.path.insert(0, src_path) |
|---|
| | | |
|---|
| | | from protos import aerial_pb2 as pb |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | | UDP_STATE_IDLE = 0 |
|---|
| | | UDP_STATE_HAND_SHAKE = 1 |
|---|
| | | UDP_STATE_DATA = 2 |
|---|
| | | UDP_STATE_FOOTER = 3 |
|---|
| | | |
|---|
| | | class MicroWaveReceiver: |
|---|
| | | """ |
|---|
| | | micro wave data receiver |
|---|
| | | use AddBuf method add udp packet to receiver |
|---|
| | | when DataFrameReady == True, you can get data frame |
|---|
| | | """ |
|---|
| | | def __init__(self): |
|---|
| | | self.state = UDP_STATE_IDLE |
|---|
| | | self.PointCount = 0 |
|---|
| | | self.TrackCount = 0 |
|---|
| | | self.DataFrameReady = False |
|---|
| | | self.DataFrame = None |
|---|
| | | self.Header = None |
|---|
| | | self.Footer = None |
|---|
| | | |
|---|
| | | def _receive_data(self, data) -> None: |
|---|
| | | """ |
|---|
| | | receive data packet from udp |
|---|
| | | """ |
|---|
| | | self.data_buf.extend(data) |
|---|
| | | self.ReceivedDataLength += len(data) |
|---|
| | | if self.ReceivedDataLength == self.DataLength: |
|---|
| | | # if received data length equal expected data length |
|---|
| | | # then we can get footer block |
|---|
| | | self.state = UDP_STATE_FOOTER |
|---|
| | | return |
|---|
| | | |
|---|
| | | def _receive_footer(self, data) -> None: |
|---|
| | | # receive footer block |
|---|
| | | self.footer_buf.extend(data) |
|---|
| | | self.ReceivedFooterLength += len(data) |
|---|
| | | if self.ReceivedFooterLength == sizeof(FooterBlock): |
|---|
| | | self.Footer = FooterBlock.from_buffer_copy(self.footer_buf) |
|---|
| | | # after received footer block, the data frame is ready |
|---|
| | | self.DataFrameReady = True |
|---|
| | | # reset state to idle, ready for next data frame |
|---|
| | | self.state = UDP_STATE_IDLE |
|---|
| | | return |
|---|
| | | |
|---|
| | | def AddBuf(self, buf: bytes) -> None: |
|---|
| | | # add a udp packet to receiver |
|---|
| | | 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.DataFrameReady = False |
|---|
| | | self.state = UDP_STATE_HAND_SHAKE |
|---|
| | | self.DataFrame = None |
|---|
| | | self.Header = None |
|---|
| | | self.Footer = None |
|---|
| | | elif self.state == UDP_STATE_HAND_SHAKE: |
|---|
| | | # after received handshake packet, we can get header block |
|---|
| | | self.Header = HeaderBlock.from_buffer_copy(buf[:sizeof(HeaderBlock)]) |
|---|
| | | if HEADER_MAGIC == string_at(self.Header.Magic, 8): |
|---|
| | | self.DataFrame = bytearray(buf) |
|---|
| | | N = self.Header.NumberOfDetection |
|---|
| | | Nt = self.Header.NumberOfTrack |
|---|
| | | self.PointCount = 0 |
|---|
| | | self.TrackCount = 0 |
|---|
| | | self.DataLength = N * 8 + 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 |
|---|
| | | # the next packet should be data block |
|---|
| | | 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: |
|---|
| | | # receive data block |
|---|
| | | self.DataFrame.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: |
|---|
| | | # receive footer block |
|---|
| | | self.DataFrame.extend(buf) |
|---|
| | | self._receive_footer(buf) |
|---|
| | | |
|---|
| | | |
|---|
| | | |
|---|
| | | class MicroWaveRequestHandler(BaseRequestHandler): |
|---|
| | | """ |
|---|
| | | micro wave udp request handler |
|---|
| | | """ |
|---|
| | | def handle(self): |
|---|
| | | # Get message and client socket |
|---|
| | | buf, _ = self.request |
|---|
| | | receiver = self.server.Receiver |
|---|
| | | # add buffer to receiver |
|---|
| | | receiver.AddBuf(buf) |
|---|
| | | if receiver.DataFrameReady: |
|---|
| | | # dataframe ready, add to udp_queue |
|---|
| | | msg = pb.RequestCommand() |
|---|
| | | msg.sys_command = pb.SysCommand.MICRO_WAVE_COMMAND |
|---|
| | | msg.sub_command = pb.SubCommand.MICRO_WAVE_SUB_COMMAND |
|---|
| | | data1 = pb.MicroWaveMessage() |
|---|
| | | data1.length = len(receiver.DataFrame) |
|---|
| | | data1.data = bytes(receiver.DataFrame) |
|---|
| | | data1_str = data1.SerializeToString() |
|---|
| | | msg.data1.length = len(data1_str) |
|---|
| | | msg.data1.data = data1_str |
|---|
| | | self.server.msg_queue.put(msg) |
|---|
| | | |
|---|
| | | |
|---|
| | | class MicroWaveUDPServer(UDPServer): |
|---|
| | | """ |
|---|
| | | micro wave udp server |
|---|
| | | """ |
|---|
| | | def __init__(self, server_address, msg_queue): |
|---|
| | | super().__init__(server_address, MicroWaveRequestHandler) |
|---|
| | | # create receiver instance |
|---|
| | | self.Receiver = MicroWaveReceiver() |
|---|
| | | self.msg_queue = msg_queue |
|---|
