auto slam

wu_xinjun

2022-03-21 74e72304b0f38fddbfe3d12a70d0c9a09d25e85f

auto slam

(cherry picked from commit 3d379031a6546f83543aa3e5129dc6dccb5a033b)

 .gitignore

@@ -13,12 +13,16 @@

# data & image & microwave
Data
data
Images
Image*
image*
Images*
Microwaves
Manual_Microwave*

# log
**/log*/
*.log
*.ply


 .vscode/launch.json

@@ -5,6 +5,25 @@
    "version": "0.2.0",
    "configurations": [
        {
            "name": "test auto slam",
            "type": "python",
            "request": "launch",
            "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
            "program": "${workspaceFolder}/src/auto_slam.py",
            "console": "integratedTerminal",
            "args": [
                "COM11",
                "100",
                "5",
                "0",
                "20",
                "auto",
                "192.168.2.65",
                "1000",
                "testDevice",
                "T:\\Data\\projects\\huaneng\\04_Data\\20220316test"]
        },
        {
            "name": "test_slam",
            "type": "python",
            "request": "launch",
@@ -12,9 +31,9 @@
            "program": "${workspaceFolder}/src/mannul_slam.py",
            "console": "integratedTerminal",
            "args": [
                "T:\\Data\\projects\\huaneng\\00_Docs\\slamtest",
                "T:\\Data\\projects\\huaneng\\04_Data\\20220316test",
                "0",
                "60",
                "20",
                "auto",
                "test60120"
            ],

 .vscode/settings.json

@@ -8,8 +8,8 @@
            "--python_out=${workspaceRoot}/src/protos"
        ]
    },
    "python":{
    },
    
    "python.poetryPath": "${workspaceFolder}/env/aerial_deploy/python.exe",
    "jupyter.kernels.filter": [
        {
            "type": "jupyterKernelspec",

 RUN/NLog.config

New file
@@ -0,0 +1,45 @@
<?xml version="1.0" encoding="utf-8" ?>
<nlog xmlns="http://www.nlog-project.org/schemas/NLog.xsd"
      xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
      xsi:schemaLocation="http://www.nlog-project.org/schemas/NLog.xsd NLog.xsd"
      autoReload="true"
      throwExceptions="false"
      internalLogLevel="Off" internalLogFile="c:\temp\nlog-internal.log">

  <!-- optional, add some variables
  https://github.com/nlog/NLog/wiki/Configuration-file#variables
  -->
  <variable name="myvar" value="myvalue"/>
  <variable name="logDirectory" value="logs/"/>

  <!--
  See https://github.com/nlog/nlog/wiki/Configuration-file
  for information on customizing logging rules and outputs.
   -->
  <targets>
    <target name="logfile" xsi:type="File" fileName="${logDirectory}/${shortdate}.txt" />
    <target name="logfile1" xsi:type="File" fileName="${logDirectory}/${longdate}.txt" />

    <!--
    add your targets here
    See https://github.com/nlog/NLog/wiki/Targets for possible targets.
    See https://github.com/nlog/NLog/wiki/Layout-Renderers for the possible layout renderers.
    -->

    <!--
    Write events to a file with the date in the filename.
    <target xsi:type="File" name="f" fileName="${basedir}/logs/${shortdate}.log"
            layout="${longdate} ${uppercase:${level}} ${message}" />
    -->
  </targets>

  <rules>
    <!-- add your logging rules here -->
    <logger name="*" minlevel="Debug" writeTo="logfile" />
    <logger name="*" minlevel="Info" writeTo="logfile" />
    <!--
    Write all events with minimal level of Debug (So Debug, Info, Warn, Error and Fatal, but not Trace)  to "f"
    <logger name="*" minlevel="Debug" writeTo="f" />
    -->
  </rules>
</nlog>

 RUN/NLog.dll

Binary files differ

 RUN/System.IO.Ports.dll

Binary files differ

 RUN/Test_Servo.deps.json

New file
@@ -0,0 +1,138 @@
{
  "runtimeTarget": {
    "name": ".NETCoreApp,Version=v3.0",
    "signature": ""
  },
  "compilationOptions": {},
  "targets": {
    ".NETCoreApp,Version=v3.0": {
      "Test_Servo/1.0.0": {
        "dependencies": {
          "NLog": "5.0.0-rc2",
          "NLog.Config": "4.7.14",
          "NLog.Schema": "4.7.14",
          "System.IO.Ports": "4.4.0"
        },
        "runtime": {
          "Test_Servo.dll": {}
        }
      },
      "Microsoft.NETCore.Platforms/2.0.0": {},
      "Microsoft.Win32.Registry/4.4.0": {
        "dependencies": {
          "Microsoft.NETCore.Platforms": "2.0.0",
          "System.Security.AccessControl": "4.4.0",
          "System.Security.Principal.Windows": "4.4.0"
        }
      },
      "NLog/5.0.0-rc2": {
        "runtime": {
          "lib/netstandard2.0/NLog.dll": {
            "assemblyVersion": "5.0.0.0",
            "fileVersion": "5.0.0.664"
          }
        }
      },
      "NLog.Config/4.7.14": {
        "dependencies": {
          "NLog": "5.0.0-rc2",
          "NLog.Schema": "4.7.14"
        }
      },
      "NLog.Schema/4.7.14": {},
      "System.IO.Ports/4.4.0": {
        "dependencies": {
          "Microsoft.Win32.Registry": "4.4.0"
        },
        "runtime": {
          "lib/netstandard2.0/System.IO.Ports.dll": {
            "assemblyVersion": "4.0.0.0",
            "fileVersion": "4.6.25519.3"
          }
        },
        "runtimeTargets": {
          "runtimes/win/lib/netstandard2.0/System.IO.Ports.dll": {
            "rid": "win",
            "assetType": "runtime",
            "assemblyVersion": "4.0.0.0",
            "fileVersion": "4.6.25519.3"
          }
        }
      },
      "System.Security.AccessControl/4.4.0": {
        "dependencies": {
          "Microsoft.NETCore.Platforms": "2.0.0",
          "System.Security.Principal.Windows": "4.4.0"
        }
      },
      "System.Security.Principal.Windows/4.4.0": {
        "dependencies": {
          "Microsoft.NETCore.Platforms": "2.0.0"
        }
      }
    }
  },
  "libraries": {
    "Test_Servo/1.0.0": {
      "type": "project",
      "serviceable": false,
      "sha512": ""
    },
    "Microsoft.NETCore.Platforms/2.0.0": {
      "type": "package",
      "serviceable": true,
      "sha512": "sha512-VdLJOCXhZaEMY7Hm2GKiULmn7IEPFE4XC5LPSfBVCUIA8YLZVh846gtfBJalsPQF2PlzdD7ecX7DZEulJ402ZQ==",
      "path": "microsoft.netcore.platforms/2.0.0",
      "hashPath": "microsoft.netcore.platforms.2.0.0.nupkg.sha512"
    },
    "Microsoft.Win32.Registry/4.4.0": {
      "type": "package",
      "serviceable": true,
      "sha512": "sha512-dA36TlNVn/XfrZtmf0fiI/z1nd3Wfp2QVzTdj26pqgP9LFWq0i1hYEUAW50xUjGFYn1+/cP3KGuxT2Yn1OUNBQ==",
      "path": "microsoft.win32.registry/4.4.0",
      "hashPath": "microsoft.win32.registry.4.4.0.nupkg.sha512"
    },
    "NLog/5.0.0-rc2": {
      "type": "package",
      "serviceable": true,
      "sha512": "sha512-F+y9t2obY9y0bxAaxvJJtbpBqFWkkqXZjMJPeT3SLqVvSNZq7aFax5cu57hkssHSDF1C/RsHT5uymcmPXgY9Vw==",
      "path": "nlog/5.0.0-rc2",
      "hashPath": "nlog.5.0.0-rc2.nupkg.sha512"
    },
    "NLog.Config/4.7.14": {
      "type": "package",
      "serviceable": true,
      "sha512": "sha512-hnaxwbx/Ajgv/F25oZvSFSNih4Jv8aXU0H4pxFp8kasF8A0KKtP/gLQVjJKMempj2lhE+tp8IzWLU/kGcUWICw==",
      "path": "nlog.config/4.7.14",
      "hashPath": "nlog.config.4.7.14.nupkg.sha512"
    },
    "NLog.Schema/4.7.14": {
      "type": "package",
      "serviceable": true,
      "sha512": "sha512-ExtaAbkpoF7RgiVj6FcNPU1+VrCUt60S/5NLEcnusrmMTWlmQ4HCCUTXFOMPUGcK3q9bJtxwzgdiake0Ttifyw==",
      "path": "nlog.schema/4.7.14",
      "hashPath": "nlog.schema.4.7.14.nupkg.sha512"
    },
    "System.IO.Ports/4.4.0": {
      "type": "package",
      "serviceable": true,
      "sha512": "sha512-izaIWbjFZdik3ypDuA6GWj6fabhB+tR5M7QLcvAqd+I+VjI8UWoVZkh68Ao8Vf8poWWrCU875r3HQZnQW6a7GA==",
      "path": "system.io.ports/4.4.0",
      "hashPath": "system.io.ports.4.4.0.nupkg.sha512"
    },
    "System.Security.AccessControl/4.4.0": {
      "type": "package",
      "serviceable": true,
      "sha512": "sha512-2NRFPX/V81ucKQmqNgGBZrKGH/5ejsvivSGMRum0SMgPnJxwhuNkzVS1+7gC3R2X0f57CtwrPrXPPSe6nOp82g==",
      "path": "system.security.accesscontrol/4.4.0",
      "hashPath": "system.security.accesscontrol.4.4.0.nupkg.sha512"
    },
    "System.Security.Principal.Windows/4.4.0": {
      "type": "package",
      "serviceable": true,
      "sha512": "sha512-pP+AOzt1o3jESOuLmf52YQTF7H3Ng9hTnrOESQiqsnl2IbBh1HInsAMHYtoh75iUYV0OIkHmjvveraYB6zM97w==",
      "path": "system.security.principal.windows/4.4.0",
      "hashPath": "system.security.principal.windows.4.4.0.nupkg.sha512"
    }
  }
}

 RUN/Test_Servo.dll

Binary files differ

 RUN/Test_Servo.exe

Binary files differ

 RUN/Test_Servo.pdb

Binary files differ

 RUN/Test_Servo.runtimeconfig.dev.json

New file
@@ -0,0 +1,9 @@
{
  "runtimeOptions": {
    "additionalProbingPaths": [
      "C:\\Users\\Administrator\\.dotnet\\store\\|arch|\\|tfm|",
      "C:\\Users\\Administrator\\.nuget\\packages",
      "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
    ]
  }
}

 RUN/Test_Servo.runtimeconfig.json

New file
@@ -0,0 +1,9 @@
{
  "runtimeOptions": {
    "tfm": "netcoreapp3.0",
    "framework": {
      "name": "Microsoft.NETCore.App",
      "version": "3.0.0"
    }
  }
}

 RUN/runtimes/win/lib/netstandard2.0/System.IO.Ports.dll

Binary files differ

 TEST_SlamAuto.bat

New file
@@ -0,0 +1,67 @@
@echo off

@REM change to the filepath
cd %~dp0
echo change to path: %cd%

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat

@REM Setting the env
if not exist %aerialpython% (
    call %envsetting% 
)

@REM pre-defined parameters
set port=COM11
set speed=100
set degree_step=5
set alpha=0
set beta=20
set gamma=auto
set microwaveHost=192.168.2.65
set udp_packets=1000
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: 100):
set /P degree_step=Please input the degree step 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):
set /p saveFolder=Please input the folder to save the data (example: ./data/run_test/):

