Merge dev commit '28668532ed2f97a3883be7e68bced252bd1f99bf' into addFeature

wu_xinjun

2022-03-23 563f7741ad28a6a578b56c98c27f92b3677958bd

Merge dev commit '28668532ed2f97a3883be7e68bced252bd1f99bf' into addFeature

 .gitignore

@@ -13,11 +13,16 @@

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

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


 .vscode/launch.json

@@ -5,6 +5,42 @@
    "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",
                "5",
                "0",
                "20",
                "auto",
                "192.168.2.65",
                "1000",
                "testDevice",
                "T:\\Data\\projects\\huaneng\\04_Data\\20220316test"]
        },
        {
            "name": "test_slam",
            "type": "python",
            "request": "launch",
            "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
            "program": "${workspaceFolder}/src/mannul_slam.py",
            "console": "integratedTerminal",
            "args": [
                "T:\\Data\\projects\\huaneng\\04_Data\\20220316test",
                "0",
                "20",
                "auto",
                "test60120"
            ],
            "justMyCode": true
        },
        {
            "name": "Client_test",
            "type": "python",
            "request": "launch",
@@ -77,7 +113,7 @@
            "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
            "program": "${workspaceFolder}/src/utils/udp2bin.py",
            "args": [
                "./Manual_Microwave/pod1-2022-02-21-15-34-45/"
                "./src/microwave/demo/demoData"
            ],
            "console": "integratedTerminal"
        },
@@ -88,7 +124,7 @@
            "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
            "program": "${workspaceFolder}/src/utils/bin2ply.py",
            "args": [
                "./Manual_Microwave/pod1-2022-02-21-15-34-45/"
                "T:/Data/projects/huaneng/00_Docs/mutiFrameBin"
            ],
            "console": "integratedTerminal"
        }

 .vscode/settings.json

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

 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_udp_get.bat

File was deleted

 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!

 scripts/bin2ply.py

New file
@@ -0,0 +1,28 @@
import os
import sys
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(1, src_path)

from src.microwave.deviceTools import bin2ply_tool


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 = os.path.abspath(str(sys.argv[1]))

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

 scripts/plot_ply.py

New file
@@ -0,0 +1,26 @@
import open3d as o3d
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.microwave.deviceTools import plot_ply

if __name__ == "__main__":
    def help():
        print("Usage: plot_ply.py [PLYfile or plyfolder]")
        print("EXAMPLE: plot_ply ./Manual_Microwave/pod1-2022-02-21-15-34-45/2522.ply")
        exit(1)

    if len(sys.argv) != 2:
        help()
    try:
        PLYfile = os.path.abspath(str(sys.argv[1]))

        plot_ply(PLYfile)

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

 scripts/slamAutoMode.py

New file
@@ -0,0 +1,35 @@
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.slam.autoMode import rotateAndMerge

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

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

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

 scripts/slamManualMode.py

New file
@@ -0,0 +1,30 @@
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.slam.manualMode import manual_merge

if __name__ == "__main__":
    def help():
        print("Usage: mannul_slam.py [folder] [alpha] [beta] [gamma] [savename]")
        print("EXAMPLE: mannul_slam.py T:\\Data\\chuizhi60°_shuiping120° 0 60 auto test60120")
        exit(1)

    if len(sys.argv) != 6:
        help()
    try:
        folder = os.path.abspath(str(sys.argv[1]))
        alpha = str(sys.argv[2])
        beta = str(sys.argv[3])
        gamma = str(sys.argv[4])
        savename = str(sys.argv[5])

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

    manual_merge(folder,alpha,beta,gamma,savename)
    print("Done")

 scripts/udp2bin.py

New file
@@ -0,0 +1,29 @@
import os
import sys
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(1, src_path)


from src.microwave.deviceTools import udp2bin_tool

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 = os.path.abspath(str(sys.argv[1]))

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

 scripts/udp_get.py

New file
@@ -0,0 +1,33 @@
import time
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.microwave.deviceTools import udp_get_tool

