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/demo/AutoSendUDPtest.bat | 4
src/protos/aerial.proto | 14 --
src/microwave/servers.py | 99 +++++++++++++---
src/microwave/deviceTools.py | 17 +-
src/engine/Microwave.py | 43 ++++---
src/engine/MessageThreads.py | 93 +++++++++++---
src/engine/AcquisitionMain.py | 36 +++--
src/engine/devices.py | 36 +++++
8 files changed, 241 insertions(+), 101 deletions(-)
diff --git a/src/engine/AcquisitionMain.py b/src/engine/AcquisitionMain.py
index 4c98a0f..a401666 100644
--- a/src/engine/AcquisitionMain.py
+++ b/src/engine/AcquisitionMain.py
@@ -80,6 +80,8 @@
self.CAMERA_COUNT = CAMERA_COUNT
# microwave
self.MICROWAVEhost = MICROWAVEhost
+ # self.MICROWAVEPort = MICROWAVEhost
+ self.MICROWAVEport = 9911
# dust cleaner
self.com = "COM11"
@@ -207,10 +209,16 @@
routing_key="server"
)
- self.mc = microwaveController()
-
-
-
+ self.mc = microwaveController(
+ MicrowaveHost=self.MICROWAVEhost,
+ MicrowavePort=self.MICROWAVEport,
+ init_packet_threshold=100,
+ init_timeusage_threshold=3,
+ device_id=self.device_id,
+ logs_queue=self.LogsSender_queue,
+ plys_queue=self.PlysSender_queue,
+ routing_key="server"
+ )
def run_rabbitmq(self):
@@ -236,15 +244,17 @@
print(f"stop {t.name}")
def start(self):
- try:
- self.run_all()
- print("[*] Client is runing ...")
- self.CommandsReceiver_Thread.join()
- except Exception as e:
- print(e)
- print("running error, stop all threads")
- self.stop_all()
- print("main thread end.")
+ # try:
+ # self.run_all()
+ # print("[*] Client is runing ...")
+ # self.CommandsReceiver_Thread.join()
+ # except Exception as e:
+ # print(e)
+ # print("running error, stop all threads")
+ # self.stop_all()
+ # print("main thread end.")
+ self.run_all()
+ self.CommandsReceiver_Thread.join()
return 0
diff --git a/src/engine/MessageThreads.py b/src/engine/MessageThreads.py
index d66975a..9775989 100644
--- a/src/engine/MessageThreads.py
+++ b/src/engine/MessageThreads.py
@@ -9,6 +9,7 @@
from typing import List
import numpy as np
import cv2
+from tomlkit import value
# # add python path of src to sys.path
# src_path = os.path.join(__file__, *(['..'] * 2))
@@ -288,7 +289,7 @@
# extracting the msg from queue
msg = queue.get()
- msg_pb = pb.PlyMessage()
+ msg_pb = pb.PlysMessage()
msg_pb.ParseFromString(msg)
# save the raw pb msg
@@ -309,7 +310,7 @@
rows = ply.rows
cols = ply.cols
order = ply.order
- dt = ply.d_types
+ dt = ply.dtype
# save the pointcloud array data
if (data_byte is None) | (len(data_byte) != data_legth):
@@ -317,7 +318,7 @@
else:
# frame the bytes to array
data_array = np.frombuffer(data_byte,dtype=dt)
- data_array = data_array.reshape(rows,cols)
+ data_array = data_array.reshape(rows,cols,order=order)
# save the data with .ply format
filename = f"{msg_source}_{msg_timestamp}_pointcloud{id}"
write_ply(data_array,self.plysmsg_folder,filename)
@@ -354,27 +355,75 @@
source:str,
routing_key:str):
- # constract a images message
- imagesMsg = pb.ImagesMessage()
- imagesMsg.source = source
- curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
- imagesMsg.timestamp = curr_time
+ # constract a images message
+ imagesMsg = pb.ImagesMessage()
+ imagesMsg.source = source
+ curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
+ imagesMsg.timestamp = curr_time
- for img in images:
- if img is not None:
- success,image_encoded = cv2.imencode(".jpg",img)
- if success:
- image_bytes = image_encoded.tobytes()
- image_length = len(image_bytes)
- # add the encoded image to msg
- image_item = imagesMsg.Image.add()
- image_item.length = image_length
- image_item.data = image_bytes
+ for img in images:
+ if img is not None:
+ success,image_encoded = cv2.imencode(".jpg",img)
+ if success:
+ image_bytes = image_encoded.tobytes()
+ image_length = len(image_bytes)
+ # add the encoded image to msg
+ image_item = imagesMsg.Image.add()
+ image_item.length = image_length
+ image_item.data = image_bytes
- # converting the pb image msg to bytes
- imageMsg_bytes = imagesMsg.SerializeToString()
- # add the imageMsg to the queue
- images_queue.put([routing_key,imageMsg_bytes])
+ # converting the pb image msg to bytes
+ imageMsg_bytes = imagesMsg.SerializeToString()
+ # add the imageMsg to the queue
+ images_queue.put([routing_key,imageMsg_bytes])
+
+def send_pointcloud_tool(pointclouds:queue.Queue,
+ plys_queue:queue.Queue,
+ source:str,
+ routing_key:str):
+
+ # constract a plys message
+ plysMsg = pb.PlysMessage()
+ plysMsg.source = source
+ curr_time = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
+ plysMsg.timestamp = curr_time
+
+
+ run =True
+ while run:
+ while not pointclouds.empty():
+
+ value = pointclouds.get()
+
+ if value is not False:
+ pointcloud = np.array(value)
+ # get the attributes of the PointCloudItem
+ rows = pointcloud.shape[0]
+ cols = pointcloud.shape[1]
+ order = "C"
+ dtype = str(pointcloud.dtype)
+ data = pointcloud.tobytes(order=order)
+ length = len(data)
+
+ # add the pointcloud to the msg
+ item = plysMsg.Plydata.add()
+ item.length = length
+ item.rows = rows
+ item.cols = cols
+ item.order = order
+ item.dtype = dtype
+ item.data = data
+
+ else:
+ # converting the pb ply msg to bytes
+ plysMsg_bytes = plysMsg.SerializeToString()
+ # add the plysMsg to the queue
+ plys_queue.put([routing_key,plysMsg_bytes])
+
+ # end the outer while loop and triminate this function
+ run = False
+
+
diff --git a/src/engine/Microwave.py b/src/engine/Microwave.py
index a59cfed..de3f410 100644
--- a/src/engine/Microwave.py
+++ b/src/engine/Microwave.py
@@ -3,9 +3,10 @@
import time
from datetime import datetime
from typing import Union
+import threading
from .MessageThreads import send_logs_tool
-from ..microwave.servers import MicroWaveUDPThread
+from ..microwave.servers import MicroWaveUDPThread,UDP2PointcloudThread
class micorwaveMannager(object):
@@ -13,43 +14,47 @@
def __init__(self,
MicrowaveHost:str,
MicrowavePort:int,
- packet_threshold:Union[int, None],
- timeusage_threshold:Union[int, None]
-
-
- ):
+ ):
# microwave
self.MicrowaveHost = MicrowaveHost
self.MicrowavePort = MicrowavePort
# udp get
- self.packet_threshold = packet_threshold
- self.timeusage_threshold = timeusage_threshold
+ self._udp_get_thread = None
+ # self.packet_threshold = packet_threshold
+ # self.timeusage_threshold = timeusage_threshold
self.udp_queue = queue.Queue()
- self.udp_get_stop = {"value":False}
+ self.udp_get_stop = threading.Event()
self._udp_get_thread_name = "Thread-UDPGet"
# udp to pointcloud
+ self._udp2pointcloud_thread = None
+ self.pointcloud_queue = queue.Queue()
+ self._udp2pointcloud_thread_name = "Thread-UDP2PointCloud"
- def _udp_get_thread_launcher(self):
+ def _udp_get_thread_launcher(self,
+ packet_threshold:Union[int, None],
+ timeusage_threshold:Union[int, None]):
+ self.udp_get_stop.clear()
self._udp_get_thread = MicroWaveUDPThread(MicrowaveHost=self.MicrowaveHost,
MicrowavePort=self.MicrowavePort,
udp_queue=self.udp_queue,
udp_get_stop=self.udp_get_stop,
- packet_threshold=self.packetThreshold,
- timeusage_threshold=self.timeusage_threshold)
+ packet_threshold=packet_threshold,
+ timeusage_threshold=timeusage_threshold)
self._udp_get_thread.setDaemon(True)
self._udp_get_thread.setName(self._udp_get_thread_name)
+ self._udp_get_thread.start()
def _udp2pointcloud_thread_launcher(self):
-
- pass
-
-
-
-
-
+ self._udp2pointcloud_thread = UDP2PointcloudThread(
+ udp_queue = self.udp_queue,
+ pointcloud_queue=self.pointcloud_queue
+ )
+ self._udp2pointcloud_thread.setDaemon(True)
+ self._udp2pointcloud_thread.setName(self._udp2pointcloud_thread_name)
+ self._udp2pointcloud_thread.start()
\ No newline at end of file
diff --git a/src/engine/devices.py b/src/engine/devices.py
index 07f5d32..eb34a80 100644
--- a/src/engine/devices.py
+++ b/src/engine/devices.py
@@ -4,7 +4,7 @@
import threading
import numpy as np
import os, sys
-from typing import List
+from typing import List, Union
import queue
from datetime import datetime
@@ -15,7 +15,8 @@
from ..protos import aerial_pb2 as pb
from .DustCleaner import DustCleaner
-from .MessageThreads import send_logs_tool, send_images_tool
+from .Microwave import micorwaveMannager
+from .MessageThreads import send_logs_tool, send_images_tool, send_pointcloud_tool
######################################### CAMERAS ############################################
@@ -144,9 +145,36 @@
######################################## MICROWAVE ###########################################
-class microwaveController(object):
- pass
+class microwaveController(micorwaveMannager):
+ def __init__(self,
+ MicrowaveHost: str,
+ MicrowavePort: int,
+ init_packet_threshold: Union[int, None],
+ init_timeusage_threshold: Union[int, None],
+ device_id:str,
+ logs_queue:queue.Queue,
+ plys_queue:queue.Queue,
+ routing_key:str,
+ ):
+ super().__init__(MicrowaveHost, MicrowavePort)
+ self.device_id = device_id
+ self.plys_queue = plys_queue
+ self.routing_key = routing_key
+ # testing the device when instance the class
+ self._get_pointcloud(init_packet_threshold,init_timeusage_threshold)
+ def _get_pointcloud(self,
+ packet_threshold:Union[int, None],
+ timeusage_threshold:Union[int, None]):
+ self._udp_get_thread_launcher(packet_threshold,timeusage_threshold)
+ self._udp2pointcloud_thread_launcher()
+ self._generate_msg()
+
+ def _generate_msg(self):
+ send_pointcloud_tool(pointclouds=self.pointcloud_queue,
+ plys_queue=self.plys_queue,
+ source=self.device_id,
+ routing_key=self.routing_key+".plys.test0")
####################################### DUST CLEANER #########################################
class dustCleanerController(DustCleaner):
diff --git a/src/microwave/demo/AutoSendUDPtest.bat b/src/microwave/demo/AutoSendUDPtest.bat
index f321d14..426321c 100644
--- a/src/microwave/demo/AutoSendUDPtest.bat
+++ b/src/microwave/demo/AutoSendUDPtest.bat
@@ -4,8 +4,8 @@
cd %~dp0
echo change to path: %cd%
-set aerialpython=%cd%/../../../env/aerial_deploy/python.exe
-set envsetting=%cd%/env/SettingEnv.bat
+set aerialpython=%cd%/../../../.env/python.exe
+set envsetting=%cd%/../../../env/SettingEnv.bat
@REM Setting the env
if not exist %aerialpython% (
diff --git a/src/microwave/deviceTools.py b/src/microwave/deviceTools.py
index e57379a..01af872 100644
--- a/src/microwave/deviceTools.py
+++ b/src/microwave/deviceTools.py
@@ -6,8 +6,9 @@
import open3d as o3d
from multiprocessing import Pool
import numpy as np
+import threading
-from ..utils import file_fliter, write_ply
+from ..utils.tools import file_fliter, write_ply
from .servers import MicroWaveReceiver,MicroWaveUDPThread
from .parsers import MicroWaveParser
@@ -18,7 +19,7 @@
# host = '192.168.2.65' # use this host to get the udp data from microwave device
port = 9911
udp_queue = queue.Queue()
- udp_get_stop = {"value": False}
+ udp_get_stop = threading.Event()
if subFolder is not None:
path = os.path.join(saveFolder,subFolder)
@@ -48,9 +49,9 @@
while run:
packed_count = 0
- while run or not udp_queue.empty():
+ while not udp_queue.empty():
value = udp_queue.get()
- while value is not False:
+ if value is not False:
time_tag = datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
with ('{}/{}_packet{}.udp'.format(path,time_tag,packed_count), 'wb') as f:
f.write(udp_queue.get())
@@ -59,11 +60,11 @@
run = False
except KeyboardInterrupt:
- udp_get_stop = {"value": True}
+ udp_get_stop.set()
print(time.asctime(),"Server Stopped")
except:
- udp_get_stop = {"value": True}
- print(time.asctime(),"Server Stopped")
+ udp_get_stop.set()
+ print(time.asctime(),"Server Stopped with unknown reasons")
# udp2bin
def udp2bin_tool(udp_folder_Or_list):
@@ -76,7 +77,7 @@
else:
UDPfolder = udp_folder_Or_list
# example pod1-2022-02-21-13-25-03_packet125.udp --> 125
- sort_lambda = lambda x:int(x.split('packet')[1][:-4])
+ sort_lambda = lambda x:int(x.split('packet')[-1][:-4])
udp_files = file_fliter(UDPfolder,'udp', sort_lambda)
Save = True
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!")
diff --git a/src/protos/aerial.proto b/src/protos/aerial.proto
index bbab4ff..c03a45b 100644
--- a/src/protos/aerial.proto
+++ b/src/protos/aerial.proto
@@ -138,18 +138,8 @@
}
-enum DataType
-{
- c_int16 = 0;
- c_int32 = 1;
- c_int64 = 2;
- c_float16 = 3;
- c_float32 = 4;
- c_float = 5;
-}
-
-message PlyMessage
+message PlysMessage
{
string source = 1;
string timestamp = 2;
@@ -160,7 +150,7 @@
int32 rows = 2;
int32 cols = 3;
string order = 4;
- DataType d_types = 5;
+ string dtype = 5;
bytes data = 6;
}
--
Gitblit v1.9.1