set PLYfile=%saveFolder%/data.ply

@REM verify the parameters
echo    ################# parameters #################
echo    port                    :%port%
echo    speed                   :%speed%
echo    degree_step             :%degree_step%
echo    alpha                   :%alpha%
echo    beta                    :%beta%
echo    gamma                   :%gamma%
echo    microwaveHost           :%microwaveHost%
echo    udp_packets             :%udp_packets%
echo    deviceName              :%deviceName%
echo    saveFolder              :%saveFolder%
echo    PLYfile                 :%PLYfile%

set /p choice=Please verify the parameters and choice [y/n]:

cd %~dp0
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%
    @REM -------------- Usage: plot_ply.py [PLYfile]"
    %aerialpython% ./src/utils/plot_ply.py %PLYfile%
) else (
    exit
)




 src/auto_slam.py

New file
@@ -0,0 +1,67 @@
import os,sys
import numpy as np
import time
from slam.slam import compose_ply
from utils import read_ply,write_ply,file_fliter,udp_get_tool,udp2bin_tool,bin2ply_tool
from mannul_slam import run as compose_muti_ply

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):

    # 旋转机构复位
    os.system(f"{exe_path} {port} 100 0")

    # 间隔获取udp
    for i in range(0,360,degre_step):
        os.system(f"{exe_path} {port} {speed} {i}")
        time.sleep(3)
        subFolder = str(-i)
        udp_get_tool(microwaveHost,udp_packets,deviceName,saveFolder,subFolder)
    
    # 旋转机构复位
    os.system(f"{exe_path} {port} 100 0")

    # iterate the folder
    for root, dirs, files in os.walk(saveFolder,topdown=True):
        break

    for dir in dirs:
        # udp2bin bin2ply
        subdir = os.path.join(saveFolder,dir)
        udp2bin_tool(subdir)
        bin2ply_tool(subdir)


    # compose ply
    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/")
        exit(1)

    if len(sys.argv) != 11:
        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])
    except Exception as e:
        print(e)
        help()

    run(port,speed,degre_step,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder)
    print("Done")




 src/mannul_slam.py

