From 658d5449f4e926f77bb9de34dfab486a451347dd Mon Sep 17 00:00:00 2001
From: wu_xinjun <bent20140204@outlook.com>
Date: 星期日, 27 三月 2022 10:12:19 +0800
Subject: [PATCH] debug for udp_get server
---
src/slam/manualMode.py | 2
src/microwave/servers.py | 106 ++++++++++++++++++++++++++++++++++------------------
.vscode/launch.json | 4 +-
src/slam/autoMode.py | 2
4 files changed, 73 insertions(+), 41 deletions(-)
diff --git a/.vscode/launch.json b/.vscode/launch.json
index 62bbb88..9692457 100644
--- a/.vscode/launch.json
+++ b/.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
},
diff --git a/src/microwave/servers.py b/src/microwave/servers.py
index c24edef..8686b4a 100644
--- a/src/microwave/servers.py
+++ b/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])
-
- # 鍒ゆ柇绗竴涓暟鎹寘涓槸鍚﹀寘鍚獸ooter 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]):
+
+ # 鍒ゆ柇绗竴涓暟鎹寘涓槸鍚﹀寘鍚獸ooter 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))
diff --git a/src/slam/autoMode.py b/src/slam/autoMode.py
index 0d64093..7abd946 100644
--- a/src/slam/autoMode.py
+++ b/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)
diff --git a/src/slam/manualMode.py b/src/slam/manualMode.py
index 71372da..a23be39 100644
--- a/src/slam/manualMode.py
+++ b/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
--
Gitblit v1.9.1