From e175b351286c34defd8c2bbe1ddd09d64d2ac862 Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期四, 24 三月 2022 16:12:06 +0800
Subject: [PATCH] debug for slam core
---
scripts_launch/udp_get.bat | 2
.gitignore | 1
env/SettingEnv.bat | 4 +
scripts_launch/TEST_startClient.bat | 2
.vscode/settings.json | 17 +----
scripts_launch/TEST_SlamAuto.bat | 10 +-
scripts_launch/TEST_udp_get.bat | 2
scripts_launch/startClient.bat | 2
scripts_launch/TEST_Slam.bat | 4 +
scripts_launch/TEST_udp2ply.bat | 2
scripts_launch/TEST_ply_show.bat | 2
scripts_launch/bin2ply.bat | 2
.vscode/launch.json | 30 +++++-----
src/slam/core.py | 28 ++++++---
scripts_launch/TEST_bin2ply.bat | 2
scripts_launch/TEST_startServer.bat | 2
scripts_launch/ply_show.bat | 2
scripts_launch/udp2bin.bat | 2
scripts_launch/startServer.bat | 2
scripts_launch/udp2ply.bat | 2
scripts_launch/TEST_udp2bin.bat | 2
21 files changed, 65 insertions(+), 57 deletions(-)
diff --git a/.gitignore b/.gitignore
index d05e1c9..e9715de 100644
--- a/.gitignore
+++ b/.gitignore
@@ -6,6 +6,7 @@
# env
env/aerial_deploy
+.env/
# demodata
src/microwave/demo/saveUdPData/
diff --git a/.vscode/launch.json b/.vscode/launch.json
index 2381a38..62bbb88 100644
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -8,8 +8,8 @@
"name": "test auto slam",
"type": "python",
"request": "launch",
- "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
- "program": "${workspaceFolder}/src/auto_slam.py",
+ "python": "${workspaceFolder}/.env/python.exe",
+ "program": "${workspaceFolder}/scripts/slamAutoMode.py",
"console": "integratedTerminal",
"args": [
"COM11",
@@ -22,17 +22,17 @@
"192.168.2.65",
"1000",
"testDevice",
- "T:\\Data\\projects\\huaneng\\04_Data\\20220316test"]
+ "../../04_Data/20220316test"]
},
{
"name": "test_slam",
"type": "python",
"request": "launch",
- "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
- "program": "${workspaceFolder}/src/mannul_slam.py",
+ "python": "${workspaceFolder}/.env/python.exe",
+ "program": "${workspaceFolder}/scripts/slamManualMode.py",
"console": "integratedTerminal",
"args": [
- "T:\\Data\\projects\\huaneng\\04_Data\\20220316test",
+ "../../04_Data/20220316test",
"0",
"20",
"auto",
@@ -44,7 +44,7 @@
"name": "Client_test",
"type": "python",
"request": "launch",
- "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
+ "python": "${workspaceFolder}/.env/python.exe",
"program": "${workspaceFolder}/src/Client.py",
"args": [
"192.168.1.224",
@@ -61,7 +61,7 @@
"name": "Client_production",
"type": "python",
"request": "launch",
- "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
+ "python": "${workspaceFolder}/.env/python.exe",
"program": "${workspaceFolder}/src/Client.py",
"args": [
"172.17.17.206",
@@ -77,7 +77,7 @@
{
"name": "Server_test",
"type": "python",
- "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
+ "python": "${workspaceFolder}/.env/python.exe",
"request": "launch",
"program": "${workspaceFolder}/src/Server.py",
"args": [
@@ -94,7 +94,7 @@
"name": "Server_production",
"type": "python",
"request": "launch",
- "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
+ "python": "${workspaceFolder}/.env/python.exe",
"program": "${workspaceFolder}/src/Server.py",
"args": [
"172.17.17.206",
@@ -110,8 +110,8 @@
"name": "udp2bin",
"type": "python",
"request": "launch",
- "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
- "program": "${workspaceFolder}/src/utils/udp2bin.py",
+ "python": "${workspaceFolder}/.env/python.exe",
+ "program": "${workspaceFolder}/scripts/udp2bin.py",
"args": [
"./src/microwave/demo/demoData"
],
@@ -121,10 +121,10 @@
"name": "bin2ply",
"type": "python",
"request": "launch",
- "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
- "program": "${workspaceFolder}/src/utils/bin2ply.py",
+ "python": "${workspaceFolder}/.env/python.exe",
+ "program": "${workspaceFolder}/scripts/bin2ply.py",
"args": [
- "T:/Data/projects/huaneng/00_Docs/mutiFrameBin"
+ "../../00_Docs/mutiFrameBin"
],
"console": "integratedTerminal"
}
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 71b48b3..4479c53 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,21 +1,12 @@
{
- "protoc": {
- // "path": "D:/anaconda3/Library/bin/protoc.exe",
- "path": "C:/Users/zhaoyipeng/miniconda3/envs/py39/Library/bin/protoc.exe",
- "compile_on_save": true,
- "options": [
- "--proto_path=${workspaceRoot}/proto",
- "--python_out=${workspaceRoot}/src/protos"
- ]
- },
- "python.poetryPath": "${workspaceFolder}/env/aerial_deploy/python.exe",
+ "python.poetryPath": "${workspaceFolder}/.env/python.exe",
"jupyter.kernels.filter": [
{
"type": "jupyterKernelspec",
- "path": "${workspaceFolder}/env/aerial_deploy/python.exe"
+ "path": "${workspaceFolder}/.env/python.exe"
}
],
- "python.envFile": "${workspaceFolder}/env/aerial_deploy",
- "python.defaultInterpreterPath": "${workspaceFolder}/env/aerial_deploy/python.exe"
+ "python.envFile": "${workspaceFolder}/.env",
+ "python.defaultInterpreterPath": "${workspaceFolder}/.env/python.exe"
}
\ No newline at end of file
diff --git a/env/SettingEnv.bat b/env/SettingEnv.bat
index c125f84..7d79291 100644
--- a/env/SettingEnv.bat
+++ b/env/SettingEnv.bat
@@ -18,6 +18,10 @@
echo setting the env ...
tar -xzf "./aerial.tar.gz" -C "../../aerial_deploy"
+
+cd ..
+mklink /j ".env" "../aerial_deploy/"
+
echo ALL DONE!
diff --git a/scripts_launch/TEST_Slam.bat b/scripts_launch/TEST_Slam.bat
index 9f8fb6f..91ee694 100644
--- a/scripts_launch/TEST_Slam.bat
+++ b/scripts_launch/TEST_Slam.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
@@ -25,6 +25,8 @@
echo [the folder example]: ./slamdata/
set /p folder=Please input the slam data folder:
+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 PLYfile=%folder%/%savename%.ply
diff --git a/scripts_launch/TEST_SlamAuto.bat b/scripts_launch/TEST_SlamAuto.bat
index f0786a9..cdb2170 100644
--- a/scripts_launch/TEST_SlamAuto.bat
+++ b/scripts_launch/TEST_SlamAuto.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
@@ -19,25 +19,25 @@
@REM pre-defined parameters
set port=COM11
set speed=200
-set degree_step=5
+set degree_step=2
set time_delay=5
set alpha=0
set beta=20
set gamma=auto
set microwaveHost=192.168.2.65
-set udp_packets=1000
+set udp_packets=500
set deviceName=testDevice
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):
-@REM set /P degree_step=Please input the degree step of rotation machine (example: 5):
+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 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):
-@REM set /p udp_packets=Please input the udp packets num (example: 1000):
+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
diff --git a/scripts_launch/TEST_bin2ply.bat b/scripts_launch/TEST_bin2ply.bat
index 6cca107..469f60d 100644
--- a/scripts_launch/TEST_bin2ply.bat
+++ b/scripts_launch/TEST_bin2ply.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/TEST_ply_show.bat b/scripts_launch/TEST_ply_show.bat
index 3415e5c..ac3f3b0 100644
--- a/scripts_launch/TEST_ply_show.bat
+++ b/scripts_launch/TEST_ply_show.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/TEST_startClient.bat b/scripts_launch/TEST_startClient.bat
index 78138e6..065dd95 100644
--- a/scripts_launch/TEST_startClient.bat
+++ b/scripts_launch/TEST_startClient.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/TEST_startServer.bat b/scripts_launch/TEST_startServer.bat
index 9b8bc79..c648595 100644
--- a/scripts_launch/TEST_startServer.bat
+++ b/scripts_launch/TEST_startServer.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/TEST_udp2bin.bat b/scripts_launch/TEST_udp2bin.bat
index feafb43..32f2fc7 100644
--- a/scripts_launch/TEST_udp2bin.bat
+++ b/scripts_launch/TEST_udp2bin.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/TEST_udp2ply.bat b/scripts_launch/TEST_udp2ply.bat
index e18a036..bebdf23 100644
--- a/scripts_launch/TEST_udp2ply.bat
+++ b/scripts_launch/TEST_udp2ply.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/TEST_udp_get.bat b/scripts_launch/TEST_udp_get.bat
index 5c6897b..f5f8eb1 100644
--- a/scripts_launch/TEST_udp_get.bat
+++ b/scripts_launch/TEST_udp_get.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/bin2ply.bat b/scripts_launch/bin2ply.bat
index 2432a97..e499239 100644
--- a/scripts_launch/bin2ply.bat
+++ b/scripts_launch/bin2ply.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/ply_show.bat b/scripts_launch/ply_show.bat
index a51f579..42c3182 100644
--- a/scripts_launch/ply_show.bat
+++ b/scripts_launch/ply_show.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/startClient.bat b/scripts_launch/startClient.bat
index 53345d6..76c5815 100644
--- a/scripts_launch/startClient.bat
+++ b/scripts_launch/startClient.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/startServer.bat b/scripts_launch/startServer.bat
index c93b201..4e61f58 100644
--- a/scripts_launch/startServer.bat
+++ b/scripts_launch/startServer.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/udp2bin.bat b/scripts_launch/udp2bin.bat
index c9f37b9..9355331 100644
--- a/scripts_launch/udp2bin.bat
+++ b/scripts_launch/udp2bin.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/udp2ply.bat b/scripts_launch/udp2ply.bat
index b28b3cd..d5ce011 100644
--- a/scripts_launch/udp2ply.bat
+++ b/scripts_launch/udp2ply.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/scripts_launch/udp_get.bat b/scripts_launch/udp_get.bat
index 1c27f14..a145981 100644
--- a/scripts_launch/udp_get.bat
+++ b/scripts_launch/udp_get.bat
@@ -4,7 +4,7 @@
cd %~dp0
cd ..
-set aerialpython=%cd%/../aerial_deploy/python.exe
+set aerialpython=%cd%/.env/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@REM Setting the env
diff --git a/src/slam/core.py b/src/slam/core.py
index c277da8..7bf6a94 100644
--- a/src/slam/core.py
+++ b/src/slam/core.py
@@ -1,5 +1,6 @@
import numpy as np
import os, sys
+import math
# # add python path of src to sys.path
# src_path = os.path.join(__file__, *(['..'] * 2))
# src_path = os.path.abspath(src_path)
@@ -20,15 +21,16 @@
# 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,verbose=0):
+def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,order = "x_y_z",verbose=0):
"""generate a rotation matrix from the specific angel
"""
+
rad_list = []
for i, angle in enumerate([alpha_degree,beta_degree,gamma_degree]):
if np.abs(float(angle))>360:
raise ValueError(f"incorrect angle value: value in [-360,360] is excepted, but {angle} is offered.")
- rad_list.append(np.pi * float(angle) / 180)
+ rad_list.append(math.radians(angle))
alpha = rad_list[0]
Rx_alpha = np.matrix([[1,0,0],
@@ -44,13 +46,21 @@
Rz_gamma = np.matrix([[np.cos(gamma),np.sin(gamma),0],
[-np.sin(gamma),np.cos(gamma),0],
[0,0,1]])
-
- rotation_matrix = Rx_alpha * Ry_beta * Rz_gamma
+ R_dic = {"x":Rx_alpha,
+ "y":Ry_beta,
+ "z":Rz_gamma
+ }
+
+ for i, s in enumerate(order.split("_")):
+ if i == 0:
+ rotation_matrix = R_dic[s]
+ else:
+ rotation_matrix = R_dic[s] * rotation_matrix
if verbose == 1:
print(f"\n{alpha_degree}, rad {alpha}, Rx_alpha \n", Rx_alpha)
- print(f"\n{beta_degree}, {beta}, Ry_beta \n", Ry_beta)
- print(f"\n{gamma_degree}, {gamma}, Rz_gamma\n",Rz_gamma)
+ 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)
else:
pass
@@ -69,7 +79,7 @@
# z = (X * Y^T)^T = (Y * X^T)
# np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
rotation_matrix = get_rotation_matrix(alpha,beta,gamma)
- rotated_ply = np.matmul(ply,rotation_matrix)
+ rotated_ply = np.matmul(ply,rotation_matrix.transpose())
# 杞负鍙虫墜鍧愭爣绯�
if reverse:
rotated_ply[:,-1] = -rotated_ply.copy()[:,-1]
@@ -87,7 +97,7 @@
moved_data = data + np.array([x,y,z])
# 鏃嬭浆
rotation_matrix = get_rotation_matrix(-alpha,-beta,-gamma)
- rotated_data = np.matmul(moved_data,rotation_matrix)
+ rotated_data = np.matmul(moved_data,rotation_matrix.transpose())
# 宸﹀彸鎵嬪潗鏍囩郴杞崲, 浣跨敤z杞磋浆鎹�
@@ -114,7 +124,7 @@
# z = (X * Y^T)^T = (Y * X^T)
# np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
- rotated_ply = np.matmul(ply,rotation_matrix)
+ rotated_ply = np.matmul(ply,rotation_matrix.transpose())
if i == 0:
merged_ply_array = rotated_ply.copy()
else:
--
Gitblit v1.9.1