@@ -5,12 +5,17 @@

def run(folder,alpha,beta,gamma,savename):
    
    # literate the folder
    dir_List_ = os.listdir(folder)
    sort_lambda = lambda x:int(x)
    dir_List_.sort(key=sort_lambda)
    # # literate the folder
    # dir_List_ = os.listdir(folder)
    # sort_lambda = lambda x:int(x)
    # dir_List_.sort(key=sort_lambda)
    
    dir_List=dir_List_
    # dir_List=dir_List_
    # iterate the folder
    for root, dirs, files in os.walk(folder,topdown=True):
        break
    
    dir_List = dirs


    angle = []
@@ -42,7 +47,7 @@
    cloudpoint = compose_ply(data)

    # write the file
    write_ply(cloudpoint,".", savename)
    write_ply(cloudpoint, folder, savename)




 src/slam/slam.py

@@ -56,6 +56,26 @@

    return rotation_matrix

def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
    # Notice: 假定当前坐标系为右手系
    # step 1: 平移；current_coor + delta(delta_x, delta_y, delta_z) = target_coor
    #               x' = x - delta_x, y' = y - delta_y, z' = z -delta_y
    # step 2: 旋转; 当前坐标系朝正方面给旋转到指定坐标系。
    # step 3: 检查是否需要从右手系转换到左手系。
    # 平移
    ply = current - np.array([delta_x,delta_y,delta_z])
    # 旋转
    # 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)
    # 转为右手坐标系
    if reverse:
        rotated_ply[:,-1] = -rotated_ply.copy()[:,-1]

    data = rotated_ply
    return data

def compose_ply(muti_ply_with_angle: list):

    ply_nums = len(muti_ply_with_angle)
@@ -83,3 +103,10 @@
    return composed_ply_array


if __name__ == "__main__":
    current = np.array([[1,1,1],[1,3,9],[4,8,5]])
    print(current)
    target = transferTo(current,1,1,1,0,0,0,True)
    print(target)



 src/utils/__init__.py

@@ -1 +1,4 @@
from .tools import *
from .udp_get import *
from .udp2bin import *
from .bin2ply import *

 src/utils/bin2ply.py

@@ -8,31 +8,16 @@
sys.path.insert(0, src_path)

from microwave.parsers import MicroWaveParser
from tools import file_fliter
from .tools import file_fliter

if __name__ == "__main__":
    def help():
        print("Usage: bin2ply.py [BINfolder]")
        print("EXAMPLE: BIN2ply.py ./manual_ply/pod1-2012-12-12-12-12-12/")
        exit(1)

    if len(sys.argv) != 2:
        help()
    try:
        BINfolder = str(sys.argv[1])
        # example pod1-2022-02-21-13-25-03_packet125.udp --> 125
        # sort_lambda = lambda x:int(x.split('.')[0])
        bin_files = file_fliter(BINfolder,'bin')

    except Exception as e:
        print(e)
        help()
