排查影响时间复杂度的代码

lib/cfg/cfg.py

@@ -28,6 +28,7 @@ PORT = 65444
 
 # TODO: 帧率控制配置
 PERIOD = 1
+PERIOD = 0.05
 
 
 class VisionMode(enum.Enum):
@@ -40,3 +41,14 @@ class VisionMode(enum.Enum):
 class CameraControl(enum.Enum):
     DESTORY = 0
     CAPTURE = 1
+
+
+DURATION_DEBUG = True
+
+TRACK_DEBUG = True
+
+OBSTACLE_DEBUG = True
+
+FRONT_DEBUG = True
+
+REAR_DEBUG = True

lib/presenter/presenter.py

@@ -1,4 +1,5 @@
 from concurrent.futures import ThreadPoolExecutor, as_completed
+from os import stat_result
 from lib.alg.track_detection import TrackDetector
 from ctypes import set_errno
 from threading import Semaphore
@@ -28,6 +29,10 @@ from lib.cfg.cfg import (
     RAIL_KEYS,
     TITLE2D_KEY,
     PERIOD,
+    DURATION_DEBUG,
+    OBSTACLE_DEBUG,
+    FRONT_DEBUG,
+    REAR_DEBUG,
 )
 
 
@@ -120,12 +125,17 @@ class Presenter:
     # TODO: 避障模式相机采集和数据处理
     def obstacle_mode_data_handle(self):
         """获取所有3D避障相机的数据并处理，直到所有处理完成再继续"""
+        st = time.time()
         futures = []
         # 1. 发出CAPTURE命令
         for key in self.process3d_info.keys():
             if key.endswith("上轨"):
                 continue
             self.process3d_info[key].in_q.put(CameraControl.CAPTURE)
+
+        et = time.time()
+        dua = (et - st) * 1000
+        print(f"[DEBUG]: 采图用时{dua:.2f}ms")
         with ThreadPoolExecutor(max_workers=4) as executor:
             # 2. 等待图像并提交处理任务
             while not self.fifo_3d.empty():
@@ -141,6 +151,7 @@ class Presenter:
             if not result["ok"]:
                 self.last_d[MAPPING[result["title"]]] = result["distance"]
 
+        # 4. 将信息放置在历史队列当中
         for key in OBSTACLE_KEYS:
             hist = self.hist_ok[key]
             last_dist = self.last_d[key]
@@ -151,7 +162,7 @@ class Presenter:
 
             self.pkt[key + "_distance"] = dist_str
 
-            # 6. 总状态字段
+        # 5. 总状态字段
         self.pkt["f_obstacle_status"] = (
             "OK"
             if self.pkt["f_l_obstacle_distance"] == "000"
@@ -197,13 +208,13 @@ class Presenter:
     def run(self):
         # TODO: 初始化TCP服务和收收数据缓存
         server = TcpServer(host=HOST, port=PORT)
-        print("DEBUG:TCPServer init completed")
+        print("[DEBUG]:TCPServer init completed")
         tcp_rec_buf = ""
         tcp_send_buf = ""
         try:
             server.accept_client()
             while True:
-                start = time.time() 
+                start_time = time.time() 
                 # TODO: next_time 记录时钟控制帧率
                 next_time = time.perf_counter() + PERIOD
                 try:
@@ -212,8 +223,10 @@ class Presenter:
                     if tcp_rec_buf:
                         self.rec_tcp_data_handle(tcp_rec_buf)
                 except ConnectionResetError:
-                    print("Warring: clietn force disconnect!!! ")
-                    break
+                    print("[Warring]: clietn force disconnect!!! ")
+                    server.conn_close()
+                    print("[SERVER] Connection closed, waiting new client…")
+                    continue
                 except socket.error as e:
                     print(f"Net Error: {e}")
                     break
@@ -230,6 +243,11 @@ class Presenter:
                     self.rear_mode_data_handle()
                 elif self.mode == VisionMode.TRACK_INSPECTION:
                     pass
+                # TODO: DURATION DEBUG
+                end_time = time.time()
+                duration_ms = (end_time - start_time) * 1000
+                if DURATION_DEBUG:
+                    print(f"[DEBUG]: 总共用时{duration_ms:.2f}ms")
 
                 # TODO: tcp发送数据
                 try:
@@ -239,13 +257,7 @@ class Presenter:
                 except TypeError as e:
                     print(f"JSON encode failed: {e}")
                     tcp_send_buf = b"{}"
-                
-                print(f"DEBUG:{self.pkt}")
-
                 server.send_data(tcp_send_buf)
-                end = time.time()
-                duration_ms = (end - start) * 1000
-                print(f"[INFO] 每帧耗时: {duration_ms:.2f} ms")
 
                 # TODO: 控制帧率
                 now = time.perf_counter()
@@ -253,14 +265,18 @@ class Presenter:
                 if wait > 0:
                     time.sleep(wait)
                 next_time += PERIOD
+                
+                # TODO: DEBUG
+                print(f"[DEBUG]:{self.pkt}")
         except KeyboardInterrupt:
             print("KeyboardInterrupt (Ctrl+C) shutting down")
 
         finally:
+            server.close()
+            print("[SERVER]: Tcp Server close")
             for key in self.process3d_info.keys():
                 self.process3d_info[key].in_q.put(0)
             for key in self.process2d_info.keys():
                 self.process2d_info[key].in_q.put(0)
             ArenaCamera.shutdown()
-            print("关闭连接")
-            server.close()
+            print("[CAMERA]: ALl Destory")

lib/tcp/tcp_server.py

@@ -25,6 +25,8 @@ class TcpServer:
     def send_data(self, data: bytes):
         self.conn.sendall(data)
 
-    def close(self):
+    def conn_close(self):
         self.conn.close()
+
+    def close(self):
         self.sock.close()