debug for slam core

wu_xinjun

2022-03-24 225e18670eed17c3732b1ae97cdc8198161748d4

debug for slam core

(cherry picked from commit 91407545d6137f4a628bc8214220564caaee4412)

 .gitignore

@@ -6,6 +6,7 @@

# env
env/aerial_deploy
.env/

# demodata
src/microwave/demo/saveUdPData/

 .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"
        }

 .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"
}

 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!



 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


 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

 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

 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

 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

 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

 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

 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

 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

 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

 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

 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

 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

 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

 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

 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

 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: