import os, sys
|
from ctypes import sizeof, string_at
|
# from socketserver import _AddressType, _RequestType, BaseRequestHandler, BaseServer, UDPServer
|
from socketserver import BaseRequestHandler, BaseServer, UDPServer
|
import threading
|
from typing import Union
|
import warnings
|
import time
|
import queue
|
import numpy as np
|
from .components import *
|
from .constants import *
|
from .parsers import MicroWaveParser
|
|
|
|
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 _is_handshake(self,data):
|
handshake = Handshake.from_buffer_copy(data)
|
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
|
return True
|
else:
|
return False
|
|
def _get_header(self, buf):
|
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
|
return True
|
else:
|
False
|
|
def _receive_data(self, data) -> None:
|
"""
|
receive data packet from udp
|
"""
|
self.data_buf.extend(bytes(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 True
|
elif self.ReceivedDataLength > self.DataLength:
|
self.state = UDP_STATE_IDLE
|
warnings.warn(f"encounter an error when getting the detection data in udp packets ...")
|
return False
|
else:
|
return True
|
|
def _receive_footer(self, data) -> None:
|
# receive footer block
|
self.footer_buf.extend(bytes(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 buf_size == sizeof(Handshake):
|
self._is_handshake(buf)
|
return # there is only handshake(24 bytes) on it, end AddBuf func.
|
|
if self.state == UDP_STATE_HAND_SHAKE:
|
# after received handshake packet, we can get header block
|
try:
|
# make sure to get a complete header
|
result = self._get_header(buf)
|
except Exception as e:
|
warnings.warn(f"encounter an unknow error [{e}] when getting the header data in udp packets ...")
|
result = False
|
|
if not result:
|
# waiting for the next handshake udp packet
|
self.state = UDP_STATE_IDLE
|
else:
|
# process the detection data in the first data packets
|
header_size = sizeof(HeaderBlock)
|
data_size = min(buf_size - header_size, self.DataLength)
|
|
if 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(bytes(buf))
|
remain_data_size = self.DataLength - self.ReceivedDataLength
|
data_size = min(buf_size, remain_data_size)
|
if 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(bytes(buf))
|
self._receive_footer(buf)
|
|
class UDP2PointcloudThread(threading.Thread):
|
def __init__(self,
|
udp_queue:queue.Queue,
|
pointcloud_queue:queue.Queue,
|
|
|
):
|
|
self.udp_queue = udp_queue
|
self.pointcloud_queue = pointcloud_queue
|
|
super().__init__()
|
|
|
def run(self):
|
receiver = MicroWaveReceiver()
|
print(time.asctime(),"DP2Pointcloud Thread is starting to decode the udp packets... ")
|
run = True
|
|
while run:
|
while not self.udp_queue.empty():
|
udp = self.udp_queue.get()
|
|
if udp is not False:
|
receiver.AddBuf(udp)
|
if receiver.DataFrameReady:
|
buf = receiver.DataFrame
|
buf_data = MicroWaveParser.ParserFrame(buf)
|
|
for frame in buf_data:
|
Frame_id = frame[0]
|
FrameNumber = frame[1]
|
point_cloud = np.array(frame[2])
|
if len(point_cloud) == 0 :
|
print(f"Notice: Frame {FrameNumber} is empty!")
|
else:
|
print(time.asctime(),f"Frame {FrameNumber} ready")
|
self.pointcloud_queue.put(point_cloud)
|
else:
|
run = False
|
self.pointcloud_queue.put(False)
|
print(time.asctime(),"DP2Pointcloud Thread finshed to decode the udp packets... ")
|
|
|
class MicroWaveRequestHandler(BaseRequestHandler):
|
"""
|
micro wave udp request handler
|
"""
|
def __init__(self,
|
request,
|
client_address,
|
server: BaseServer) -> None:
|
super().__init__(request, client_address, server)
|
|
def handle(self):
|
# Get message and client socket
|
if not self.server.udp_get_stop.isSet():
|
buf, _ = self.request
|
packed_count = self.server.packed_count
|
print(f"{time.asctime()} | Got connection from {self.client_address}, {packed_count}")
|
self.server.udp_queue.put(buf)
|
self.server.packed_count += 1
|
# else:
|
# UDPServer.server_close(self.server)
|
# self.server.udp_queue.put(False)
|
# print(f"{time.asctime()} | UDP-Get server is stopped by thresholds or keyborad interrupt.")
|
|
|
|
class MicroWaveUDPServer(UDPServer):
|
"""
|
micro wave udp server
|
"""
|
def __init__(self,
|
server_address: tuple[str,int],
|
RequestHandlerClass: MicroWaveRequestHandler,
|
udp_queue:queue.Queue,
|
udp_get_stop:threading.Event,
|
packet_threshold:Union[int, None],
|
timeusage_threshold:Union[int, None]
|
) -> None:
|
|
self.server_address = server_address
|
self.RequestHandlerClass = MicroWaveRequestHandler
|
self.udp_queue = udp_queue
|
self.udp_get_stop = udp_get_stop
|
self.udp_get_stop.clear()
|
self.packet_threshold = packet_threshold
|
self.timeusage_threshold = timeusage_threshold # mins
|
self.packed_count = 0
|
|
super().__init__(server_address, RequestHandlerClass)
|
print(f"[*] | Listening on address {server_address}")
|
|
# record start time of the server
|
if self.timeusage_threshold is not None:
|
self.start_time = time.time()
|
|
def service_actions(self) -> None:
|
|
# check to stop the server via muti-thresdhold
|
if self.packet_threshold is not None and \
|
self.packed_count >= self.packet_threshold:
|
|
self.udp_get_stop.set()
|
|
if self.timeusage_threshold is not None:
|
cur_time = time.time() # seconds
|
time_usage = int(cur_time - self.start_time)
|
|
if time_usage > 60*self.timeusage_threshold:
|
self.udp_get_stop.set()
|
|
if self.udp_get_stop.isSet():
|
print(f"{time.asctime()} | UDP-Get server is stopped by thresholds or keyborad interrupt.")
|
self.udp_queue.put(False)
|
self._BaseServer__shutdown_request = True
|
# self.__is_shut_down.set()
|
|
|
|
|
class MicroWaveUDPThread(threading.Thread):
|
def __init__(self,
|
MicrowaveHost:str,
|
MicrowavePort:int,
|
udp_queue:queue.Queue,
|
udp_get_stop:threading.Event,
|
packet_threshold:Union[int, None],
|
timeusage_threshold:Union[int, None]
|
):
|
super().__init__()
|
|
self.MicrowaveHost = MicrowaveHost
|
self.MicrowavePort = MicrowavePort
|
self.udp_queue = udp_queue
|
self.udp_get_stop = udp_get_stop
|
self.packet_threshold = packet_threshold
|
self.timeusage_threshold = timeusage_threshold
|
|
def stop(self):
|
"""stop the server to stop the """
|
self.udp_get_stop.set()
|
|
def run(self):
|
logs_list = []
|
server = MicroWaveUDPServer(server_address=(self.MicrowaveHost,self.MicrowavePort),
|
RequestHandlerClass=MicroWaveRequestHandler,
|
udp_queue=self.udp_queue,
|
udp_get_stop=self.udp_get_stop,
|
packet_threshold=self.packet_threshold,
|
timeusage_threshold=self.timeusage_threshold
|
)
|
try:
|
print(time.asctime(),"UDP Getting Server is starting to capture the udp packet... ")
|
server.serve_forever()
|
print(time.asctime(),"UDP Getting Server finshed to capture the udp packet ")
|
except Exception as e:
|
print(e)
|
server.server_close()
|
self.udp_get_stop.set()
|
print(time.asctime(),"[ERROR] UDP Getting Server Stopped with Unknown Reason!")
|
|
|
