add feature [process mutiFrame bin file]:

wu_xinjun

2022-03-15 83512537f8f72912386611488eb39437001e050e

add feature [process mutiFrame bin file]:

commit 263e56f9ec8d94a984bc45588acb662c590e6e3a
Author: wu_xinjun <bent20140204@outlook.com>
Date:   Tue Mar 15 14:51:34 2022 +0800

    process mutiFrame bin file

commit 9671b3582a1cb9bddf461908e83617cc22001bf5
Author: wu_xinjun <bent20140204@outlook.com>
Date:   Mon Mar 14 14:29:27 2022 +0800

    add muti-frame ply to the composed ply

 .vscode/launch.json

@@ -93,7 +93,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"
        },
@@ -104,7 +104,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"
        }

 TEST_Slam.bat

@@ -12,13 +12,17 @@
    call %envsetting% 
)

set folder=T:\\Data\\projects\\huaneng\\00_Docs\\slamtest
echo [the folder example]: ./slamdata/
set /p folder=Please input the slam data folder:

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

set PLYfile=./%savename%.ply

cd %~dp0
echo change to path: %cd%

 TEST_bin2ply.bat

@@ -14,7 +14,9 @@
cd %~dp0
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%

 TEST_udp2bin.bat

@@ -13,8 +13,8 @@
)
cd %~dp0
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%

 TEST_udp2ply.bat

@@ -14,7 +14,8 @@
cd %~dp0
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%

 TEST_udp_get.bat

@@ -17,7 +17,15 @@
set MICROWAVEhost=192.168.2.65
SET PACKETthreshold=1000
SET DEVICEname=pod1
SET SAVEfolder=./Manual_Microwave_test

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% ./src/utils/udp_get.py %MICROWAVEhost% %PACKETthreshold% %DEVICEname% %SAVEfolder%
%aerialpython% ./src/utils/udp_get.py %MICROWAVEhost% %PACKETthreshold% %DEVICEname% %SAVEfolder% %SUBfolder%
pause

 src/mannul_slam.py

@@ -7,12 +7,10 @@
    
    # literate the folder
    dir_List_ = os.listdir(folder)
    sort_lambda = lambda x:int(x.split("°")[0])
    sort_lambda = lambda x:int(x)
    dir_List_.sort(key=sort_lambda)
    
    dir_List=[]
    for k in dir_List_:
        dir_List.append(k[:-1])
    dir_List=dir_List_


    angle = []
@@ -32,11 +30,13 @@
            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]))
        ply_name_list = file_fliter(os.path.join(folder,an), "ply",lambda x:int(x.split(".")[0]))
        # select the first ply file
        ply = read_ply(os.path.join(folder,an+"°",ply_name_list[0]))
        ply_with_mutiangle.append(ply)
        data.append(ply_with_mutiangle)
        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 = compose_ply(data)

 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)])
        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):
            footer = FooterBlock(buffer[-sizeof(FooterBlock):])
                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(header.NumberOfDetection):
                detection = DetectionBlock(header, footer, buffer[i * 8 + sizeof(HeaderBlock): (i + 1) * 8 + sizeof(HeaderBlock)])

                    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)
            return point_cloud

                    # 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:
            return []
                raise ValueError("wrong header magic code for microwave data")
                
        return ply_data_list

 src/utils/bin2ply.py

@@ -21,8 +21,8 @@
    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', sort_lambda)
        # sort_lambda = lambda x:int(x.split('.')[0])
        bin_files = file_fliter(BINfolder,'bin')

    except Exception as e:
        print(e)
@@ -31,11 +31,18 @@
    for file in bin_files:

        filename = file
        (fn, ext) = os.path.splitext(filename)
        # (fn, ext) = os.path.splitext(filename)
        with open(os.path.join(BINfolder,file), "rb") as bin_file:
            buf = bin_file.read()
            point_cloud = MicroWaveParser.ParserFrame(buf)
            with open(os.path.join(BINfolder,fn +".ply"), "w") as f:
            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(BINfolder,str(FrameNumber) +".ply"), "w") as f:
                f.write("ply\n")
                f.write("format ascii 1.0\n")
                f.write("comment MicroWave2Ply generated\n")
@@ -51,4 +58,4 @@
                    x, y, z = p
                    f.write("{:.5f} {:.5f} {:.5f}\n".format(x, y, z))
                    n_p += 1
                print(time.asctime(),f"Frame {fn}.ply have been saved with {n_p} points!")
                        print(time.asctime(),f"File {file} Frame {FrameNumber}.ply have been saved with {n_p} points!")

 src/utils/tools.py

@@ -2,7 +2,7 @@
import numpy as np
import time

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 = []
@@ -11,6 +11,8 @@
    for file in files_List:
        if file.split('.')[-1] == file_ext:
            target_files.append(file)

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


 src/utils/udp_get.py

@@ -25,6 +25,9 @@
            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)
@@ -46,17 +49,19 @@
if __name__ == '__main__':

    def help():
        print("Usage: udp_get.py [MICROWAVEhost] [PACKETthreshold] [DEVICEname] [SAVEfolder]")
        print("EXAMPLE: udp_get.py 192.168.2.65 1000 pod1 ./manual_ply")
        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) != 5:
    if len(sys.argv) != 6:
        help()
    try:
        MICROWAVEhost = str(sys.argv[1])
        PACKETthreshold = int(sys.argv[2])
        DEVICEname = str(sys.argv[3])
        SAVEfolder = str(sys.argv[4])
        SUBfolder = str(sys.argv[5])

        global packet_threshold
        packet_threshold = PACKETthreshold
@@ -64,6 +69,8 @@
        device_id = DEVICEname
        global save_folder
        save_folder = SAVEfolder
        global sub_folder
        sub_folder = SUBfolder

    except Exception as e:
        print(e)

 test60120.ply

Diff too large