if __name__ == '__main__':

    def help():
        print("**************************ERROR**************************")
        print("Usage: udp_get.py [MICROWAVEhost] [PACKETthreshold] [DEVICEname] [SAVEfolder] [SUBfolder")
        print("EXAMPLE: udp_get.py 192.168.2.65 1000 pod1 ./manual_ply 60 \n")
        exit(1)
        
    if len(sys.argv) != 6:
        help()
    try:
        MICROWAVEhost = str(sys.argv[1])
        PACKETthreshold = int(sys.argv[2])
        DEVICEname = str(sys.argv[3])
        SAVEfolder = os.path.abspath(str(sys.argv[4]))
        SUBfolder = str(sys.argv[5])


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

    udp_get_tool(MICROWAVEhost,PACKETthreshold,DEVICEname,SAVEfolder,SUBfolder)

 scripts_launch/TEST_Slam.bat

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

@REM change to the filepath
cd %~dp0
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% 
)
cd %~dp0
cd ..

echo change to path: %cd%

@REM set folder=T:\\Data\\projects\\huaneng\\00_Docs\\slamtest
set alpha=0
set beta=20
@REM 毫米波设备面朝下，顺时针方向旋转
set gamma=auto
set savename=data

echo [the folder example]: ./slamdata/
set /p folder=Please input the slam data folder:

set PLYfile=%folder%/%savename%.ply

@REM verify the parameters
echo    ################# parameters #################
echo    alpha                   :%alpha%
echo    beta                    :%beta%
echo    gamma                   :%gamma%
echo    folder                  :%folder%
echo    savename                :%savename%
echo    PLYfile                 :%PLYfile%

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


if %choice% == y (
    %aerialpython% ./scripts/slamManualMode.py %folder% %alpha% %beta% %gamma% %savename%
    @REM -------------- Usage: plot_ply.py [PLYfile]"
    %aerialpython% ./scripts/plot_ply.py %PLYfile%
) else (
    echo you choose stop!
    exit
)
pause



 scripts_launch/TEST_SlamAuto.bat

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

@REM change to the filepath
cd %~dp0
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% 
)
cd %~dp0
cd ..

echo change to path: %cd%

@REM pre-defined parameters
set port=COM11
set speed=200
set degree_step=5
set time_delay=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: 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):
@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

@REM verify the parameters
echo    ################# parameters #################
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%
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]:


