debug for udp_get server

wu_xinjun

2022-03-27 658d5449f4e926f77bb9de34dfab486a451347dd

debug for udp_get server

(cherry picked from commit 4bbf4c2dcd0a7a1485cacf3373f41904782ae02c)

 .vscode/launch.json

@@ -32,11 +32,11 @@
            "program": "${workspaceFolder}/scripts/slamManualMode.py",
            "console": "integratedTerminal",
            "args": [
                "../../04_Data/20220316test",
                "../../04_Data/LinChiLi220325-1",
                "0",
                "20",
                "auto",
                "test60120"
                "data"
            ],
            "justMyCode": true
        },

 src/microwave/servers.py

@@ -1,6 +1,7 @@
import os, sys
from ctypes import sizeof, string_at
from socketserver import BaseRequestHandler, UDPServer
import warnings
from .components import *
from .constants import *
from ..protos import aerial_pb2 as pb
@@ -26,6 +27,39 @@
        self.DataFrame = None 
        self.Header = None
        self.Footer = None

    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
            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)
            N = self.Header.NumberOfDetection
            Nt = self.Header.NumberOfTrack
            self.PointCount = 0
            self.TrackCount = 0
            self.DataLength = N * 8 + Nt * sizeof(TrackerBlock)
            self.RemainLength = self.DataLength + sizeof(FooterBlock)
            self.data_buf = bytearray()
            self.footer_buf = bytearray()
            self.Points = []
            self.ReceivedDataLength = 0
            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:
        """
@@ -37,7 +71,13 @@
            # if received data length equal expected data length
            # then we can get footer block
            self.state = UDP_STATE_FOOTER
        return
            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
@@ -50,54 +90,46 @@
            # 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)
                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 
            self.Header = HeaderBlock.from_buffer_copy(buf[:sizeof(HeaderBlock)])
            if HEADER_MAGIC == string_at(self.Header.Magic, 8):
                self.DataFrame = bytearray(buf)
                N = self.Header.NumberOfDetection
                Nt = self.Header.NumberOfTrack
                self.PointCount = 0
                self.TrackCount = 0
                self.DataLength = N * 8 + Nt * sizeof(TrackerBlock)
                self.RemainLength = self.DataLength + sizeof(FooterBlock)
                self.data_buf = bytearray()
                self.footer_buf = bytearray()
                self.Points = []
                self.ReceivedDataLength = 0
                self.ReceivedFooterLength = 0
                # the next packet should be data block
                self.state = UDP_STATE_DATA
        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])
                
                # 判断第一个数据包中是否包含Footer Block
                if (header_size + data_size) < buf_size:
                    self._receive_footer(buf[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:])

        elif self.state == UDP_STATE_DATA:
            # receive data block
            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 data_size < buf_size:
                self._receive_footer(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(bytes(buf))

 src/slam/autoMode.py

@@ -15,7 +15,7 @@
    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/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