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) super().shutdown() 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!")