if %choice% == y (
    %aerialpython% ./scripts/slamAutoMode.py %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 (
    echo you choose stop!
    exit
)
pause




 scripts_launch/TEST_bin2ply.bat

File was renamed from TEST_bin2ply.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,10 +12,14 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set BINfolder=./Manual_Microwave_test/pod1-2022-02-21-15-34-45/
echo [the folder example]: ./slamdata/
set /p BINfolder=Please input the bin data folder:


@REM ---------------Usage: bin2ply.py [BINfolder]
%aerialpython% ./src/utils/bin2ply.py %BINfolder%
%aerialpython% ./scripts/bin2ply.py %BINfolder%
pause

 scripts_launch/TEST_ply_show.bat

File was renamed from TEST_ply_show .bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -11,11 +11,12 @@
if not exist %aerialpython% (
    call %envsetting% 
)

cd %~dp0
cd ..

echo change to path: %cd%

set PLYfile=./Manual_Microwave_test/pod1-2022-02-22-15-44-51/28344.ply
set PLYfile=./test60120.ply
@REM -------------- Usage: plot_ply.py [PLYfile]"
%aerialpython% ./src/utils/plot_ply.py %PLYfile%
%aerialpython% ./scripts/plot_ply.py %PLYfile%
pause

 scripts_launch/TEST_startClient.bat

File was renamed from TEST_startClient.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -11,6 +11,10 @@
if not exist %aerialpython% (
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set MQhost=192.168.1.14
set MQuser=chai
@@ -21,8 +25,6 @@
set MICROWAVEhost=localhost
set LOGfile=ClientTEST.log

cd %~dp0
echo change to path: %cd%
@REM Launch the Client
echo the log file will be saved in the [%LOGfile%] file
echo Client.py is running ...

 scripts_launch/TEST_startServer.bat

File was renamed from TEST_startServer.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,6 +12,8 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set MQhost=localhost

 scripts_launch/TEST_udp2bin.bat

File was renamed from TEST_udp2bin.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,10 +12,13 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set UDPfolder=./Manual_Microwave_test/pod1-2022-02-21-15-34-45/
echo [the UDP folder example]: ./slamdata/60/
set /p UDPfolder=Please input the UDP folder:

@REM ---------------Usage: udp2bin.py [UDPfolder]
%aerialpython% ./src/utils/udp2bin.py %UDPfolder%
%aerialpython% ./scripts/udp2bin.py %UDPfolder%
pause

 scripts_launch/TEST_udp2ply.bat

File was renamed from TEST_udp2ply.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,12 +12,15 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set UDPfolder=./Manual_Microwave_test/pod1-2022-02-22-15-44-51/
echo [the UDP folder example]: ./slamdata/60/
set /p UDPfolder=Please input the UDP folder:

@REM ---------------Usage: udp2bin.py [UDPfolder]
%aerialpython% ./src/utils/udp2bin.py %UDPfolder%
%aerialpython% ./scripts/udp2bin.py %UDPfolder%
@REM ---------------Usage: bin2ply.py [BINfolder]
%aerialpython% ./src/utils/bin2ply.py %UDPfolder%
%aerialpython% ./scripts/bin2ply.py %UDPfolder%
pause

 scripts_launch/TEST_udp_get.bat

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

@REM change to the filepath
cd %~dp0
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% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set MICROWAVEhost=192.168.2.65
SET PACKETthreshold=1000
SET DEVICEname=pod1

echo [the data folder example]: ./slamdata
SET /p SAVEfolder=Please input the data folder path:

echo [the sub folder example]: 60 
SET /p SUBfolder=Please input the sub folder name:

set folder=./%SAVEfolder%/%SUBfolder%/
echo the udp data will be saved in folder %folder%
@REM ---------------Usage: udp_get.py [MICROWAVEhost] [PACKETthreshold] [DEVICEname] [SAVEfolder]
%aerialpython% ./scripts/udp_get.py %MICROWAVEhost% %PACKETthreshold% %DEVICEname% %SAVEfolder% %SUBfolder%
pause

 scripts_launch/bin2ply.bat

File was renamed from bin2ply.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,10 +12,13 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

@REM please the folder path of the bin files.
@REM set BINfolder=./Manual_Microwave/pod1-2012-12-12-12-12-12/

@REM ---------------Usage: bin2ply.py [BINfolder]
%aerialpython% ./src/utils/bin2ply.py %BINfolder%
%aerialpython% ./scripts/bin2ply.py %BINfolder%
pause

 scripts_launch/ply_show.bat

File was renamed from ply_show.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,10 +12,12 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%
@REM please the folder path of the bin files.
@REM set PLYfile=./Manual_Microwave/pod1-2022-02-21-15-34-45/2522.ply

@REM -------------- Usage: plot_ply.py [PLYfile]"
%aerialpython% ./src/utils/plot_ply.py %PLYfile%
%aerialpython% ./scripts/plot_ply.py %PLYfile%
pause

 scripts_launch/startClient.bat

File was renamed from startClient.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,6 +12,8 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set MQhost=172.17.17.206

 scripts_launch/startServer.bat

File was renamed from startServer.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,6 +12,8 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set MQhost=172.17.17.206

 scripts_launch/udp2bin.bat

File was renamed from udp2bin.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -11,13 +11,14 @@
if not exist %aerialpython% (
    call %envsetting% 
)

cd %~dp0
cd ..

echo change to path: %cd%

@REM please the folder path of the udp files.
@REM set UDPfolder=./Manual_Microwave/pod1-2012-12-12-12-12-12/

@REM ---------------Usage: udp2bin.py [UDPfolder]
%aerialpython% ./src/utils/udp2bin.py %UDPfolder%
%aerialpython% ./scripts/udp2bin.py %UDPfolder%
pause

 scripts_launch/udp2ply.bat

File was renamed from udp2ply.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -11,15 +11,16 @@
if not exist %aerialpython% (
    call %envsetting% 
)

cd %~dp0
cd ..

echo change to path: %cd%

@REM please the folder path of the udp files.
@REM set UDPfolder=./Manual_Microwave/pod1-2012-12-12-12-12-12/

@REM ---------------Usage: udp2bin.py [UDPfolder]
%aerialpython% ./src/utils/udp2bin.py %UDPfolder%
%aerialpython% ./scripts/udp2bin.py %UDPfolder%
@REM ---------------Usage: bin2ply.py [BINfolder]
%aerialpython% ./src/utils/bin2ply.py %UDPfolder%
%aerialpython% ./scripts/bin2ply.py %UDPfolder%
pause

 scripts_launch/udp_get.bat

File was renamed from udp_get.bat
@@ -2,7 +2,7 @@

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

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat
@@ -12,6 +12,8 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

set MICROWAVEhost=192.168.2.65
@@ -19,5 +21,5 @@
SET DEVICEname=pod1
SET SAVEfolder=./Manual_Microwave
@REM ---------------Usage: udp_get.py [MICROWAVEhost] [PACKETthreshold] [DEVICEname] [SAVEfolder]
%aerialpython% ./src/utils/udp_get.py %MICROWAVEhost% %PACKETthreshold% %DEVICEname% %SAVEfolder%
%aerialpython% ./scripts/udp_get.py %MICROWAVEhost% %PACKETthreshold% %DEVICEname% %SAVEfolder%
pause

 src/__init__.py

 src/microwave/__init__.py

@@ -1,4 +1,5 @@
from .components import *
from  .constants import *
from  .parsers import *
from  .servers import *
from  .servers import *
from .deviceTools import *

 src/microwave/deviceTools.py

New file
@@ -0,0 +1,165 @@
import datetime
from socketserver import BaseRequestHandler, UDPServer
import time
import os, sys
import open3d as o3d

from ..utils import file_fliter, write_ply
from .servers import MicroWaveReceiver
from .parsers import MicroWaveParser

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

# udp_get
class PointCloudUDPRequestHandler(BaseRequestHandler):

    def handle(self):
        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
        msg, sock = self.request
        time_tag = datetime.datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
        f = open('{}/{}_packet{}.udp'.format(path,time_tag,packed_count), 'wb')
        f.write(msg)
        f.close()
        self.server.packed_count += 1
        self.server.udp_list.append(msg)
        
        if packed_count >= packet_threshold:
            UDPServer.server_close(self.server)

def udp_get_tool(microwaveHost,packetThreshold,deviceName,saveFolder,subFolder):

    # 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.')
    try:
        server.serve_forever()
    except KeyboardInterrupt:
        server.server_close()
        print(time.asctime(),"Server Stopped")
    except:
        server.server_close()
        print(time.asctime(),"Server Stopped")
    return server.udp_list

# udp2bin
def udp2bin_tool(udp_folder_Or_list):
    Save = True
    bin_list = []

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

# bin2ply
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(folder,file), "rb") as bin_file:
            buf = bin_file.read()
            buf_data = MicroWaveParser.ParserFrame(buf)


            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):
        abspath = os.path.abspath(path)
    else:
        raise ValueError("Not a path!")
    
    if os.path.isdir(abspath):
        # open folder to load all ply files
        ply_files = file_fliter(abspath,'ply')
        show = []
        for ply in ply_files:
            pcd = o3d.io.read_point_cloud(os.path.join(abspath,ply))
            show.append(pcd)
    else:
        show = [o3d.io.read_point_cloud(abspath)]

    o3d.visualization.draw_geometries(show)
    return

 src/microwave/parsers.py

