From 8f8099f618d6b910103555a04ae3b9830e1e6c92 Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期二, 22 三月 2022 13:00:56 +0800
Subject: [PATCH] optimization for auto slam
---
src/utils/udp2bin.py | 44 ++++++++--
TEST_SlamAuto.bat | 13 ++-
src/mannul_slam.py | 5 +
src/slam/slam.py | 31 ++++++-
src/auto_slam.py | 34 ++++---
.vscode/launch.json | 1
src/utils/tools.py | 3
src/utils/udp_get.py | 57 +++++--------
env/ExportEnv.bat | 3
9 files changed, 118 insertions(+), 73 deletions(-)
diff --git a/.vscode/launch.json b/.vscode/launch.json
index ae2b71d..2381a38 100644
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -15,6 +15,7 @@
"COM11",
"100",
"5",
+ "5",
"0",
"20",
"auto",
diff --git a/TEST_SlamAuto.bat b/TEST_SlamAuto.bat
index a56d66b..8c5bcd8 100644
--- a/TEST_SlamAuto.bat
+++ b/TEST_SlamAuto.bat
@@ -14,8 +14,9 @@
@REM pre-defined parameters
set port=COM11
-set speed=100
+set speed=200
set degree_step=5
+set time_delay=5
set alpha=0
set beta=20
set gamma=auto
@@ -26,12 +27,13 @@
@REM re-defined parameters
set /p port=Please input the serial port of rotation machine (example: COM11):
-set /P speed=Please input the speed of rotation machine (example: 100):
-set /P degree_step=Please input the degree step of rotation machine (example: 5):
+set /P speed=Please input the speed of rotation machine (example: 200):
+@REM set /P degree_step=Please input the degree step of rotation machine (example: 5):
+set /P time_delay=Please input the delay time of rotation machine (example: 5):
set /P alpha=Please input the alpha degree of microwave device (example: 0/-20/auto):
set /P beta=Please input the beta degree of microwave device (example: 0/-20/auto):
set /P gamma=Please input the gamma degree of microwave device (example: 0/-20/auto):
-set /p udp_packets=Please input the udp packets num (example: 1000):
+@REM set /p udp_packets=Please input the udp packets num (example: 1000):
set /p saveFolder=Please input the folder to save the data (example: ./data/run_test/):
set PLYfile=%saveFolder%/data.ply
@@ -41,6 +43,7 @@
echo port :%port%
echo speed :%speed%
echo degree_step :%degree_step%
+echo time_delay :%time_delay%
echo alpha :%alpha%
echo beta :%beta%
echo gamma :%gamma%
@@ -56,7 +59,7 @@
echo change to path: %cd%
if %choice% == y (
- %aerialpython% ./src/auto_slam.py %port% %speed% %degree_step% %alpha% %beta% %gamma% %microwaveHost% %udp_packets% %deviceName% %saveFolder%
+ %aerialpython% ./src/auto_slam.py %port% %speed% %degree_step% %time_delay% %alpha% %beta% %gamma% %microwaveHost% %udp_packets% %deviceName% %saveFolder%
@REM -------------- Usage: plot_ply.py [PLYfile]"
%aerialpython% ./src/utils/plot_ply.py %PLYfile%
) else (
diff --git a/env/ExportEnv.bat b/env/ExportEnv.bat
index 69604f1..9956ec6 100644
--- a/env/ExportEnv.bat
+++ b/env/ExportEnv.bat
@@ -3,8 +3,9 @@
@REM change to the filepath
cd %~dp0
echo change to path: %cd%
+set currentenv=%cd%/aerial_deploy/
echo Exporting env ...
-conda pack -o aerial.tar.gz
+conda pack -p %currentenv% -o aerial.tar.gz -f
echo Done!
diff --git a/src/auto_slam.py b/src/auto_slam.py
index 1a15834..dfac28c 100644
--- a/src/auto_slam.py
+++ b/src/auto_slam.py
@@ -8,16 +8,18 @@
cudir = src_path = os.path.abspath(os.path.join(__file__, *(['..'] * 1)))
exe_path = os.path.abspath(os.path.join(cudir,"../RUN/Test_Servo.exe"))
-def run(port,speed,degre_step,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):
+def run(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):
# 鏃嬭浆鏈烘瀯澶嶄綅
- os.system(f"{exe_path} {port} 100 0")
+ os.system(f"{exe_path} {port} 400 0")
+ time.sleep(10)
# 闂撮殧鑾峰彇udp
for i in range(0,360,degre_step):
os.system(f"{exe_path} {port} {speed} {i}")
- time.sleep(3)
+ time.sleep(int(time_delay))
subFolder = str(-i)
+ print (f"Geting udp files with {subFolder} degree ...")
udp_get_tool(microwaveHost,udp_packets,deviceName,saveFolder,subFolder)
# 鏃嬭浆鏈烘瀯澶嶄綅
@@ -35,32 +37,34 @@
# compose ply
+ print("merge ply files, processing ...")
compose_muti_ply(saveFolder,alpha,beta,gamma,"data")
if __name__ == "__main__":
def help():
- print("Usage: auto_slam.py [port] [speed] [degre_step] [alpha] [beta] [gamma] [microwaveHost] [udp_packets] [deviceName] [saveFolder]")
- print("EXAMPLE: auto_slam.py COM11 100 5 0 20 auto 192.168.2.65 1000 pod1 ./data/run_test/")
+ print("Usage: auto_slam.py [port] [speed] [degre_step] [time_delay] [alpha] [beta] [gamma] [microwaveHost] [udp_packets] [deviceName] [saveFolder]")
+ print("EXAMPLE: auto_slam.py COM11 100 5 5 0 20 auto 192.168.2.65 1000 pod1 ./data/run_test/")
exit(1)
- if len(sys.argv) != 11:
+ if len(sys.argv) != 12:
help()
try:
port = str(sys.argv[1])
speed = str(sys.argv[2])
- degre_step = str(sys.argv[3])
- alpha = str(sys.argv[4])
- beta = str(sys.argv[5])
- gamma = str(sys.argv[6])
- microwaveHost = str(sys.argv[7])
- udp_packets = int(sys.argv[8])
- deviceName = str(sys.argv[9])
- saveFolder = str(sys.argv[10])
+ degre_step = int(sys.argv[3])
+ time_delay = int(sys.argv[4])
+ alpha = str(sys.argv[5])
+ beta = str(sys.argv[6])
+ gamma = str(sys.argv[7])
+ microwaveHost = str(sys.argv[8])
+ udp_packets = int(sys.argv[9])
+ deviceName = str(sys.argv[10])
+ saveFolder = str(sys.argv[11])
except Exception as e:
print(e)
help()
- run(port,speed,degre_step,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
+ run(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
print("Done")
diff --git a/src/mannul_slam.py b/src/mannul_slam.py
index 215414e..55fd0af 100644
--- a/src/mannul_slam.py
+++ b/src/mannul_slam.py
@@ -2,6 +2,7 @@
import numpy as np
from slam.slam import compose_ply
from utils import read_ply,write_ply,file_fliter
+from tqdm import tqdm
def run(folder,alpha,beta,gamma,savename):
@@ -28,8 +29,9 @@
data = []
+ print("reading ply files ...")
# iterate the auto angle degree
- for an in angle[index]:
+ for an in tqdm(angle[index]):
ply_with_mutiangle = []
for z in angle:
if isinstance(z,list):
@@ -47,6 +49,7 @@
cloudpoint = compose_ply(data)
# write the file
+ print("Writing the data to disk ...")
write_ply(cloudpoint, folder, savename)
diff --git a/src/slam/slam.py b/src/slam/slam.py
index be8a376..de3904d 100644
--- a/src/slam/slam.py
+++ b/src/slam/slam.py
@@ -6,6 +6,7 @@
sys.path.insert(0, src_path)
from utils.tools import read_ply
+from tqdm import tqdm
@@ -76,12 +77,33 @@
data = rotated_ply
return data
+def transferToZero(data,x,y,z,alpha,beta,gamma,reverse=False):
+ """
+ 杞崲褰撳墠鍧愭爣鑷冲彟涓�鍧愭爣, xyz鍒嗗埆涓哄綋鍓嶅潗鏍囧師鐐瑰湪鐩爣鍧愭爣绯荤殑浣嶇疆;
+ alpha,beta,gamma 涓哄綋鍓嶅潗鏍囩郴鐩稿鐩爣鍧愭爣绯荤殑鏃嬭浆鍙傛暟銆�
+ """
+
+ # 骞崇Щ
+ moved_data = current + np.array([x,y,z])
+ # 鏃嬭浆
+ rotation_matrix = get_rotation_matrix(-alpha,-beta,-gamma)
+ rotated_data = np.matmul(moved_data,rotation_matrix)
+
+ # 宸﹀彸鎵嬪潗鏍囩郴杞崲, 浣跨敤z杞磋浆鎹�
+
+ if reverse:
+ rotated_data[:,-1] = -rotated_data.copy()[:,-1]
+
+ return rotated_data
+
+
def compose_ply(muti_ply_with_angle: list):
ply_nums = len(muti_ply_with_angle)
composed_ply_array = None
- for i, ply_with_angle in enumerate( muti_ply_with_angle):
+ print ("rotating the ply ...")
+ for i, ply_with_angle in enumerate(tqdm(muti_ply_with_angle)):
angle = ply_with_angle[:-1]
ply = ply_with_angle[-1]
@@ -94,12 +116,11 @@
# np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
rotated_ply = np.matmul(ply,rotation_matrix)
if i == 0:
- composed_ply_array = np.array(rotated_ply)
+ composed_ply_array = rotated_ply.copy()
else:
- temp = np.array(rotated_ply)
- composed_ply_array = np.concatenate((composed_ply_array,temp),axis= 0 )
+ composed_ply_array = np.concatenate((composed_ply_array,rotated_ply),axis= 0 )
- composed_ply_array = composed_ply_array.reshape(-1,3)
+ composed_ply_array = np.array(composed_ply_array).reshape(-1,3)
return composed_ply_array
diff --git a/src/utils/tools.py b/src/utils/tools.py
index 3db661f..80b55d1 100644
--- a/src/utils/tools.py
+++ b/src/utils/tools.py
@@ -1,6 +1,7 @@
import os, sys
import numpy as np
import time
+from tqdm import tqdm
def file_fliter(folder:str, file_ext:str, sort_format = None):
@@ -56,7 +57,7 @@
f.write("end_header\n")
n_p = 0
- for p in data:
+ for p in tqdm(data):
x = p[0]
y = p[1]
z = p[2]
diff --git a/src/utils/udp2bin.py b/src/utils/udp2bin.py
index 9715d43..dc3a703 100644
--- a/src/utils/udp2bin.py
+++ b/src/utils/udp2bin.py
@@ -11,25 +11,47 @@
from microwave.servers import MicroWaveReceiver
from .tools import file_fliter
-def udp2bin_tool(udp_folder):
+def udp2bin_tool(udp_folder_Or_list):
+ Save = True
+ bin_list = []
- UDPfolder = udp_folder
- # 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)
+ 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:
- filename = os.path.join(UDPfolder, file)
- with open(filename, "rb") as f:
- data = f.read()
- receiver.AddBuf(data)
- if receiver.DataFrameReady:
- frame_number = receiver.Header.FrameNumber
+ 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
+
+
diff --git a/src/utils/udp_get.py b/src/utils/udp_get.py
index 1dcea13..bf4f68e 100644
--- a/src/utils/udp_get.py
+++ b/src/utils/udp_get.py
@@ -12,26 +12,12 @@
# os.makedirs(file_folder)
# print(f"make a new folder: {file_folder}")
-packed_count = 0
-
class PointCloudUDPRequestHandler(BaseRequestHandler):
def handle(self):
-
- global packed_count
- global path
-
- if packed_count == 0:
- start = datetime.datetime.now()
- if not os.path.exists(save_folder):
- os.mkdir(save_folder)
- if sub_folder is not None:
- path = os.path.join(save_folder,sub_folder)
- else:
- path = os.path.join(save_folder,f"{device_id}-{start.strftime('%Y-%m-%d-%H-%M-%S')}")
- if not os.path.exists(path):
- os.mkdir(path)
- path = os.path.abspath(path)
+ packet_threshold = self.server.packet_threshold
+ packed_count = self.server.packed_count
+ path = self.server.path
print(time.asctime(),' Got connection from {}, {}'.format(self.client_address, packed_count))
# Get message and client socket
@@ -40,29 +26,33 @@
f = open('{}/{}_packet{}.udp'.format(path,time_tag,packed_count), 'wb')
f.write(msg)
f.close()
- packed_count += 1
+ self.server.packed_count += 1
+ self.server.udp_list.append(msg)
if packed_count >= packet_threshold:
- UDPServer.server_close(server)
-
+ UDPServer.server_close(self.server)
def udp_get_tool(microwaveHost,packetThreshold,deviceName,saveFolder,subFolder):
- global packet_threshold
- packet_threshold = packetThreshold,
- global device_id
- device_id = deviceName
- global save_folder
- save_folder = saveFolder
- global sub_folder
- sub_folder = subFolder
-
- global server
-
- # host = ''
+ # host = ''
# host = '192.168.2.65' # use this host to get the udp data from microwave device
port = 9911
server =UDPServer((microwaveHost, port), PointCloudUDPRequestHandler)
+
+
+ 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)
+
+ server.path = os.path.abspath(path)
+ server.packet_threshold = packetThreshold
+ server.packed_count = 0
+ server.udp_list = []
+
print(time.asctime(),'Server started on port', port)
print('....')
print('ctrl-c to quit server.')
@@ -74,8 +64,7 @@
except:
server.server_close()
print(time.asctime(),"Server Stopped")
-
-
+ return server.udp_list
if __name__ == '__main__':
--
Gitblit v1.9.1