rearrange the dir tree

wu_xinjun

2022-03-23 1605366b3f6a522cbc40a3b5ec52f9cb873759ba

rearrange the dir tree

(cherry picked from commit 0ffe6428db7ebf90bf8310026ea43af4eb8d9c0d)
(cherry picked from commit 28668532ed2f97a3883be7e68bced252bd1f99bf)

 .vscode/settings.json

@@ -15,5 +15,7 @@
            "type": "jupyterKernelspec",
            "path": "${workspaceFolder}/env/aerial_deploy/python.exe"
        }
    ]
    ],
    "python.envFile": "${workspaceFolder}/env/aerial_deploy",
    "python.defaultInterpreterPath": "${workspaceFolder}/env/aerial_deploy/python.exe"
}

 TEST_Slam.bat

File was deleted

 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

File was renamed from TEST_SlamAuto.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%

@REM pre-defined parameters
set port=COM11
@@ -55,16 +59,16 @@

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% %time_delay% %alpha% %beta% %gamma% %microwaveHost% %udp_packets% %deviceName% %saveFolder%
    %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% ./src/utils/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,6 +12,8 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

echo [the folder example]: ./slamdata/
@@ -19,5 +21,5 @@


@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=./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.224
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%

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,13 +12,15 @@
    call %envsetting% 
)
cd %~dp0
cd ..

echo change to path: %cd%

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

File was renamed from TEST_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
@@ -27,5 +29,5 @@
set folder=./%SAVEfolder%/%SUBfolder%/
echo the udp data will be saved in folder %folder%
@REM ---------------Usage: udp_get.py [MICROWAVEhost] [PACKETthreshold] [DEVICEname] [SAVEfolder]
%aerialpython% ./src/utils/udp_get.py %MICROWAVEhost% %PACKETthreshold% %DEVICEname% %SAVEfolder% %SUBfolder%
%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/auto_slam.py

File was deleted

 src/mannul_slam.py

File was deleted

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

File was renamed from src/slam/slam.py
@@ -1,11 +1,11 @@
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)
# # 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 ..utils.tools import read_ply
from tqdm import tqdm


@@ -84,7 +84,7 @@
    """

    # 平移
    moved_data = current + np.array([x,y,z])
    moved_data = data + np.array([x,y,z])
    # 旋转
    rotation_matrix = get_rotation_matrix(-alpha,-beta,-gamma)
    rotated_data = np.matmul(moved_data,rotation_matrix)
@@ -97,10 +97,10 @@
    return rotated_data


def compose_ply(muti_ply_with_angle: list):
def merge_ply(muti_ply_with_angle: list):

    ply_nums = len(muti_ply_with_angle)
    composed_ply_array = None
    merged_ply_array = None

    print ("rotating the ply ...")
    for i, ply_with_angle in enumerate(tqdm(muti_ply_with_angle)):
@@ -116,18 +116,12 @@
        # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
        rotated_ply = np.matmul(ply,rotation_matrix)
        if i == 0:
            composed_ply_array = rotated_ply.copy()
            merged_ply_array = rotated_ply.copy()
        else:
            composed_ply_array = np.concatenate((composed_ply_array,rotated_ply),axis= 0 )
            merged_ply_array = np.concatenate((merged_ply_array,rotated_ply),axis= 0 )
    
    composed_ply_array = np.array(composed_ply_array).reshape(-1,3)
    return composed_ply_array
    merged_ply_array = np.array(merged_ply_array).reshape(-1,3)
    return merged_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/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/__init__.py

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

 src/utils/bin2ply.py

File was deleted

 src/utils/plot_ply.py

File was deleted

 src/utils/udp2bin.py

File was deleted

 src/utils/udp_get.py

File was deleted