From 6b37ae2b15eca92b3cacac0e113290eb28ccfa8b Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期一, 20 六月 2022 15:49:50 +0800
Subject: [PATCH] get pointclouds from client
---
src/microwave/servers.py | 99 +++++++++++++++++++++++++++++++++++++++----------
1 files changed, 78 insertions(+), 21 deletions(-)
diff --git a/src/microwave/servers.py b/src/microwave/servers.py
index fec7edd..c678379 100644
--- a/src/microwave/servers.py
+++ b/src/microwave/servers.py
@@ -1,13 +1,16 @@
import os, sys
from ctypes import sizeof, string_at
-from socketserver import _AddressType, _RequestType, BaseRequestHandler, BaseServer, UDPServer
+# 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
@@ -100,6 +103,7 @@
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
@@ -107,7 +111,7 @@
# 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 ...")
+ warnings.warn(f"encounter an unknow error [{e}] when getting the header data in udp packets ...")
result = False
if not result:
@@ -138,6 +142,48 @@
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):
@@ -145,23 +191,23 @@
micro wave udp request handler
"""
def __init__(self,
- request: _RequestType,
- client_address: _AddressType,
+ request,
+ client_address,
server: BaseServer) -> None:
super().__init__(request, client_address, server)
def handle(self):
# Get message and client socket
- while not self.server.udp_get_stop["value"]:
+ 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()} | Server is stopped by thresholds or keyborad interrupt.")
+ # 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.")
@@ -173,7 +219,7 @@
server_address: tuple[str,int],
RequestHandlerClass: MicroWaveRequestHandler,
udp_queue:queue.Queue,
- udp_get_stop:dict,
+ udp_get_stop:threading.Event,
packet_threshold:Union[int, None],
timeusage_threshold:Union[int, None]
) -> None:
@@ -182,32 +228,38 @@
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
- while self.timeusage_threshold is not None:
+ if self.timeusage_threshold is not None:
self.start_time = time.time()
- def close_request(self, request: _RequestType) -> None:
+ def service_actions(self) -> None:
# check to stop the server via muti-thresdhold
- while self.packet_threshold is not None and \
+ if self.packet_threshold is not None and \
self.packed_count >= self.packet_threshold:
- self.udp_get_stop["value"] = True
+ self.udp_get_stop.set()
- while self.timeusage_threshold is not None:
+ 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["value"] = True
+ self.udp_get_stop.set()
- return super().close_request(request)
+ 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()
+
@@ -216,7 +268,7 @@
MicrowaveHost:str,
MicrowavePort:int,
udp_queue:queue.Queue,
- udp_get_stop:dict,
+ udp_get_stop:threading.Event,
packet_threshold:Union[int, None],
timeusage_threshold:Union[int, None]
):
@@ -231,9 +283,10 @@
def stop(self):
"""stop the server to stop the """
- self.udp_get_stop["value"] = True
+ 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,
@@ -242,10 +295,14 @@
timeusage_threshold=self.timeusage_threshold
)
try:
+ print(time.asctime(),"UDP Getting Server is starting to capture the udp packet... ")
server.serve_forever()
- except:
+ print(time.asctime(),"UDP Getting Server finshed to capture the udp packet ")
+ except Exception as e:
+ print(e)
server.server_close()
- print(time.asctime(),"UDP Getting Server Stopped with Unknown Reason!")
+ self.udp_get_stop.set()
+ print(time.asctime(),"[ERROR] UDP Getting Server Stopped with Unknown Reason!")
--
Gitblit v1.9.1