Merge commit 'mqdev' into mqaddFeature

wu_xinjun

2022-06-07 da6ff0858f4997f79f5d8cdc88b7367a5cb710b1

Merge commit 'mqdev' into mqaddFeature

 .gitignore

@@ -4,6 +4,11 @@
# proto
src/protos/aerial_pb2.py

# projects
.vscode
.vs
.idea

# env
env/aerial_deploy
.env/

 .vscode/launch.json

File was deleted

 .vscode/settings.json

File was deleted

 RUN/Test_Servo.dll

Binary files differ

 RUN/Test_Servo.pdb

Binary files differ

 env/JustDoubleClickMe.bat

@@ -6,7 +6,8 @@
echo setting done!
echo ========== Testing Env ==============
echo test it with hello word...
set aerialpython=%cd%/../../aerial_deploy/python.exe
cd %~dp0
set aerialpython=%cd%/../.env/python.exe
%aerialpython% -c "import cv2, pika, flask, pika; print('hello world!')"
echo =========== ALL DONE ================


 env/SettingEnv.bat

@@ -20,7 +20,11 @@
tar -xzf "./aerial.tar.gz" -C "../../aerial_deploy"

cd .. 
mklink /j ".env" "../aerial_deploy/"
if not exist ".env" (
    mklink /j ".env" "../aerial_deploy/")

cd %~dp0
call "../src/protos/compile.bat"

echo ALL DONE!


 scripts/mqClient.py

New file
@@ -0,0 +1,54 @@
import signal
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.engine.AcquisitionMain import AcquisitionMain
from src.engine.AcquisitionMain import Config

def quit(signum, frame):
    print('You choose to stop me.')
    sys.exit()

if __name__ == '__main__':
    def help():
        print("Usage: Client.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [DEVICEname] [SERVERhost] [MICROWAVEhost]")
        print("EXAMPLE: Client.py 172.17.17.206 chai password123 aerial_rpc pod1 http://172.17.17.206/api 192.168.2.65")
        exit(1)

    if len(sys.argv) != 8:
        help()
    try:
        MQhost = str(sys.argv[1])
        MQuser = str(sys.argv[2])
        MQpassword = str(sys.argv[3])
        QUEUEname = str(sys.argv[4])
        DEVICEname = str(sys.argv[5])
        SERVERhost = str(sys.argv[6])
        MICROWAVEhost = str(sys.argv[7])

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

    print("Start AcquisitionMain")
    signal.signal(signal.SIGINT, quit)
    signal.signal(signal.SIGTERM, quit)
    # client = AcquisitionMain('192.168.3.54', 'chai',
    #                       'password123', 'aerial_rpc', 'pod1', 'http://192.168.3.54:5000/api', 10)
    config_dir = os.path.join(__file__, "..",  "../src/config.json" )
    config_path = os.path.abspath(config_dir)
    config = Config(config_path)
    client = AcquisitionMain(MQhost,
                             MQuser,
                             MQpassword, 
                             QUEUEname, 
                             DEVICEname, 
                             SERVERhost, 
                             MICROWAVEhost, # udp server host, 192.168.2.65 for production
                             config)
    client.start()

 scripts/mqServer.py

New file
@@ -0,0 +1,33 @@
import sys
import os

# 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.Server import MyServer


if __name__ == "__main__":
    def help():
        print("Usage: Server.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [WEBport] [SAVEfolder]")
        print("EXAMPLE: Server.py localhost chai password123 aerial_rpc 5000 ./Images")
        exit(1)

    if len(sys.argv) != 7:
        help()
    try:
        MQhost = str(sys.argv[1])
        MQuser = str(sys.argv[2])
        MQpassword = str(sys.argv[3])
        QUEUEname = str(sys.argv[4])
        WEBport = int(sys.argv[5])
        SAVEfolder = str(sys.argv[6])
        save_folder = SAVEfolder

    except Exception as e:
        print(e)
        help()
    server = MyServer(host=MQhost, user=MQuser, password=MQpassword, queue=QUEUEname, port=WEBport, SAVEfolder=save_folder)
    server.run_all()

 scripts/slamAutoMode.py

