optimization for auto slam

wu_xinjun

2022-03-22 8f8099f618d6b910103555a04ae3b9830e1e6c92

optimization for auto slam

(cherry picked from commit fee0c7c1872b32141612ad0b3730b865e7e4dace)

 .vscode/launch.json

@@ -15,6 +15,7 @@
                "COM11",
                "100",
                "5",
                "5",
                "0",
                "20",
                "auto",

 TEST_SlamAuto.bat

@@ -14,8 +14,9 @@

@REM pre-defined parameters
set port=COM11
set speed=100
set speed=200
set degree_step=5
set time_delay=5
set alpha=0
set beta=20
set gamma=auto
@@ -26,12 +27,13 @@

@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 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):
set /p udp_packets=Please input the udp packets num (example: 1000):
@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
@@ -41,6 +43,7 @@
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%
@@ -56,7 +59,7 @@
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%
    %aerialpython% ./src/auto_slam.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%
) else (

 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!

 src/auto_slam.py

@@ -8,16 +8,18 @@
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):
def run(port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):

    # 旋转机构复位
    os.system(f"{exe_path} {port} 100 0")
    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(3)
        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)
    
    # 旋转机构复位
@@ -35,32 +37,34 @@


    # compose ply
    print("merge ply files, processing ...")
    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/")
        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) != 11:
    if len(sys.argv) != 12:
        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])
        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 = str(sys.argv[11])
    except Exception as e:
        print(e)
        help()

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



 src/mannul_slam.py

@@ -2,6 +2,7 @@
import numpy as np
from slam.slam import compose_ply
from utils import read_ply,write_ply,file_fliter
from tqdm import tqdm

def run(folder,alpha,beta,gamma,savename):
    
@@ -28,8 +29,9 @@

    data = []
    
    print("reading ply files ...")
    # iterate the auto angle degree
    for an in angle[index]:
    for an in tqdm(angle[index]):
        ply_with_mutiangle = []
        for z in angle:
            if isinstance(z,list):
@@ -47,6 +49,7 @@
    cloudpoint = compose_ply(data)

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



 src/slam/slam.py

@@ -6,6 +6,7 @@
sys.path.insert(0, src_path)

from utils.tools import read_ply
from tqdm import tqdm



@@ -76,12 +77,33 @@
    data = rotated_ply
    return data

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

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

    ply_nums = len(muti_ply_with_angle)
    composed_ply_array = None

    for i, ply_with_angle in enumerate( muti_ply_with_angle):
    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]

@@ -94,12 +116,11 @@
        # 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 = np.array(rotated_ply)
            composed_ply_array = rotated_ply.copy()
        else:
            temp = np.array(rotated_ply)
            composed_ply_array = np.concatenate((composed_ply_array,temp),axis= 0 )
            composed_ply_array = np.concatenate((composed_ply_array,rotated_ply),axis= 0 )
    
    composed_ply_array = composed_ply_array.reshape(-1,3)
    composed_ply_array = np.array(composed_ply_array).reshape(-1,3)
    return composed_ply_array



 src/utils/tools.py

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

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

@@ -56,7 +57,7 @@
        f.write("end_header\n")
        n_p = 0

        for p in data:
        for p in tqdm(data):
            x = p[0]
            y = p[1]
            z = p[2]

 src/utils/udp2bin.py

@@ -11,25 +11,47 @@
from microwave.servers import MicroWaveReceiver
from .tools import file_fliter

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

    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)
    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:
        filename = os.path.join(UDPfolder, file)
        with open(filename, "rb") as f:
            data = f.read()
            receiver.AddBuf(data)
            if receiver.DataFrameReady:
                frame_number = receiver.Header.FrameNumber
        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
    





 src/utils/udp_get.py

@@ -12,26 +12,12 @@
#     os.makedirs(file_folder)
#     print(f"make a new folder: {file_folder}")

packed_count = 0

class PointCloudUDPRequestHandler(BaseRequestHandler):

    def handle(self):

        global packed_count
        global path

        if packed_count == 0:
            start = datetime.datetime.now()
            if not os.path.exists(save_folder):
                os.mkdir(save_folder)
            if sub_folder is not None:
                path = os.path.join(save_folder,sub_folder)
            else:
                path = os.path.join(save_folder,f"{device_id}-{start.strftime('%Y-%m-%d-%H-%M-%S')}")
            if not os.path.exists(path):
                os.mkdir(path)
            path = os.path.abspath(path)
        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
@@ -40,29 +26,33 @@
        f = open('{}/{}_packet{}.udp'.format(path,time_tag,packed_count), 'wb')
        f.write(msg)
        f.close()
        packed_count += 1
        self.server.packed_count += 1
        self.server.udp_list.append(msg)
        
        if packed_count >= packet_threshold:
            UDPServer.server_close(server)

            UDPServer.server_close(self.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 = ''
    # 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.')
@@ -74,8 +64,7 @@
    except:
        server.server_close()
        print(time.asctime(),"Server Stopped")


    return server.udp_list


if __name__ == '__main__':