@@ -1,23 +1,60 @@

from tkinter import Frame
from .components import *
from .constants import *
from ctypes import sizeof, string_at


perDetectionBytes = 8
perTrackerBytes = 32

class MicroWaveParser:
    @classmethod
    def ParserFrame(cls, buffer: bytes) -> list:
        """
        解析完整的一帧数据，数据包括数据头，数据体，数据尾，返回点云数组
        解析完整的帧数据，数据包括数据头，数据体，数据尾，返回点云数组
        """
        header = HeaderBlock(buffer[:sizeof(HeaderBlock)])
        if HEADER_MAGIC == string_at(header.Magic, 8):
            footer = FooterBlock(buffer[-sizeof(FooterBlock):])
            point_cloud = []
            for i in range(header.NumberOfDetection):
                detection = DetectionBlock(header, footer, buffer[i * 8 + sizeof(HeaderBlock): (i + 1) * 8 + sizeof(HeaderBlock)])
                point = (detection.X, detection.Y, detection.Z)
                point_cloud.append(point)
            return point_cloud
        else:
            return []
        buffer_size = len(buffer)
        frame_start = 0
        frame_end = 0
        frame_id = 0

        ply_data_list = []

        while frame_start < buffer_size:
            # process the current frame
            header = HeaderBlock(buffer[frame_start:frame_start+sizeof(HeaderBlock)])
            if HEADER_MAGIC == string_at(header.Magic, 8):
                numsDetection = header.NumberOfDetection
                numsTracker = header.NumberOfTrack

                # get the end point of the current frame
                frame_end = frame_start + \
                            sizeof(HeaderBlock) + \
                            numsDetection * perDetectionBytes + \
                            numsTracker * perTrackerBytes + \
                            sizeof(FooterBlock)

                if frame_end <= buffer_size:
                    # get the frame data and footer data
                    frame = buffer[frame_start:frame_end]
                    footer = FooterBlock(frame[-sizeof(FooterBlock):])
                    # get the cloud point data
                    point_cloud = []

                    for i in range(numsDetection):

                        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_cloud.append(point)

                    # append the point cloud data to the list
                    ply_data_list.append([frame_id, header.FrameNumber, point_cloud])

                # update the parameters for next frame in the while loop
                frame_id += 1
                frame_start = frame_end
            else:
                raise ValueError("wrong header magic code for microwave data")
                
        return ply_data_list

 src/slam/__init__.py