@@ -9,27 +9,28 @@

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/")
        print("Usage: auto_slam.py [servo_path][port] [speed] [degre_step] [time_delay] [alpha] [beta] [gamma] [microwaveHost] [udp_packets] [deviceName] [saveFolder]")
        print("EXAMPLE: auto_slam.py ./RUN/Test_Servo.exe COM11 100 5 5 0 20 auto 192.168.2.65 1000 pod1 ./data/run_test/")
        exit(1)

    if len(sys.argv) != 12:
    if len(sys.argv) != 13:
        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]))
        servo_path = str(sys.argv[1])
        port = str(sys.argv[2])
        speed = str(sys.argv[3])
        degre_step = float(sys.argv[4])
        time_delay = int(sys.argv[5])
        alpha = str(sys.argv[6])
        beta = str(sys.argv[7])
        gamma = str(sys.argv[8])
        microwaveHost = str(sys.argv[9])
        udp_packets = int(sys.argv[10])
        deviceName = str(sys.argv[11])
        saveFolder = os.path.abspath(str(sys.argv[12]))
    except Exception as e:
        print(e)
        help()

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

 scripts_launch/TEST_SlamAuto.bat

@@ -17,7 +17,8 @@
echo change to path: %cd%

@REM pre-defined parameters
set port=COM11
set servo_path=./RUN/Test_Servo.exe
set port=COM14
set speed=200
set degree_step=2
set time_delay=5
@@ -30,10 +31,11 @@
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):
set /P degree_step=Please input the degree step of rotation machine (example: 2):
set /P time_delay=Please input the delay time of rotation machine (example: 5):
set /P servo_path=Please input the path of servo controller (example: ./RUN/Test_Servo.exe):
set /p port=Please input the serial port of servo (example: COM14):
set /P speed=Please input the speed of servo (example: 200):
set /P degree_step=Please input the degree step of servo (example: 2):
set /P time_delay=Please input the delay time of servo (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):
@@ -44,6 +46,7 @@

@REM verify the parameters
echo    ################# parameters #################
echo    servo_path              :%servo_path%
echo    port                    :%port%
echo    speed                   :%speed%
echo    degree_step             :%degree_step%
@@ -61,7 +64,7 @@