def bin2ply_tool(folder):
    bin_files = file_fliter(folder,'bin')
    
    for file in bin_files:

        filename = file
        # (fn, ext) = os.path.splitext(filename)
        with open(os.path.join(BINfolder,file), "rb") as bin_file:
        with open(os.path.join(folder,file), "rb") as bin_file:
            buf = bin_file.read()
            buf_data = MicroWaveParser.ParserFrame(buf)

@@ -42,7 +27,7 @@
                FrameNumber = frame[1]
                point_cloud = frame[2]
                if len(point_cloud) != 0 :
                    with open(os.path.join(BINfolder,str(FrameNumber) +".ply"), "w") as f:
                    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")
@@ -59,3 +44,22 @@
                            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!")



if __name__ == "__main__":
    def help():
        print("Usage: bin2ply.py [BINfolder]")
        print("EXAMPLE: BIN2ply.py ./manual_ply/pod1-2012-12-12-12-12-12/")
        exit(1)

    if len(sys.argv) != 2:
        help()
    try:
        BINfolder = str(sys.argv[1])

    except Exception as e:
        print(e)
        help()
    
    bin2ply_tool(BINfolder)

 src/utils/udp2bin.py

@@ -9,28 +9,14 @@
sys.path.insert(0, src_path)

from microwave.servers import MicroWaveReceiver
from tools import file_fliter
from .tools import file_fliter

def udp2bin_tool(udp_folder):

if __name__ == '__main__':

    def help():
        print("Usage: udp2bin.py [UDPfolder]")
        print("EXAMPLE: udp2bin.py ./manual_ply/pod1-2012-12-12-12-12-12/")
        exit(1)

    if len(sys.argv) != 2:
        help()
    try:
        UDPfolder = str(sys.argv[1])
    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)

    except Exception as e:
        print(e)
        help()

    receiver = MicroWaveReceiver()
    for file in udp_files:
        filename = os.path.join(UDPfolder, file)
@@ -44,3 +30,23 @@
                with open(bin_name, "wb") as bin_file:
                    bin_file.write(receiver.DataFrame)
                    bin_file.close()



if __name__ == '__main__':

    def help():
        print("Usage: udp2bin.py [UDPfolder]")
        print("EXAMPLE: udp2bin.py ./manual_ply/pod1-2012-12-12-12-12-12/")
        exit(1)

    if len(sys.argv) != 2:
        help()
    try:
        udp_folder = str(sys.argv[1])

    except Exception as e:
        print(e)
        help()
    
    udp2bin_tool(udp_folder)

 src/utils/udp_get.py

@@ -46,6 +46,38 @@
            UDPServer.server_close(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 = '192.168.2.65' # use this host to get the udp data from microwave device
    port = 9911
    server =UDPServer((microwaveHost, port), PointCloudUDPRequestHandler)
    print(time.asctime(),'Server started on port', port)
    print('....')
    print('ctrl-c to quit server.')
    try:
        server.serve_forever()
    except KeyboardInterrupt:
        server.server_close()
        print(time.asctime(),"Server Stopped")
    except:
        server.server_close()
        print(time.asctime(),"Server Stopped")




if __name__ == '__main__':

    def help():
@@ -63,33 +95,9 @@
        SAVEfolder = str(sys.argv[4])
        SUBfolder = str(sys.argv[5])

        global packet_threshold
        packet_threshold = PACKETthreshold
        global device_id
        device_id = DEVICEname
        global save_folder
        save_folder = SAVEfolder
        global sub_folder
        sub_folder = SUBfolder

    except Exception as e:
        print(e)
        help()

    global server
    
    # host = ''
    # host = '192.168.2.65' # use this host to get the udp data from microwave device
    port = 9911
    server =UDPServer((MICROWAVEhost, port), PointCloudUDPRequestHandler)
    print(time.asctime(),'Server started on port', port)
    print('....')
    print('ctrl-c to quit server.')
    try:
        server.serve_forever()
    except KeyboardInterrupt:
        server.server_close()
        print(time.asctime(),"Server Stopped")
    except:
        server.server_close()
        print(time.asctime(),"Server Stopped")
    udp_get_tool(MICROWAVEhost,PACKETthreshold,DEVICEname,SAVEfolder,SUBfolder)

 test60120.ply

File was deleted