import datetime
|
import queue
|
from socketserver import BaseRequestHandler, UDPServer
|
import time
|
import os, sys
|
import open3d as o3d
|
from multiprocessing import Pool
|
import numpy as np
|
import threading
|
|
from ..utils.tools import file_fliter, write_ply
|
from .servers import MicroWaveReceiver,MicroWaveUDPThread
|
from .parsers import MicroWaveParser
|
|
|
def udp_get_tool(microwaveHost,packetThreshold,deviceName,saveFolder,subFolder):
|
|
# host = ''
|
# 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 = threading.Event()
|
|
if subFolder is not None:
|
path = os.path.join(saveFolder,subFolder)
|
else:
|
start = datetime.datetime.now()
|
path = os.path.join(saveFolder,f"{deviceName}-{start.strftime('%Y-%m-%d-%H-%M-%S')}")
|
if not os.path.exists(path):
|
os.makedirs(path)
|
|
udp_thread = MicroWaveUDPThread(MicrowaveHost=microwaveHost,
|
MicrowavePort=port,
|
udp_queue=udp_queue,
|
udp_get_stop=udp_get_stop,
|
packet_threshold=packetThreshold,
|
timeusage_threshold=None)
|
|
udp_thread.setDaemon(True)
|
udp_thread.setName("UDPGet_Thread")
|
|
print(time.asctime(),'Server started on port', port)
|
print('....')
|
print('ctrl-c to quit server.')
|
try:
|
udp_thread.start()
|
|
run = True
|
|
while run:
|
packed_count = 0
|
while not udp_queue.empty():
|
value = udp_queue.get()
|
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())
|
packed_count += 1
|
else:
|
run = False
|
|
except KeyboardInterrupt:
|
udp_get_stop.set()
|
print(time.asctime(),"Server Stopped")
|
except:
|
udp_get_stop.set()
|
print(time.asctime(),"Server Stopped with unknown reasons")
|
|
# udp2bin
|
def udp2bin_tool(udp_folder_Or_list):
|
Save = True
|
bin_list = []
|
|
if isinstance(udp_folder_Or_list, list):
|
udp_files = udp_folder_Or_list
|
Save = False
|
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])
|
udp_files = file_fliter(UDPfolder,'udp', sort_lambda)
|
Save = True
|
|
receiver = MicroWaveReceiver()
|
|
|
for file in udp_files:
|
data = None
|
if isinstance(file,str):
|
filename = os.path.join(UDPfolder, file)
|
with open(filename, "rb") as f:
|
data = f.read()
|
else:
|
data = file
|
|
receiver.AddBuf(data)
|
if receiver.DataFrameReady:
|
frame_number = receiver.Header.FrameNumber
|
if Save:
|
bin_name = os.path.join(UDPfolder, "{}.bin".format(frame_number))
|
print(time.asctime(),"Frame {} ready".format(frame_number))
|
with open(bin_name, "wb") as bin_file:
|
bin_file.write(receiver.DataFrame)
|
bin_file.close()
|
|
bin_list.append((frame_number,receiver.DataFrame))
|
|
return bin_list
|
|
def bin2ply_f(folder,file):
|
|
with open(os.path.join(folder,file), "rb") as bin_file:
|
print(f"Extracting the data in {file} ...")
|
buf = bin_file.read()
|
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:
|
write_ply(point_cloud,folder,str(FrameNumber))
|
|
# bin2ply
|
def bin2ply_tool(folder,nums_worker = 4):
|
pool = Pool(processes = nums_worker)
|
|
bin_files = file_fliter(folder,'bin')
|
|
for file in bin_files:
|
pool.apply_async(func=bin2ply_f,args=(folder,file,))
|
|
pool.close()
|
pool.join()
|
|
|
def plot_ply(path):
|
if isinstance(path,str):
|
abspath = os.path.abspath(path)
|
else:
|
raise ValueError("Not a path!")
|
|
if os.path.isdir(abspath):
|
# open folder to load all ply files
|
ply_files = file_fliter(abspath,'ply')
|
show = []
|
for ply in ply_files:
|
pcd = o3d.io.read_point_cloud(os.path.join(abspath,ply))
|
show.append(pcd)
|
else:
|
show = [o3d.io.read_point_cloud(abspath)]
|
|
o3d.visualization.draw_geometries(show)
|
return
|
