From da6ff0858f4997f79f5d8cdc88b7367a5cb710b1 Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期二, 07 六月 2022 10:55:26 +0800
Subject: [PATCH] Merge commit 'mqdev' into mqaddFeature
---
.gitignore | 5
src/engine/MessageQueueThread.py | 12
src/protos/compile.bat | 5
scripts_launch/TEST_SlamAuto.bat | 15
src/utils/tools.py | 79 ++++---
RUN/Test_Servo.pdb | 0
src/engine/ControllThread.py | 10
src/slam/manualMode.py | 2
scripts/mqServer.py | 33 +++
src/Client.py | 85 +++---
src/Server.py | 32 +-
scripts_launch/TEST_startServer.bat | 7
RUN/Test_Servo.dll | 0
src/engine/DustCleanerThread.py | 10
src/engine/MicroWaveUDPThread.py | 2
src/microwave/servers.py | 122 ++++++----
scripts/mqClient.py | 54 ++++
env/SettingEnv.bat | 6
scripts_launch/TEST_startClient.bat | 4
src/microwave/parsers.py | 2
env/JustDoubleClickMe.bat | 3
src/__init__.py | 5
src/engine/AcquisitionMain.py | 10
src/slam/autoMode.py | 9
/dev/null | 12 -
src/engine/CaptureThread.py | 10
src/microwave/deviceTools.py | 53 ++--
src/slam/core.py | 27 +
scripts/slamAutoMode.py | 31 +-
29 files changed, 393 insertions(+), 252 deletions(-)
diff --git a/.gitignore b/.gitignore
index e9715de..3487ac0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -4,6 +4,11 @@
# proto
src/protos/aerial_pb2.py
+# projects
+.vscode
+.vs
+.idea
+
# env
env/aerial_deploy
.env/
diff --git a/.vscode/launch.json b/.vscode/launch.json
deleted file mode 100644
index e156e93..0000000
--- a/.vscode/launch.json
+++ /dev/null
@@ -1,132 +0,0 @@
-{
- // 浣跨敤 IntelliSense 浜嗚В鐩稿叧灞炴�с��
- // 鎮仠浠ユ煡鐪嬬幇鏈夊睘鎬х殑鎻忚堪銆�
- // 娆蹭簡瑙ｆ洿澶氫俊鎭紝璇疯闂�: https://go.microsoft.com/fwlink/?linkid=830387
- "version": "0.2.0",
- "configurations": [
- {
- "name": "test auto slam",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/scripts/slamAutoMode.py",
- "console": "integratedTerminal",
- "args": [
- "COM11",
- "100",
- "5",
- "5",
- "0",
- "20",
- "auto",
- "192.168.2.65",
- "1000",
- "testDevice",
- "../../04_Data/20220316test"]
- },
- {
- "name": "test_slam",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/scripts/slamManualMode.py",
- "console": "integratedTerminal",
- "args": [
- "../../04_Data/20220316test",
- "0",
- "20",
- "auto",
- "test60120"
- ],
- "justMyCode": true
- },
- {
- "name": "Client_test",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/src/Client.py",
- "args": [
- "192.168.1.14",
- "chai",
- "password123",
- "aerial_rpc",
- "pod1",
- "http://192.168.1.224/api",
- "localhost"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "Client_production",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/src/Client.py",
- "args": [
- "172.17.17.206",
- "chai",
- "password123",
- "aerial_rpc",
- "pod1",
- "http://172.17.17.206/api",
- "192.168.2.65"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "Server_test",
- "type": "python",
- "python": "${workspaceFolder}/.env/python.exe",
- "request": "launch",
- "program": "${workspaceFolder}/src/Server.py",
- "args": [
- "localhost",
- "chai",
- "password123",
- "0.0.0.0",
- "5000",
- "./Received_data_test"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "Server_production",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/src/Server.py",
- "args": [
- "172.17.17.206",
- "chai",
- "password123",
- "aerial_rpc",
- "5000",
- "./Images"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "udp2bin",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/scripts/udp2bin.py",
- "args": [
- "./src/microwave/demo/demoData"
- ],
- "console": "integratedTerminal"
- },
- {
- "name": "bin2ply",
- "type": "python",
- "request": "launch",
- "python": "${workspaceFolder}/.env/python.exe",
- "program": "${workspaceFolder}/scripts/bin2ply.py",
- "args": [
- "../../00_Docs/mutiFrameBin"
- ],
- "console": "integratedTerminal"
- }
- ]
-}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
deleted file mode 100644
index 4479c53..0000000
--- a/.vscode/settings.json
+++ /dev/null
@@ -1,12 +0,0 @@
-{
-
- "python.poetryPath": "${workspaceFolder}/.env/python.exe",
- "jupyter.kernels.filter": [
- {
- "type": "jupyterKernelspec",
- "path": "${workspaceFolder}/.env/python.exe"
- }
- ],
- "python.envFile": "${workspaceFolder}/.env",
- "python.defaultInterpreterPath": "${workspaceFolder}/.env/python.exe"
-}
\ No newline at end of file
diff --git a/RUN/Test_Servo.dll b/RUN/Test_Servo.dll
index bf249d6..8cdeaad 100644
--- a/RUN/Test_Servo.dll
+++ b/RUN/Test_Servo.dll
Binary files differ
diff --git a/RUN/Test_Servo.pdb b/RUN/Test_Servo.pdb
index e8760a3..7d98179 100644
--- a/RUN/Test_Servo.pdb
+++ b/RUN/Test_Servo.pdb
Binary files differ
diff --git a/env/JustDoubleClickMe.bat b/env/JustDoubleClickMe.bat
index b8dfd94..b4b9ffc 100644
--- a/env/JustDoubleClickMe.bat
+++ b/env/JustDoubleClickMe.bat
@@ -6,7 +6,8 @@
echo setting done!
echo ========== Testing Env ==============
echo test it with hello word...
-set aerialpython=%cd%/../../aerial_deploy/python.exe
+cd %~dp0
+set aerialpython=%cd%/../.env/python.exe
%aerialpython% -c "import cv2, pika, flask, pika; print('hello world!')"
echo =========== ALL DONE ================
diff --git a/env/SettingEnv.bat b/env/SettingEnv.bat
index 7d79291..b0d30b1 100644
--- a/env/SettingEnv.bat
+++ b/env/SettingEnv.bat
@@ -20,7 +20,11 @@
tar -xzf "./aerial.tar.gz" -C "../../aerial_deploy"
cd ..
-mklink /j ".env" "../aerial_deploy/"
+if not exist ".env" (
+ mklink /j ".env" "../aerial_deploy/")
+
+cd %~dp0
+call "../src/protos/compile.bat"
echo ALL DONE!
diff --git a/scripts/mqClient.py b/scripts/mqClient.py
new file mode 100644
index 0000000..56f546c
--- /dev/null
+++ b/scripts/mqClient.py
@@ -0,0 +1,54 @@
+import signal
+import os, sys
+
+# add python path of src to sys.path
+src_path = os.path.join(__file__, *(['..'] * 2))
+src_path = os.path.abspath(src_path)
+sys.path.insert(1, src_path)
+
+
+from src.engine.AcquisitionMain import AcquisitionMain
+from src.engine.AcquisitionMain import Config
+
+def quit(signum, frame):
+ print('You choose to stop me.')
+ sys.exit()
+
+if __name__ == '__main__':
+ def help():
+ print("Usage: Client.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [DEVICEname] [SERVERhost] [MICROWAVEhost]")
+ print("EXAMPLE: Client.py 172.17.17.206 chai password123 aerial_rpc pod1 http://172.17.17.206/api 192.168.2.65")
+ exit(1)
+
+ if len(sys.argv) != 8:
+ help()
+ try:
+ MQhost = str(sys.argv[1])
+ MQuser = str(sys.argv[2])
+ MQpassword = str(sys.argv[3])
+ QUEUEname = str(sys.argv[4])
+ DEVICEname = str(sys.argv[5])
+ SERVERhost = str(sys.argv[6])
+ MICROWAVEhost = str(sys.argv[7])
+
+ except Exception as e:
+ print(e)
+ help()
+
+ print("Start AcquisitionMain")
+ signal.signal(signal.SIGINT, quit)
+ signal.signal(signal.SIGTERM, quit)
+ # client = AcquisitionMain('192.168.3.54', 'chai',
+ # 'password123', 'aerial_rpc', 'pod1', 'http://192.168.3.54:5000/api', 10)
+ config_dir = os.path.join(__file__, "..", "../src/config.json" )
+ config_path = os.path.abspath(config_dir)
+ config = Config(config_path)
+ client = AcquisitionMain(MQhost,
+ MQuser,
+ MQpassword,
+ QUEUEname,
+ DEVICEname,
+ SERVERhost,
+ MICROWAVEhost, # udp server host, 192.168.2.65 for production
+ config)
+ client.start()
diff --git a/scripts/mqServer.py b/scripts/mqServer.py
new file mode 100644
index 0000000..d1db978
--- /dev/null
+++ b/scripts/mqServer.py
@@ -0,0 +1,33 @@
+import sys
+import os
+
+# add python path of src to sys.path
+src_path = os.path.join(__file__, *(['..'] * 2))
+src_path = os.path.abspath(src_path)
+sys.path.insert(1, src_path)
+
+from src.Server import MyServer
+
+
+if __name__ == "__main__":
+ def help():
+ print("Usage: Server.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [WEBport] [SAVEfolder]")
+ print("EXAMPLE: Server.py localhost chai password123 aerial_rpc 5000 ./Images")
+ exit(1)
+
+ if len(sys.argv) != 7:
+ help()
+ try:
+ MQhost = str(sys.argv[1])
+ MQuser = str(sys.argv[2])
+ MQpassword = str(sys.argv[3])
+ QUEUEname = str(sys.argv[4])
+ WEBport = int(sys.argv[5])
+ SAVEfolder = str(sys.argv[6])
+ save_folder = SAVEfolder
+
+ except Exception as e:
+ print(e)
+ help()
+ server = MyServer(host=MQhost, user=MQuser, password=MQpassword, queue=QUEUEname, port=WEBport, SAVEfolder=save_folder)
+ server.run_all()
\ No newline at end of file
diff --git a/scripts/slamAutoMode.py b/scripts/slamAutoMode.py
index dc0e979..f687ec2 100644
--- a/scripts/slamAutoMode.py
+++ b/scripts/slamAutoMode.py
@@ -9,27 +9,28 @@
if __name__ == "__main__":
def help():
- 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/")
+ print("Usage: auto_slam.py [servo_path][port] [speed] [degre_step] [time_delay] [alpha] [beta] [gamma] [microwaveHost] [udp_packets] [deviceName] [saveFolder]")
+ print("EXAMPLE: auto_slam.py ./RUN/Test_Servo.exe COM11 100 5 5 0 20 auto 192.168.2.65 1000 pod1 ./data/run_test/")
exit(1)
- if len(sys.argv) != 12:
+ if len(sys.argv) != 13:
help()
try:
- port = str(sys.argv[1])
- speed = str(sys.argv[2])
- 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 = os.path.abspath(str(sys.argv[11]))
+ servo_path = str(sys.argv[1])
+ port = str(sys.argv[2])
+ speed = str(sys.argv[3])
+ degre_step = float(sys.argv[4])
+ time_delay = int(sys.argv[5])
+ alpha = str(sys.argv[6])
+ beta = str(sys.argv[7])
+ gamma = str(sys.argv[8])
+ microwaveHost = str(sys.argv[9])
+ udp_packets = int(sys.argv[10])
+ deviceName = str(sys.argv[11])
+ saveFolder = os.path.abspath(str(sys.argv[12]))
except Exception as e:
print(e)
help()
- rotateAndMerge(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
+ rotateAndMerge(servo_path,port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
print("Done")
\ No newline at end of file
diff --git a/scripts_launch/TEST_SlamAuto.bat b/scripts_launch/TEST_SlamAuto.bat
index cdb2170..4ac34c6 100644
--- a/scripts_launch/TEST_SlamAuto.bat
+++ b/scripts_launch/TEST_SlamAuto.bat
@@ -17,7 +17,8 @@
echo change to path: %cd%
@REM pre-defined parameters
-set port=COM11
+set servo_path=./RUN/Test_Servo.exe
+set port=COM14
set speed=200
set degree_step=2
set time_delay=5
@@ -30,10 +31,11 @@
set saveFolder=./data/run_test/
@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: 200):
-set /P degree_step=Please input the degree step of rotation machine (example: 2):
-set /P time_delay=Please input the delay time of rotation machine (example: 5):
+set /P servo_path=Please input the path of servo controller (example: ./RUN/Test_Servo.exe):
+set /p port=Please input the serial port of servo (example: COM14):
+set /P speed=Please input the speed of servo (example: 200):
+set /P degree_step=Please input the degree step of servo (example: 2):
+set /P time_delay=Please input the delay time of servo (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):
@@ -44,6 +46,7 @@
@REM verify the parameters
echo ################# parameters #################
+echo servo_path :%servo_path%
echo port :%port%
echo speed :%speed%
echo degree_step :%degree_step%
@@ -61,7 +64,7 @@
if %choice% == y (
- %aerialpython% ./scripts/slamAutoMode.py %port% %speed% %degree_step% %time_delay% %alpha% %beta% %gamma% %microwaveHost% %udp_packets% %deviceName% %saveFolder%
+ %aerialpython% ./scripts/slamAutoMode.py %servo_path% %port% %speed% %degree_step% %time_delay% %alpha% %beta% %gamma% %microwaveHost% %udp_packets% %deviceName% %saveFolder%
@REM -------------- Usage: plot_ply.py [PLYfile]"
%aerialpython% ./scripts/plot_ply.py %PLYfile%
) else (
diff --git a/scripts_launch/TEST_startClient.bat b/scripts_launch/TEST_startClient.bat
index 15391e0..f399e0d 100644
--- a/scripts_launch/TEST_startClient.bat
+++ b/scripts_launch/TEST_startClient.bat
@@ -16,7 +16,7 @@
echo change to path: %cd%
-set MQhost=192.168.1.14
+set MQhost=localhost
set MQuser=chai
set MQpassword=password123
set QUEUEname=aerial_rpc
@@ -30,6 +30,6 @@
echo Client.py is running ...
@REM -------- Usage: Client.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [DEVICEname] [SERVERhost] [MICROWAVEhost]
@REM %aerialpython% ./src/Client.py %MQhost% %MQuser% %MQpassword% %QUEUEname% %DEVICEname% %SERVERhost% %MICROWAVEhost% >> %LOGfile%
-%aerialpython% ./src/Client.py %MQhost% %MQuser% %MQpassword% %QUEUEname% %DEVICEname% %SERVERhost% %MICROWAVEhost%
+%aerialpython% ./scripts/mqClient.py %MQhost% %MQuser% %MQpassword% %QUEUEname% %DEVICEname% %SERVERhost% %MICROWAVEhost%
echo Client.py has been terminited.
pause
diff --git a/scripts_launch/TEST_startServer.bat b/scripts_launch/TEST_startServer.bat
index fbab24e..f956433 100644
--- a/scripts_launch/TEST_startServer.bat
+++ b/scripts_launch/TEST_startServer.bat
@@ -16,7 +16,8 @@
echo change to path: %cd%
-set MQhost=localhost
+@REM set MQhost=localhost
+set MQhost=47.92.33.19
set MQuser=chai
set MQpassword=password123
set WEBhost=0.0.0.0
@@ -27,8 +28,8 @@
@REM Launch the Server
echo the log file will be saved in the [%LOGfile%] file
echo Server.py is running ...
-@REM -------- Usage: Server.py [MQhost] [MQuser] [MQpassword] [WEBhost] [WEBport] [SAVEfolder]")
-%aerialpython% ./src/Server.py %MQhost% %MQuser% %MQpassword% %WEBhost% %WEBport% %SAVEfolder%
+@REM -------- Usage: Server.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [WEBport] [SAVEfolder]")
+%aerialpython% ./scripts/mqServer.py %MQhost% %MQuser% %MQpassword% %QUEUEname% %WEBport% %SAVEfolder%
echo Server.py has been terminited.
pause
diff --git a/src/Client.py b/src/Client.py
index 2703a4f..6fa9ebd 100644
--- a/src/Client.py
+++ b/src/Client.py
@@ -1,50 +1,49 @@
-import signal
-import os, sys
+# import signal
+# import os, sys
-batfile_dir = os.path.join(__file__, "..", "protos/compile.bat" )
-batfile_path = os.path.abspath(batfile_dir)
-os.system(batfile_path)
-from engine.AcquisitionMain import AcquisitionMain
-from engine.AcquisitionMain import Config
+# from .engine.AcquisitionMain import AcquisitionMain
+# from .engine.AcquisitionMain import Config
-def quit(signum, frame):
- print('You choose to stop me.')
- sys.exit()
+# def quit(signum, frame):
+# print('You choose to stop me.')
+# sys.exit()
-if __name__ == '__main__':
- def help():
- print("Usage: Client.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [DEVICEname] [SERVERhost] [MICROWAVEhost]")
- print("EXAMPLE: Client.py 172.17.17.206 chai password123 aerial_rpc pod1 http://172.17.17.206/api 192.168.2.65")
- exit(1)
+# if __name__ == '__main__':
+# def help():
+# print("Usage: Client.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [DEVICEname] [SERVERhost] [MICROWAVEhost]")
+# print("EXAMPLE: Client.py 172.17.17.206 chai password123 aerial_rpc pod1 http://172.17.17.206/api 192.168.2.65")
+# exit(1)
- if len(sys.argv) != 8:
- help()
- try:
- MQhost = str(sys.argv[1])
- MQuser = str(sys.argv[2])
- MQpassword = str(sys.argv[3])
- QUEUEname = str(sys.argv[4])
- DEVICEname = str(sys.argv[5])
- SERVERhost = str(sys.argv[6])
- MICROWAVEhost = str(sys.argv[7])
+# if len(sys.argv) != 8:
+# help()
+# try:
+# MQhost = str(sys.argv[1])
+# MQuser = str(sys.argv[2])
+# MQpassword = str(sys.argv[3])
+# QUEUEname = str(sys.argv[4])
+# DEVICEname = str(sys.argv[5])
+# SERVERhost = str(sys.argv[6])
+# MICROWAVEhost = str(sys.argv[7])
- except Exception as e:
- print(e)
- help()
+# except Exception as e:
+# print(e)
+# help()
- print("Start AcquisitionMain")
- signal.signal(signal.SIGINT, quit)
- signal.signal(signal.SIGTERM, quit)
- # client = AcquisitionMain('192.168.3.54', 'chai',
- # 'password123', 'aerial_rpc', 'pod1', 'http://192.168.3.54:5000/api', 10)
- config_dir = os.path.join(__file__, "..", "config.json" )
- config_path = os.path.abspath(config_dir)
- config = Config(config_path)
- client = AcquisitionMain(MQhost,
- MQuser,
- MQpassword,
- DEVICEname,
- MICROWAVEhost, # udp server host, 192.168.2.65 for production
- )
- client.start()
+# print("Start AcquisitionMain")
+# signal.signal(signal.SIGINT, quit)
+# signal.signal(signal.SIGTERM, quit)
+# # client = AcquisitionMain('192.168.3.54', 'chai',
+# # 'password123', 'aerial_rpc', 'pod1', 'http://192.168.3.54:5000/api', 10)
+# config_dir = os.path.join(__file__, "..", "config.json" )
+# config_path = os.path.abspath(config_dir)
+# config = Config(config_path)
+# client = AcquisitionMain(MQhost,
+# MQuser,
+# MQpassword,
+# QUEUEname,
+# DEVICEname,
+# SERVERhost,
+# MICROWAVEhost, # udp server host, 192.168.2.65 for production
+# config)
+# client.start()
diff --git a/src/Server.py b/src/Server.py
index d4683e5..90dacf3 100644
--- a/src/Server.py
+++ b/src/Server.py
@@ -10,27 +10,24 @@
from zmq import device
-batfile_dir = os.path.join(__file__, "..", "protos/compile.bat" )
-batfile_path = os.path.abspath(batfile_dir)
-os.system(batfile_path)
-
-from protos import aerial_pb2 as pb
-from engine import MessageThreads as ths
+from .engine.DeviceManager import DeviceManager
+from .engine.MessageQueueThread import MessageQueueThread
+from .protos import aerial_pb2 as pb
class MyServer(Flask):
- def __init__(self, MQhost, MQuser, MQpassword, WEBhost, WEBport, SAVEfolder):
+ def __init__(self, host, user, password, queue, port, SAVEfolder):
Flask.__init__(self, __name__)
- # rabbit MQ
- self.MQhost = MQhost
- self.MQuser = MQuser
- self.MQpassword = MQpassword
- self.parameters = None
- # Web server
- self.WEBhost = WEBhost
- self.WEBport = WEBport
- self.SAVEfolder = SAVEfolder
+ self.host = host
+ self.user = user
+ self.password = password
+ self.queue = queue
+ self.port = port
+ global save_folder
+ save_folder = SAVEfolder
+
+
self.route('/api', methods=['POST'])(self.api)
self.route('/', methods=['GET'])(self.hello)
self._def_routes()
@@ -151,6 +148,7 @@
self.run_rabbitmq()
self.run(debug=False, host=self.WEBhost, port=self.WEBport)
+<<<<<<< HEAD
if __name__ == "__main__":
def help():
@@ -173,3 +171,5 @@
help()
server = MyServer(MQhost, MQuser, MQpassword, WEBhost, WEBport, SAVEfolder)
server.run_all()
+=======
+>>>>>>> aa8a0ba3a86ff5dbbdfcd68c72a5e70e1c52e877
diff --git a/src/__init__.py b/src/__init__.py
index e69de29..29a8a06 100644
--- a/src/__init__.py
+++ b/src/__init__.py
@@ -0,0 +1,5 @@
+from .engine import *
+from .microwave import *
+from .protos import *
+from .slam import *
+from .utils import *
\ No newline at end of file
diff --git a/src/engine/AcquisitionMain.py b/src/engine/AcquisitionMain.py
index bd680f1..bb2c8be 100644
--- a/src/engine/AcquisitionMain.py
+++ b/src/engine/AcquisitionMain.py
@@ -15,12 +15,12 @@
from . import MessageThreads as ths
from .devices import cameraController
-# add python path of src to sys.path
-src_path = os.path.join(__file__, *(['..'] * 2))
-src_path = os.path.abspath(src_path)
-sys.path.insert(0, src_path)
+# # add python path of src to sys.path
+# src_path = os.path.join(__file__, *(['..'] * 2))
+# src_path = os.path.abspath(src_path)
+# sys.path.insert(0, src_path)
-from protos import aerial_pb2 as pb
+from ..protos import aerial_pb2 as pb
IMAGE_WIDTH = 3840
diff --git a/src/engine/CaptureThread.py b/src/engine/CaptureThread.py
index f6d588b..240229f 100644
--- a/src/engine/CaptureThread.py
+++ b/src/engine/CaptureThread.py
@@ -6,12 +6,12 @@
import time
-# add python path of src to sys.path
-src_path = os.path.join(__file__, *(['..'] * 2))
-src_path = os.path.abspath(src_path)
-sys.path.insert(0, src_path)
+# # add python path of src to sys.path
+# src_path = os.path.join(__file__, *(['..'] * 2))
+# src_path = os.path.abspath(src_path)
+# sys.path.insert(0, src_path)
-from protos import aerial_pb2 as pb
+from ..protos import aerial_pb2 as pb
class CaptureThread(threading.Thread):
diff --git a/src/engine/ControllThread.py b/src/engine/ControllThread.py
index 3de30b8..97912c0 100644
--- a/src/engine/ControllThread.py
+++ b/src/engine/ControllThread.py
@@ -6,12 +6,12 @@
import threading
from .DustCleanerThread import DustCleanerThread
-# add python path of src to sys.path
-src_path = os.path.join(__file__, *(['..'] * 2))
-src_path = os.path.abspath(src_path)
-sys.path.insert(0, src_path)
+# # add python path of src to sys.path
+# src_path = os.path.join(__file__, *(['..'] * 2))
+# src_path = os.path.abspath(src_path)
+# sys.path.insert(0, src_path)
-from protos import aerial_pb2 as pb
+from ..protos import aerial_pb2 as pb
class ControllThread(threading.Thread):
"""
diff --git a/src/engine/DustCleanerThread.py b/src/engine/DustCleanerThread.py
index 2405305..c733cf1 100644
--- a/src/engine/DustCleanerThread.py
+++ b/src/engine/DustCleanerThread.py
@@ -5,12 +5,12 @@
from .DustCleaner import DustCleaner
-# add python path of src to sys.path
-src_path = os.path.join(__file__, *(['..'] * 2))
-src_path = os.path.abspath(src_path)
-sys.path.insert(0, src_path)
+# # add python path of src to sys.path
+# src_path = os.path.join(__file__, *(['..'] * 2))
+# src_path = os.path.abspath(src_path)
+# sys.path.insert(0, src_path)
-from protos import aerial_pb2 as pb
+from ..protos import aerial_pb2 as pb
diff --git a/src/engine/MessageQueueThread.py b/src/engine/MessageQueueThread.py
index 16396af..680a988 100644
--- a/src/engine/MessageQueueThread.py
+++ b/src/engine/MessageQueueThread.py
@@ -5,13 +5,13 @@
from ctypes import sizeof
from .DeviceManager import DeviceManager
-# add python path of src to sys.path
-src_path = os.path.join(__file__, *(['..'] * 2))
-src_path = os.path.abspath(src_path)
-sys.path.insert(0, src_path)
+# # add python path of src to sys.path
+# src_path = os.path.join(__file__, *(['..'] * 2))
+# src_path = os.path.abspath(src_path)
+# sys.path.insert(0, src_path)
-from microwave.components import HeaderBlock
-from protos import aerial_pb2 as pb
+from ..microwave.components import HeaderBlock
+from ..protos import aerial_pb2 as pb
class MessageQueueThread(threading.Thread):
def __init__(self,
diff --git a/src/engine/MicroWaveUDPThread.py b/src/engine/MicroWaveUDPThread.py
index 7ec90ba..478afc2 100644
--- a/src/engine/MicroWaveUDPThread.py
+++ b/src/engine/MicroWaveUDPThread.py
@@ -6,7 +6,7 @@
src_path = os.path.abspath(src_path)
sys.path.insert(0, src_path)
-from microwave.servers import MicroWaveUDPServer
+from ..microwave.servers import MicroWaveUDPServer
class MicroWaveUDPThread (threading.Thread):
diff --git a/src/microwave/deviceTools.py b/src/microwave/deviceTools.py
index b26422e..7031f4f 100644
--- a/src/microwave/deviceTools.py
+++ b/src/microwave/deviceTools.py
@@ -3,6 +3,8 @@
import time
import os, sys
import open3d as o3d
+from multiprocessing import Pool
+import numpy as np
from ..utils import file_fliter, write_ply
from .servers import MicroWaveReceiver
@@ -109,41 +111,34 @@
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):
+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,))
- filename = file
- # (fn, ext) = os.path.splitext(filename)
- with open(os.path.join(folder,file), "rb") as bin_file:
- buf = bin_file.read()
- buf_data = MicroWaveParser.ParserFrame(buf)
+ pool.close()
+ pool.join()
-
- for frame in buf_data:
- Frame_id = frame[0]
- FrameNumber = frame[1]
- point_cloud = frame[2]
- if len(point_cloud) != 0 :
- with open(os.path.join(folder,str(FrameNumber) +".ply"), "w") as f:
- f.write("ply\n")
- f.write("format ascii 1.0\n")
- f.write("comment MicroWave2Ply generated\n")
- f.write("element vertex %d\n" % len(point_cloud))
- f.write("property float x\n")
- f.write("property float y\n")
- f.write("property float z\n")
- f.write("element face 0\n")
- f.write("property list uchar int vertex_indices\n")
- f.write("end_header\n")
- n_p = 0
- for p in point_cloud:
- x, y, z = p
- f.write("{:.5f} {:.5f} {:.5f}\n".format(x, y, z))
- n_p += 1
- print(time.asctime(),f"File {file} Frame {FrameNumber}.ply have been saved with {n_p} points!")
def plot_ply(path):
if isinstance(path,str):
diff --git a/src/microwave/parsers.py b/src/microwave/parsers.py
index ee14151..ccd2fb9 100644
--- a/src/microwave/parsers.py
+++ b/src/microwave/parsers.py
@@ -45,7 +45,7 @@
point_data = frame[i * 8 + sizeof(HeaderBlock): (i + 1) * 8 + sizeof(HeaderBlock)]
detection = DetectionBlock(header, footer, point_data)
- point = (detection.X, detection.Y, detection.Z)
+ point = [detection.X, detection.Y, detection.Z]
point_cloud.append(point)
# append the point cloud data to the list
diff --git a/src/microwave/servers.py b/src/microwave/servers.py
index e9c315b..8686b4a 100644
--- a/src/microwave/servers.py
+++ b/src/microwave/servers.py
@@ -1,15 +1,10 @@
import os, sys
from ctypes import sizeof, string_at
from socketserver import BaseRequestHandler, UDPServer
+import warnings
from .components import *
from .constants import *
-
-# add python path of src to sys.path
-src_path = os.path.join(__file__, *(['..'] * 2))
-src_path = os.path.abspath(src_path)
-sys.path.insert(0, src_path)
-
-from protos import aerial_pb2 as pb
+from ..protos import aerial_pb2 as pb
@@ -32,22 +27,61 @@
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(data)
+ 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
+ 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(data)
+ self.footer_buf.extend(bytes(data))
self.ReceivedFooterLength += len(data)
if self.ReceivedFooterLength == sizeof(FooterBlock):
self.Footer = FooterBlock.from_buffer_copy(self.footer_buf)
@@ -56,57 +90,49 @@
# 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 self.state == UDP_STATE_IDLE:
- if buf_size == sizeof(Handshake):
- handshake = Handshake.from_buffer_copy(buf)
- 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
- elif self.state == UDP_STATE_HAND_SHAKE:
- # after received handshake packet, we can get header block
- 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
+ if buf_size == sizeof(Handshake):
+ self._is_handshake(buf)
+ 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)
- self._receive_data(buf[header_size: header_size + data_size])
-
- # 鍒ゆ柇绗竴涓暟鎹寘涓槸鍚﹀寘鍚獸ooter Block
- if (header_size + data_size) < buf_size:
- self._receive_footer(buf[header_size + data_size])
+
+ if self._receive_data(buf[header_size: header_size + data_size]):
+
+ # 鍒ゆ柇绗竴涓暟鎹寘涓槸鍚﹀寘鍚獸ooter 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(buf)
+ self.DataFrame.extend(bytes(buf))
remain_data_size = self.DataLength - self.ReceivedDataLength
data_size = min(buf_size, remain_data_size)
- self._receive_data(buf[: data_size])
- if data_size < buf_size:
- self._receive_footer(buf[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(buf)
+ self.DataFrame.extend(bytes(buf))
self._receive_footer(buf)
diff --git a/src/protos/compile.bat b/src/protos/compile.bat
index bbdbfd3..2176bbf 100644
--- a/src/protos/compile.bat
+++ b/src/protos/compile.bat
@@ -2,10 +2,9 @@
@REM change to the filepath
cd %~dp0
-echo change to path: %cd%
-set protoc=%cd%/../../../aerial_deploy/Library/bin/protoc.exe
+set protoc=%cd%/../../.env/Library/bin/protoc.exe
%protoc% aerial.proto --python_out=%cd%
-echo compile done!
\ No newline at end of file
+echo protoc: aerial.proto compile done!
\ No newline at end of file
diff --git a/src/slam/autoMode.py b/src/slam/autoMode.py
index 18663f2..7abd946 100644
--- a/src/slam/autoMode.py
+++ b/src/slam/autoMode.py
@@ -4,17 +4,18 @@
from ..microwave.deviceTools import udp_get_tool,udp2bin_tool,bin2ply_tool
from .manualMode import manual_merge
-cudir = os.path.abspath(os.path.join(__file__, *(['..'] * 1)))
-exe_path = os.path.abspath(os.path.join(cudir,"../RUN/Test_Servo.exe"))
+# cudir = os.path.abspath(os.path.join(__file__, *(['..'] * 1)))
+# exe_path = os.path.abspath(os.path.join(cudir,"../RUN/Test_Servo.exe"))
-def rotateAndMerge(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):
+def rotateAndMerge(servo_path,port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):
# 鏃嬭浆鏈烘瀯澶嶄綅
+ exe_path = os.path.abspath(servo_path)
os.system(f"{exe_path} {port} 400 0")
time.sleep(10)
# 闂撮殧鑾峰彇udp
- for i in range(0,360,degre_step):
+ for i in np.arange(0,360,degre_step):
os.system(f"{exe_path} {port} {speed} {i}")
time.sleep(int(time_delay))
subFolder = str(-i)
diff --git a/src/slam/core.py b/src/slam/core.py
index 7bf6a94..eea9e94 100644
--- a/src/slam/core.py
+++ b/src/slam/core.py
@@ -21,7 +21,7 @@
# Rz_gamma = [[np.cos(gamma),np.sin(gamma),0],[-np.sin(gamma),np.cos(gamma),0],[0,0,1]]
-def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,order = "x_y_z",verbose=0):
+def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,rank,order = "x_y_z",verbose=0):
"""generate a rotation matrix from the specific angel
"""
@@ -56,16 +56,26 @@
if i == 0:
rotation_matrix = R_dic[s]
else:
- rotation_matrix = R_dic[s] * rotation_matrix
+ rotation_matrix = R_dic[s] * rotation_matrix
+
+ m = np.identity(rank)
+ m[:3,:3] = rotation_matrix
+
if verbose == 1:
print(f"\n{alpha_degree}, rad {alpha}, Rx_alpha \n", Rx_alpha)
print(f"\n{beta_degree}, rad {beta}, Ry_beta \n", Ry_beta)
print(f"\n{gamma_degree}, rad {gamma}, Rz_gamma\n",Rz_gamma)
- print("\n rotation matrix\n",rotation_matrix)
+ print("\n rotation matrix\n",m)
else:
pass
- return rotation_matrix
+ return m
+
+def get_origin_coordinates():
+ """
+ 鑾峰彇姣背娉㈣澶囨暟鎹腑蹇冪浉瀵硅埖鏈哄師鐐圭殑鍧愭爣
+ """
+ pass
def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
# Notice: 鍋囧畾褰撳墠鍧愭爣绯讳负鍙虫墜绯�
@@ -120,17 +130,20 @@
if isinstance(ply,str):
ply = read_ply(ply)
- rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2])
+ N_rows = ply.shape[0]
+ N_cols = ply.shape[1]
+
+ rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2],rank=N_cols)
# z = (X * Y^T)^T = (Y * X^T)
- # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
+ # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix.transpose())
rotated_ply = np.matmul(ply,rotation_matrix.transpose())
if i == 0:
merged_ply_array = rotated_ply.copy()
else:
merged_ply_array = np.concatenate((merged_ply_array,rotated_ply),axis= 0 )
- merged_ply_array = np.array(merged_ply_array).reshape(-1,3)
+ merged_ply_array = np.array(merged_ply_array).reshape(-1,N_cols)
return merged_ply_array
diff --git a/src/slam/manualMode.py b/src/slam/manualMode.py
index 71372da..a23be39 100644
--- a/src/slam/manualMode.py
+++ b/src/slam/manualMode.py
@@ -16,8 +16,8 @@
for root, dirs, files in os.walk(folder,topdown=True):
break
+ dirs.sort(key = lambda x:float(x))
dir_List = dirs
-
angle = []
index = 0
diff --git a/src/utils/tools.py b/src/utils/tools.py
index 80b55d1..8f9fa00 100644
--- a/src/utils/tools.py
+++ b/src/utils/tools.py
@@ -1,3 +1,4 @@
+from email import header
import os, sys
import numpy as np
import time
@@ -19,51 +20,63 @@
return target_files
-def read_ply(path:str,start_line:int = 10):
- count = 0
- ply_list = []
+def get_skip_rows(path):
with open(path,"r") as ply_file:
+ count = 0
# read the line
for line in ply_file.readlines():
# strip the str of the header
- if count < start_line:
- pass
+ flag = line.strip()
+ if flag == "end_header":
+ return count + 1
else:
- line_str_list = line.strip().split(' ')
- # convert the str type to float type
- line_float_list = []
- for value in line_str_list:
- line_float_list.append(float(value))
+ if count >= 50:
+ return None
+ count += 1
+ return None
- ply_list.append(line_float_list)
- count += 1
- ply_array = np.array(ply_list,dtype=np.float16)
-
+def read_ply(path:str):
+ start_line = get_skip_rows(path)
+ if start_line is not None:
+ ply_array = np.loadtxt(path,skiprows=start_line)
+ else:
+ ply_array = None
return ply_array
def write_ply(data:np.array,folder:str,name:str):
file = os.path.join(folder,name+".ply")
- with open (file,'w') as f:
- f.write("ply\n")
- f.write("format ascii 1.0\n")
- f.write("comment write_ply generated\n")
- f.write("element vertex %d\n" % data.shape[0])
- f.write("property float x\n")
- f.write("property float y\n")
- f.write("property float z\n")
- f.write("element face 0\n")
- f.write("property list uchar int vertex_indices\n")
- f.write("end_header\n")
- n_p = 0
+ n_p = data.shape[0]
+ n_cols = data.shape[1]
- for p in tqdm(data):
- x = p[0]
- y = p[1]
- z = p[2]
- f.write("{:.5f} {:.5f} {:.5f}\n".format(x, y, z))
- n_p += 1
- print(time.asctime(),f"Frame {name}.ply have been saved with {n_p} points!")
+ if n_cols == 3:
+ header = ["ply\nformat ascii 1.0\n",
+ "comment write_ply generated\n",
+ f"element vertex {n_p}\n",
+ "property float x\n",
+ "property float y\n",
+ "property float z\n",
+ "end_header"]
+ fmt = '%.6f','%.6f','%.6f'
+
+ elif n_cols == 6:
+ header = ["ply\nformat ascii 1.0\n",
+ "comment write_ply generated\n",
+ f"element vertex {n_p}\n",
+ "property float x\n",
+ "property float y\n",
+ "property float z\n",
+ "property uchar red\n",
+ "property uchar green\n",
+ "property uchar blue\n",
+ "end_header"]
+ fmt = '%.6f','%.6f','%.6f','%d','%d','%d'
+ np.savetxt(file,data,
+ delimiter=" ",
+ header="".join(header),
+ comments="",
+ fmt=fmt)
+ print(f"{time.asctime()} {name}.ply have been saved with {n_p} points!")
--
Gitblit v1.9.1