if %choice% == y (
    %aerialpython% ./scripts/slamAutoMode.py %port% %speed% %degree_step% %time_delay% %alpha% %beta% %gamma% %microwaveHost% %udp_packets% %deviceName% %saveFolder%
    %aerialpython% ./scripts/slamAutoMode.py %servo_path% %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 (

 scripts_launch/TEST_startClient.bat

@@ -16,7 +16,7 @@

echo change to path: %cd%

set MQhost=192.168.1.14
set MQhost=localhost
set MQuser=chai
set MQpassword=password123
set QUEUEname=aerial_rpc
@@ -30,6 +30,6 @@
echo Client.py is running ...
@REM -------- Usage: Client.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [DEVICEname] [SERVERhost] [MICROWAVEhost]
@REM %aerialpython% ./src/Client.py %MQhost% %MQuser% %MQpassword% %QUEUEname% %DEVICEname% %SERVERhost% %MICROWAVEhost% >> %LOGfile%
%aerialpython% ./src/Client.py %MQhost% %MQuser% %MQpassword% %QUEUEname% %DEVICEname% %SERVERhost% %MICROWAVEhost%
%aerialpython% ./scripts/mqClient.py %MQhost% %MQuser% %MQpassword% %QUEUEname% %DEVICEname% %SERVERhost% %MICROWAVEhost%
echo Client.py has been terminited.
pause

 scripts_launch/TEST_startServer.bat

@@ -16,7 +16,8 @@

echo change to path: %cd%

set MQhost=localhost
@REM set MQhost=localhost
set MQhost=47.92.33.19
set MQuser=chai
set MQpassword=password123
set WEBhost=0.0.0.0
@@ -27,8 +28,8 @@
@REM Launch the Server
echo the log file will be saved in the [%LOGfile%] file
echo Server.py is running ...
@REM -------- Usage: Server.py [MQhost] [MQuser] [MQpassword] [WEBhost] [WEBport] [SAVEfolder]")
%aerialpython% ./src/Server.py %MQhost% %MQuser% %MQpassword% %WEBhost% %WEBport% %SAVEfolder%
@REM -------- Usage: Server.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [WEBport] [SAVEfolder]")
%aerialpython% ./scripts/mqServer.py %MQhost% %MQuser% %MQpassword% %QUEUEname% %WEBport% %SAVEfolder%
echo Server.py has been terminited.
pause


 src/Client.py

@@ -1,50 +1,49 @@
import signal
import os, sys
# import signal
# import os, sys

batfile_dir = os.path.join(__file__, "..", "protos/compile.bat" )
batfile_path = os.path.abspath(batfile_dir)
os.system(batfile_path)

from engine.AcquisitionMain import AcquisitionMain
from engine.AcquisitionMain import Config
# from .engine.AcquisitionMain import AcquisitionMain
# from .engine.AcquisitionMain import Config

def quit(signum, frame):
    print('You choose to stop me.')
    sys.exit()
# def quit(signum, frame):
#     print('You choose to stop me.')
#     sys.exit()

if __name__ == '__main__':
    def help():
        print("Usage: Client.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [DEVICEname] [SERVERhost] [MICROWAVEhost]")
        print("EXAMPLE: Client.py 172.17.17.206 chai password123 aerial_rpc pod1 http://172.17.17.206/api 192.168.2.65")
        exit(1)
# if __name__ == '__main__':
#     def help():
#         print("Usage: Client.py [MQhost] [MQuser] [MQpassword] [QUEUEname] [DEVICEname] [SERVERhost] [MICROWAVEhost]")
#         print("EXAMPLE: Client.py 172.17.17.206 chai password123 aerial_rpc pod1 http://172.17.17.206/api 192.168.2.65")
#         exit(1)

    if len(sys.argv) != 8:
        help()
    try:
        MQhost = str(sys.argv[1])
        MQuser = str(sys.argv[2])
        MQpassword = str(sys.argv[3])
        QUEUEname = str(sys.argv[4])
        DEVICEname = str(sys.argv[5])
        SERVERhost = str(sys.argv[6])
        MICROWAVEhost = str(sys.argv[7])
#     if len(sys.argv) != 8:
#         help()
#     try:
#         MQhost = str(sys.argv[1])
#         MQuser = str(sys.argv[2])
#         MQpassword = str(sys.argv[3])
#         QUEUEname = str(sys.argv[4])
#         DEVICEname = str(sys.argv[5])
#         SERVERhost = str(sys.argv[6])
#         MICROWAVEhost = str(sys.argv[7])

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

    print("Start AcquisitionMain")
    signal.signal(signal.SIGINT, quit)
    signal.signal(signal.SIGTERM, quit)
    # client = AcquisitionMain('192.168.3.54', 'chai',
    #                       'password123', 'aerial_rpc', 'pod1', 'http://192.168.3.54:5000/api', 10)
    config_dir = os.path.join(__file__, "..", "config.json" )
    config_path = os.path.abspath(config_dir)
    config = Config(config_path)
    client = AcquisitionMain(MQhost,
                             MQuser,
                             MQpassword, 
                             DEVICEname,
                             MICROWAVEhost, # udp server host, 192.168.2.65 for production
    )
    client.start()
#     print("Start AcquisitionMain")
#     signal.signal(signal.SIGINT, quit)
#     signal.signal(signal.SIGTERM, quit)
#     # client = AcquisitionMain('192.168.3.54', 'chai',
#     #                       'password123', 'aerial_rpc', 'pod1', 'http://192.168.3.54:5000/api', 10)
#     config_dir = os.path.join(__file__, "..", "config.json" )
#     config_path = os.path.abspath(config_dir)
#     config = Config(config_path)
#     client = AcquisitionMain(MQhost,
#                              MQuser,
#                              MQpassword, 
#                              QUEUEname, 
#                              DEVICEname, 
#                              SERVERhost, 
#                              MICROWAVEhost, # udp server host, 192.168.2.65 for production
#                              config)
#     client.start()

 src/Server.py

@@ -10,27 +10,24 @@
from zmq import device


batfile_dir = os.path.join(__file__, "..", "protos/compile.bat" )
batfile_path = os.path.abspath(batfile_dir)
os.system(batfile_path)

from protos import aerial_pb2 as pb
from engine import MessageThreads as ths
from .engine.DeviceManager import DeviceManager
from .engine.MessageQueueThread import MessageQueueThread
from .protos import aerial_pb2 as pb



class MyServer(Flask):
    def __init__(self, MQhost, MQuser, MQpassword, WEBhost, WEBport, SAVEfolder):
    def __init__(self, host, user, password, queue, port, SAVEfolder):
        Flask.__init__(self, __name__)
        # rabbit MQ
        self.MQhost = MQhost
        self.MQuser = MQuser
        self.MQpassword = MQpassword
        self.parameters = None
        # Web server
        self.WEBhost = WEBhost
        self.WEBport = WEBport
        self.SAVEfolder = SAVEfolder
        self.host = host
        self.user = user
        self.password = password
        self.queue = queue
        self.port = port
        global save_folder
        save_folder = SAVEfolder
        

        self.route('/api', methods=['POST'])(self.api)
        self.route('/', methods=['GET'])(self.hello)
        self._def_routes()
@@ -151,6 +148,7 @@
        self.run_rabbitmq()
        self.run(debug=False, host=self.WEBhost, port=self.WEBport)

<<<<<<< HEAD

if __name__ == "__main__":
    def help():
@@ -173,3 +171,5 @@
        help()
    server = MyServer(MQhost, MQuser, MQpassword, WEBhost, WEBport, SAVEfolder)
    server.run_all()
=======
>>>>>>> aa8a0ba3a86ff5dbbdfcd68c72a5e70e1c52e877

 src/__init__.py

@@ -0,0 +1,5 @@
from .engine import *
from .microwave import *
from .protos import *
from .slam import *
from .utils import *

 src/engine/AcquisitionMain.py

@@ -15,12 +15,12 @@
from . import MessageThreads as ths
from .devices import cameraController

# 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 protos import aerial_pb2 as pb
from ..protos import aerial_pb2 as pb


IMAGE_WIDTH = 3840

 src/engine/CaptureThread.py

@@ -6,12 +6,12 @@
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(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 protos import aerial_pb2 as pb
from ..protos import aerial_pb2 as pb


class CaptureThread(threading.Thread):

 src/engine/ControllThread.py

@@ -6,12 +6,12 @@
import threading
from .DustCleanerThread import DustCleanerThread

# 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 protos import aerial_pb2 as pb
from ..protos import aerial_pb2 as pb

class ControllThread(threading.Thread):
    """

 src/engine/DustCleanerThread.py

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

from .DustCleaner import DustCleaner

# 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 protos import aerial_pb2 as pb
from ..protos import aerial_pb2 as pb




 src/engine/MessageQueueThread.py

@@ -5,13 +5,13 @@
from ctypes import sizeof
from .DeviceManager import DeviceManager

# 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 microwave.components import HeaderBlock
from protos import aerial_pb2 as pb
from ..microwave.components import HeaderBlock
from ..protos import aerial_pb2 as pb

class MessageQueueThread(threading.Thread):
    def __init__(self,

 src/engine/MicroWaveUDPThread.py

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

from microwave.servers import MicroWaveUDPServer
from ..microwave.servers import MicroWaveUDPServer


class MicroWaveUDPThread (threading.Thread):

 src/microwave/deviceTools.py

@@ -3,6 +3,8 @@
import time
import os, sys
import open3d as o3d
from multiprocessing import  Pool
import numpy as np

from ..utils import file_fliter, write_ply
from .servers import MicroWaveReceiver
@@ -109,41 +111,34 @@

    return bin_list

def bin2ply_f(folder,file):

    with open(os.path.join(folder,file), "rb") as bin_file:
        print(f"Extracting the data in {file} ...")
        buf = bin_file.read()
        buf_data = MicroWaveParser.ParserFrame(buf)
        for frame in buf_data:
            
            Frame_id = frame[0]
            FrameNumber = frame[1]
            point_cloud = np.array(frame[2])
            if len(point_cloud) == 0 :
                print(f"Notice: Frame {FrameNumber} is empty!")
            else:
                write_ply(point_cloud,folder,str(FrameNumber))

# bin2ply
def bin2ply_tool(folder):
def bin2ply_tool(folder,nums_worker = 4):
    pool = Pool(processes = nums_worker)

    bin_files = file_fliter(folder,'bin')

    for file in bin_files:
        pool.apply_async(func=bin2ply_f,args=(folder,file,))

        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)
    pool.close()
    pool.join()


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

 src/microwave/parsers.py

@@ -45,7 +45,7 @@

                        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 = [detection.X, detection.Y, detection.Z]
                        point_cloud.append(point)

                    # append the point cloud data to the list

 src/microwave/servers.py

@@ -1,15 +1,10 @@
import os, sys
from ctypes import sizeof, string_at
from socketserver import BaseRequestHandler, UDPServer
import warnings
from .components import *
from .constants import *

# 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 protos import aerial_pb2 as pb
from ..protos import aerial_pb2 as pb



@@ -33,44 +28,19 @@
        self.Header = None
        self.Footer = None
    
    def _receive_data(self, data) -> None:
        """
        receive data packet from udp
        """
        self.data_buf.extend(data)
        self.ReceivedDataLength += len(data)
        if self.ReceivedDataLength == self.DataLength:
            # if received data length equal expected data length
            # then we can get footer block
            self.state = UDP_STATE_FOOTER
        return

    def _receive_footer(self, data) -> None:
        # receive footer block
        self.footer_buf.extend(data)
        self.ReceivedFooterLength += len(data)
        if self.ReceivedFooterLength == sizeof(FooterBlock):
            self.Footer = FooterBlock.from_buffer_copy(self.footer_buf)
            # after received footer block, the data frame is ready
            self.DataFrameReady = True
            # reset state to idle, ready for next data frame
            self.state = UDP_STATE_IDLE
        return
    
    def AddBuf(self, buf: bytes) -> None:
        # add a udp packet to receiver
        buf_size = len(buf)
        if self.state == UDP_STATE_IDLE:
            if buf_size == sizeof(Handshake):
                handshake = Handshake.from_buffer_copy(buf)
    def _is_handshake(self,data):
        handshake = Handshake.from_buffer_copy(data)
                if HAND_SHAKE_MAGIC == string_at(handshake.Magic, 8):
                    self.DataFrameReady = False
                    self.state = UDP_STATE_HAND_SHAKE
                    self.DataFrame = None
                    self.Header = None
                    self.Footer = None
        elif self.state == UDP_STATE_HAND_SHAKE:
            # after received handshake packet, we can get header block 
            return True
        else:
            return False

    def _get_header(self, buf):
            self.Header = HeaderBlock.from_buffer_copy(buf[:sizeof(HeaderBlock)])
            if HEADER_MAGIC == string_at(self.Header.Magic, 8):
                self.DataFrame = bytearray(buf)
@@ -87,26 +57,82 @@
                self.ReceivedFooterLength = 0
                # the next packet should be data block
                self.state = UDP_STATE_DATA
            return True
        else:
            False

    def _receive_data(self, data) -> None:
        """
        receive data packet from udp
        """
        self.data_buf.extend(bytes(data))
        self.ReceivedDataLength += len(data)
        if self.ReceivedDataLength == self.DataLength:
            # if received data length equal expected data length
            # then we can get footer block
            self.state = UDP_STATE_FOOTER
            return True
        elif self.ReceivedDataLength > self.DataLength:
            self.state = UDP_STATE_IDLE
            warnings.warn(f"encounter an error when getting the detection data in udp packets ...")
            return False
        else:
            return True

    def _receive_footer(self, data) -> None:
        # receive footer block
        self.footer_buf.extend(bytes(data))
        self.ReceivedFooterLength += len(data)
        if self.ReceivedFooterLength == sizeof(FooterBlock):
            self.Footer = FooterBlock.from_buffer_copy(self.footer_buf)
            # after received footer block, the data frame is ready
            self.DataFrameReady = True
            # reset state to idle, ready for next data frame
            self.state = UDP_STATE_IDLE
        return

    
    def AddBuf(self, buf: bytes) -> None:
        # add a udp packet to receiver
        buf_size = len(buf)
        if buf_size == sizeof(Handshake):
            self._is_handshake(buf)

        if self.state == UDP_STATE_HAND_SHAKE:
            # after received handshake packet, we can get header block 
            try:
                # make sure to get a complete header
                result = self._get_header(buf)
            except Exception as e:
                warnings.warn(f"encounter an unknow error {e} when getting the header data in udp packets ...")
                result = False

            if not result:
                # waiting for the next handshake udp packet
                self.state = UDP_STATE_IDLE
            else:
                # process the detection data in the first data packets
                header_size = sizeof(HeaderBlock)
                # 处理第一个数据包中的数据
                data_size = min(buf_size - header_size, self.DataLength)
                self._receive_data(buf[header_size: header_size + data_size])

                if self._receive_data(buf[header_size: header_size + data_size]):
                
                # 判断第一个数据包中是否包含Footer Block
                if (header_size + data_size) < buf_size:
                    self._receive_footer(buf[header_size + data_size])
                        self._receive_footer(buf[header_size + data_size:])

        elif self.state == UDP_STATE_DATA:
            # receive data block
            self.DataFrame.extend(buf)
            self.DataFrame.extend(bytes(buf))
            remain_data_size = self.DataLength - self.ReceivedDataLength
            data_size = min(buf_size, remain_data_size)
            self._receive_data(buf[: data_size])
            if self._receive_data(buf[: data_size]):
            if data_size < buf_size:
                self._receive_footer(buf[data_size: ])

        elif self.state == UDP_STATE_FOOTER:
            # receive footer block
            self.DataFrame.extend(buf)
            self.DataFrame.extend(bytes(buf))
            self._receive_footer(buf)



 src/protos/compile.bat

@@ -2,10 +2,9 @@

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

set protoc=%cd%/../../../aerial_deploy/Library/bin/protoc.exe
set protoc=%cd%/../../.env/Library/bin/protoc.exe

%protoc% aerial.proto --python_out=%cd%

echo compile done!
echo protoc: aerial.proto compile done!

 src/slam/autoMode.py

@@ -4,17 +4,18 @@
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"))
# 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):
def rotateAndMerge(servo_path,port,speed,degre_step,time_delay,alpha,beta,gamma,microwaveHost,udp_packets,deviceName,saveFolder):

    # 旋转机构复位
    exe_path = os.path.abspath(servo_path)
    os.system(f"{exe_path} {port} 400 0")
    time.sleep(10)

    # 间隔获取udp
    for i in range(0,360,degre_step):
    for i in np.arange(0,360,degre_step):
        os.system(f"{exe_path} {port} {speed} {i}")
        time.sleep(int(time_delay))
        subFolder = str(-i)

 src/slam/core.py

@@ -21,7 +21,7 @@
# 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,order = "x_y_z",verbose=0):
def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,rank,order = "x_y_z",verbose=0):
    """generate a rotation matrix from the specific angel
    """

@@ -57,15 +57,25 @@
            rotation_matrix =  R_dic[s]
        else:
            rotation_matrix = R_dic[s] * rotation_matrix 

    m = np.identity(rank)
    m[:3,:3] = rotation_matrix

    if verbose == 1:
        print(f"\n{alpha_degree}, rad {alpha}, Rx_alpha \n", Rx_alpha)
        print(f"\n{beta_degree}, rad {beta}, Ry_beta \n", Ry_beta)
        print(f"\n{gamma_degree}, rad {gamma}, Rz_gamma\n",Rz_gamma)
        print("\n rotation matrix\n",rotation_matrix)
        print("\n rotation matrix\n",m)
    else:
        pass

    return rotation_matrix
    return m

def get_origin_coordinates():
    """
    获取毫米波设备数据中心相对舵机原点的坐标
    """
    pass

def transferTo(current,delta_x,delta_y,delta_z,alpha,beta,gamma,reverse=False):
    # Notice: 假定当前坐标系为右手系
@@ -120,17 +130,20 @@
        if isinstance(ply,str):
            ply = read_ply(ply)
        
        rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2])
        N_rows = ply.shape[0]
        N_cols = ply.shape[1]
        
        rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2],rank=N_cols)

        # z = (X * Y^T)^T = (Y * X^T)
        # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
        # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix.transpose())
        rotated_ply = np.matmul(ply,rotation_matrix.transpose())
        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)
    merged_ply_array = np.array(merged_ply_array).reshape(-1,N_cols)
    return merged_ply_array



 src/slam/manualMode.py

@@ -16,8 +16,8 @@
    for root, dirs, files in os.walk(folder,topdown=True):
        break
    
    dirs.sort(key = lambda x:float(x))
    dir_List = dirs


    angle = []
    index = 0 

 src/utils/tools.py

@@ -1,3 +1,4 @@
from email import header
import os, sys
import numpy as np
import time
@@ -19,51 +20,63 @@

    return target_files

def read_ply(path:str,start_line:int = 10):
    count = 0
    ply_list = []
def get_skip_rows(path):
    with open(path,"r") as ply_file:
        count = 0
        # read the line
        for line in ply_file.readlines():
            # strip the str of the header
            if count < start_line:
                pass
            flag = line.strip()
            if  flag == "end_header":
                return count + 1
            else:
                line_str_list = line.strip().split(' ')
                # convert the str type to float type
                line_float_list = []
                for value in line_str_list:
                    line_float_list.append(float(value))

                ply_list.append(line_float_list)
                if count >= 50:
                    return None
            count += 1
    ply_array = np.array(ply_list,dtype=np.float16)
    return None

def read_ply(path:str):
    start_line = get_skip_rows(path)
    if start_line is not None:
        ply_array = np.loadtxt(path,skiprows=start_line)
    else:
        ply_array = None
    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
    n_p = data.shape[0]
    n_cols = data.shape[1]

        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 n_cols == 3:
        header = ["ply\nformat ascii 1.0\n",
            "comment write_ply generated\n",
            f"element vertex {n_p}\n",
            "property float x\n",
            "property float y\n",
            "property float z\n",
            "end_header"]
        fmt = '%.6f','%.6f','%.6f'

    elif n_cols == 6:
        header = ["ply\nformat ascii 1.0\n",
            "comment write_ply generated\n",
            f"element vertex {n_p}\n",
            "property float x\n",
            "property float y\n",
            "property float z\n",
            "property uchar red\n",
            "property uchar green\n",
            "property uchar blue\n",
            "end_header"]
        fmt = '%.6f','%.6f','%.6f','%d','%d','%d'
    np.savetxt(file,data,
                delimiter=" ",
                header="".join(header),
                comments="",
                fmt=fmt)
    print(f"{time.asctime()} {name}.ply have been saved with {n_p} points!")