添加DEBUG功能

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,6 +125,7 @@ class Presenter:
     # TODO: 避障模式相机采集和数据处理
     def obstacle_mode_data_handle(self):
         """获取所有3D避障相机的数据并处理，直到所有处理完成再继续"""
+        st = time.time()
         futures = []
         # 1. 发出CAPTURE命令
         for key in self.process3d_info.keys():
@@ -141,6 +147,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 +158,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"
@@ -165,6 +172,10 @@ class Presenter:
                 else "NG"
             )
 
+            et = time.time()
+            dua = (st - et) * 1000
+            print(f"[DEBUG]: 采图用时{dua}")
+
     def wait_rec_tcp_data(self):
         pass
 
@@ -203,6 +214,7 @@ class Presenter:
         try:
             server.accept_client()
             while True:
+                start_time = time.time()  # 计算帧率用时
                 # TODO: next_time 记录时钟控制帧率
                 next_time = time.perf_counter() + PERIOD
                 try:
@@ -243,6 +255,12 @@ class Presenter:
                     tcp_send_buf = b"{}"
                 server.send_data(tcp_send_buf)
 
+                # TODO: DEBUG
+                end_time = time.time()
+                duration_ms = (end_time - start_time) * 1000
+                if DURATION_DEBUG:
+                    print(f"[DEBUG]: 用时{duration_ms}")
+
                 # TODO: 控制帧率
                 now = time.perf_counter()
                 wait = next_time - now