New file
@@ -0,0 +1,3 @@
from .core import *
from .manualMode import *
from .autoMode import *

 src/slam/autoMode.py

New file
@@ -0,0 +1,45 @@
import os,sys
import numpy as np
import time
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"))

def rotateAndMerge(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):

    # 旋转机构复位
    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(int(time_delay))
        subFolder = str(-i)
        print (f"Geting udp files with {subFolder} degree ...")
        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
    print("merge ply files, processing ...")
    manual_merge(saveFolder,alpha,beta,gamma,"data")






 src/slam/core.py

New file
@@ -0,0 +1,127 @@
import numpy as np
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(0, src_path)

from ..utils.tools import read_ply
from tqdm import tqdm



# 毫米波坐标系为右手坐标系。
# 毫米波设备分别绕xyz轴的旋转角度分别为 alpha, beta, gamma

# 左乘旋转矩阵分别为 Rx_alpha, Ry_beta, Rz_gamma

# Rx_alpha = [[1,0,0],[0,np.cos(alpha),np.sin(alpha)],[0,-np.sin(alpha),np.cos(alpha)]]
# Ry_beta = [[np.cos(beta),0,-np.sin(beta)],[0,1,0],[np.sin(beta),0,np.cos(beta)]]
# 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):
    """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)

    alpha = rad_list[0]
    Rx_alpha = np.matrix([[1,0,0],
                        [0,np.cos(rad_list[0]),np.sin(alpha)],
                        [0,-np.sin(alpha),np.cos(alpha)]])

    beta = rad_list[1]
    Ry_beta = np.matrix([[np.cos(beta),0,-np.sin(beta)],
                        [0,1,0],
                        [np.sin(beta),0,np.cos(beta)]])

    gamma = rad_list[2]
    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

    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("\n rotation matrix\n",rotation_matrix)
    else:
        pass

    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 transferToZero(data,x,y,z,alpha,beta,gamma,reverse=False):
    """
    转换当前坐标至另一坐标, xyz分别为当前坐标原点在目标坐标系的位置;
    alpha,beta,gamma 为当前坐标系相对目标坐标系的旋转参数。
    """

    # 平移
    moved_data = data + 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 merge_ply(muti_ply_with_angle: list):

    ply_nums = len(muti_ply_with_angle)
    merged_ply_array = None

    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]

        if isinstance(ply,str):
            ply = read_ply(ply)
        
        rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2])

        # 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)
        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)
    return merged_ply_array




 src/slam/manualMode.py

New file
@@ -0,0 +1,70 @@
from aifc import Error
import os,sys
from tkinter import E
import numpy as np
from tqdm import tqdm

from .core import merge_ply
from ..utils import read_ply,write_ply,file_fliter
from ..microwave.deviceTools import udp2bin_tool,bin2ply_tool

def manual_merge(folder,alpha,beta,gamma,savename):
    """ 
    merge the ply files according to the sub folder name, like 0, 20, -30 ...
    """

    for root, dirs, files in os.walk(folder,topdown=True):
        break
    
    dir_List = dirs


    angle = []
    index = 0 
    for i,degree in enumerate([alpha,beta,gamma]):
        if degree == "auto":
            degree = dir_List
            index = i
        angle.append(degree)

    data = []
    
    print("reading ply files ...")
    # iterate the auto angle degree
    for an in tqdm(angle[index]):
        ply_with_mutiangle = []
        for z in angle:
            if isinstance(z,list):
                z = an
            ply_with_mutiangle.append(-float(z))
        ply_name_list = file_fliter(os.path.join(folder,an), "ply",lambda x:int(x.split(".")[0]))
        if len(ply_name_list) == 0:
            try:
                udp2bin_tool(os.path.join(folder,an))
                bin2ply_tool(os.path.join(folder,an))
                ply_name_list = file_fliter(os.path.join(folder,an), "ply",lambda x:int(x.split(".")[0]))
            except Exception as e:
                raise ValueError(f"Can't find any .udp .bin or .ply files in folder {os.path.join(folder,an)}")
        else:
            pass

        # select the all ply files
        for i in range(len(ply_name_list)):
            ply_with_mutiangle_copy = ply_with_mutiangle.copy()
            ply = read_ply(os.path.join(folder,an,ply_name_list[i]))
            ply_with_mutiangle_copy.append(ply)
            data.append(ply_with_mutiangle_copy)

    # compose muti ply files to one ply file
    cloudpoint = merge_ply(data)

    # write the file
    print("Writing the data to disk ...")
    write_ply(cloudpoint, folder, savename)








 src/utils/bin2ply.py

File was deleted

 src/utils/plot_ply.py

File was deleted

 src/utils/tools.py

@@ -1,7 +1,9 @@
import os, sys
import numpy as np
import time
from tqdm import tqdm

def file_fliter(folder:str, file_ext:str, sort_format):
def file_fliter(folder:str, file_ext:str, sort_format = None):

    files_List = os.listdir(folder)
    target_files = []
@@ -10,8 +12,10 @@
    for file in files_List:
        if file.split('.')[-1] == file_ext:
            target_files.append(file)
    # sort the list by the sort_format
    target_files.sort(key=sort_format)

    if sort_format is not None:
        # sort the list by the sort_format
        target_files.sort(key=sort_format)

    return target_files

@@ -38,6 +42,29 @@
    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

        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 __name__ == "__main__":

 src/utils/udp2bin.py

File was deleted

 src/utils/udp_get.py

File was deleted