obstacle detection function complete

lib/io/process2d.py

@@ -1,5 +1,6 @@
 from multiprocessing import Process
 from lib.camera.MindVisionCamera import MindVisionCamera
+from lib.cfg import cfg
 from lib.cfg.cfg import CameraControl
 
 
@@ -12,14 +13,13 @@ class Process2D(Process):
         self.out_q = out_q
         self.camera = MindVisionCamera(cfg["sn"])
         self.status = 0
-        # 仅对轨道摄像头启用 TrackDetector
 
     def run(self):
         while True:
             sig = self.in_q.get()
             if sig == CameraControl.CAPTURE:
                 img = self.camera.capture()
-                self.out_q.put(img)
+                self.out_q.put({"title": cfg["title"], "img": img})
             elif sig == CameraControl.DESTORY:
                 for fn in ("destroy", "close", "release"):
                     if hasattr(self.camera, fn):

lib/io/process3d.py

@@ -1,5 +1,6 @@
 from multiprocessing import Process
 from collections import deque
+from lib import cfg
 from lib.camera.ArenaCamera import ArenaCamera
 from lib.cfg.cfg import CameraControl
 
@@ -12,15 +13,14 @@ class Process3D(Process):
         self.in_q = in_q
         self.out_q = out_q
         self.camera = ArenaCamera(cfg["sn"])
-        self.status = 0
-        # 仅对轨道摄像头启用 TrackDetector
 
     def run(self):
         while True:
             sig = self.in_q.get()
             if sig == CameraControl.CAPTURE:
                 _, _, dep_img = self.camera.capture()
-                self.out_q.put(dep_img)
+                # self.out_q.put({self.cfg["title"]: dep_img})
+                self.out_q.put({"title": cfg["title"], "dep_img": dep_img})
             elif sig == CameraControl.DESTORY:
                 self.camera.destroy()
                 break

lib/presenter/presenter.py

@@ -1,11 +1,12 @@
-from ctypes import set_errno
+from ctypes import set_er
+from concurrent.futures import ThreadPoolExecutor, as_completed
+from threading import Semaphore
 import datetime
+from typing import Deque
 from lib.alg.image_processing_3d import detect_obstacles_in_box
 import time
 from multiprocessing import Manager
 import json
-from lib.io import process2d
-from lib.io import process3d
 from lib.io.process3d import Process3D
 from lib.io.process2d import Process2D
 from collections import deque, OrderedDict
@@ -35,17 +36,21 @@ from lib.cfg.cfg import (
 # 3. 将解析好的数据通过TCP服务发送出去
 class Presenter:
     def __init__(self) -> None:
+        mgr = Manager()
+        # 存放2D 3D相机采集图像的数据队列
+        self.fifo_2d = mgr.Queue()
+        self.fifo_3d = mgr.Queue()
         # TODO: 初始化进程队列
         self.mode = VisionMode.OBSTACLE_RECO
+        self.pkt = OrderedDict()  # tcp发送数据包
         self.process3d_info = {}
         self.process2d_info = {}
-        mgr = Manager()
+        self.depth_img = {}
         with open(CAMERA_3D_CFGS, encoding="utf-8") as f:
             cfg3d = json.load(f)["camera"]
         for cfg in cfg3d:
             in_q = mgr.Queue()
-            out_q = mgr.Queue()
+            pro = Process3D(cfg, in_q, self.fifo_3d)
-            pro = Process3D(cfg["sn"], in_q, out_q)
             self.process3d_info[cfg["title"]] = pro
             pro.start()
 
@@ -53,8 +58,7 @@ class Presenter:
             cfg2d = json.load(f)["camera"]
         for cfg in cfg2d:
             in_q = mgr.Queue()
-            out_q = mgr.Queue()
+            pro = Process2D(cfg, in_q, self.fifo_2d)
-            pro = Process2D(cfg["sn"], in_q, out_q)
             self.process2d_info[cfg["title"]] = pro
             pro.start()
 
@@ -69,54 +73,87 @@ class Presenter:
         self.last_rail = {k: {"offset": None, "angle": None} for k in RAIL_KEYS}
         self.two_d_hist = {k: deque(maxlen=10) for k in TITLE2D_KEY.values()}
 
-    def get_camera_data(self):
+    def front_mode_data_handle(self):
         pass
 
-    def handle_camera_3d_data(self):
+    def rear_mode_data_handle(self):
         pass
 
-    def handle_camera_2d_data(self):
+    def handle_obstacle_data(self, img_data):
-        pass
+        """通过算法处理图像数据，并返回分析结果"""
+        intrinsic = o3d.camera.PinholeCameraIntrinsic(640, 480, 474, 505, 320, 240)
+        img = o3d.geometry.Image(img_data["dep_img"].astype(np.float32))
+        pcd = o3d.geometry.PointCloud.create_from_depth_image(
+            img, intrinsic, depth_scale=1000.0, depth_trunc=8.0
+        )
 
-    def front_mode_data_handle(self, pkt: OrderedDict):
+        # 设置检测区域
-        pass
+        if img_data["title"].startswith("前"):
+            box = (np.array([-1050, -600, 500]), np.array([1050, 1000, 6000]))
+        else:
+            box = (np.array([-800, -600, 800]), np.array([800, 1100, 6000]))
 
-    def rear_mode_data_handle(self, pkt: OrderedDict):
+        nearest, _ = detect_obstacles_in_box(pcd, box[0], box[1], 640, 480)
-        pass
 
-    def obstacle_mode_data_handle(self, pkt: OrderedDict):
+        if nearest:
-        """1.获取所有3d避障相机的数据"""
+            d = float(np.linalg.norm(nearest["position"]))
-        obstacle_depth_img = {}
+            res = {"distance": round(d, 2), "status": "NG"}
+        else:
+            res = {"distance": None, "status": "OK"}
+
+        return {
+            "ok": res["status"] == "OK",
+            "distance": res["distance"],
+            "title": img_data["title"],
+        }
+
+    # TODO: 避障模式相机采集和数据处理
+    def obstacle_mode_data_handle(self):
+        """获取所有3D避障相机的数据并处理，直到所有处理完成再继续"""
+        futures = []
+        # 1. 发出CAPTURE命令
         for key in self.process3d_info.keys():
-            """ 过滤掉所有上轨的相机 """
             if key.endswith("上轨"):
                 continue
             self.process3d_info[key].in_q.put(CameraControl.CAPTURE)
-            obstacle_depth_img[key] = self.process3d_info[key].out_q().get()
+        with ThreadPoolExecutor(max_workers=4) as executor:
+            # 2. 等待图像并提交处理任务
+            while not self.fifo_3d.empty():
+                img_data = self.fifo_3d.get()
+                future = executor.submit(self.handle_obstacle_data, img_data)
+                futures.append(future)
 
-        """2. 通过算法处理图像数据 这段算法应该跑在进程里 """
+            # 3. 等待所有算法任务完成（阻塞）
-        for key in obstacle_depth_img.keys():
+            for future in as_completed(futures):
-            intrinsic = o3d.camera.PinholeCameraIntrinsic(640, 480, 474, 505, 320, 240)
+                result = future.result()
-            img = o3d.geometry.Image(obstacle_depth_img[key].astype(np.float32))
+                # 更新历史状态（主线程写入更安全）
-            pcd = o3d.geometry.PointCloud.create_from_depth_image(
+                self.hist_ok[MAPPING[result["title"]]].append(result["ok"])
-                img, intrinsic, depth_scale=1000.0, depth_trunc=8.0
+                if not result["ok"]:
+                    self.last_d[MAPPING[result["title"]]] = result["distance"]
+
+            for key in OBSTACLE_KEYS:
+                hist = self.hist_ok[key]
+                last_dist = self.last_d[key]
+                if len(hist) == 10 and all(hist):
+                    dist_str = "000"
+                else:
+                    dist_str = "000" if last_dist is None else f"{last_dist:.2f}"
+
+                self.pkt[key] = dist_str
+
+            # 6. 总状态字段
+            self.pkt["f_obstacle_status"] = (
+                "OK"
+                if self.pkt["f_l_obstacle_distance"] == "000"
+                and self.pkt["f_r_obstacle_distance"] == "000"
+                else "NG"
+            )
+            self.pkt["b_obstacle_status"] = (
+                "OK"
+                if self.pkt["b_l_obstacle_distance"] == "000"
+                and self.pkt["b_r_obstacle_distance"] == "000"
+                else "NG"
             )
-            # TODO: 绘制矩形区域，检测区域内障碍物
-            if key.startswith("前"):
-                box = (np.array([-1050, -600, 500]), np.array([1050, 1000, 6000]))
-            else:
-                box = (np.array([-800, -600, 800]), np.array([800, 1100, 6000]))
-                nearest, _ = detect_obstacles_in_box(pcd, box[0], box[1], 640, 480)
-            if nearest:
-                d = float(np.linalg.norm(nearest["position"]))
-                res = {"distance": round(d, 2), "status": "NG"}
-            else:
-                res = {"distance": None, "status": "OK"}
-            """ 往障碍识别状态里添加历史记录, 只保存前10帧, 以及保存上一次的距离值 """
-            ok = res["status"] == "OK"
-            self.hist_ok[MAPPING[key]].append(ok)
-            if not ok:
-                self.last_d[MAPPING[key]] = res["distance"]
 
     def wait_rec_tcp_data(self):
         pass
@@ -152,7 +189,6 @@ class Presenter:
         server = TcpServer(host=HOST, port=PORT)
         tcp_rec_buf = ""
         tcp_send_buf = ""
-        pkt = OrderedDict()  # tcp发送数据包
         try:
             server.accept_client()
             while True:
@@ -173,20 +209,22 @@ class Presenter:
                     print(f"Net Error: {e}")
                     break
 
-                # 清空发送包
+                # TODO: 清空发送包
-                pkt.clear()
+                self.pkt.clear()
-                pkt["time_str"] = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
+                self.pkt["time_str"] = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
                 # TODO: 根据模式发送不同的数据
                 if self.mode == VisionMode.OBSTACLE_DETECTION:
-                    self.obstacle_mode_data_handle(pkt)
+                    self.obstacle_mode_data_handle()
                 elif self.mode == VisionMode.FRONT_2D_DETECTION:
-                    self.front_mode_data_handle(pkt)
+                    self.front_mode_data_handle()
                 elif self.mode == VisionMode.REAR_2D_DETECTION:
-                    self.rear_mode_data_handle(pkt)
+                    self.rear_mode_data_handle()
 
                 # TODO: tcp发送数据
                 try:
-                    tcp_send_buf = (json.dumps(pkt, ensure_ascii=False) + "\n").encode()
+                    tcp_send_buf = (
+                        json.dumps(self.pkt, ensure_ascii=False) + "\n"
+                    ).encode()
                 except TypeError as e:
                     print(f"JSON encode failed: {e}")
                     tcp_send_buf = b"{}"
@@ -199,7 +237,7 @@ class Presenter:
                     time.sleep(wait)
                 next_time += PERIOD
         except KeyboardInterrupt:
-            print("KeyboardInterrupt（Ctrl+C） shutting down")
+            print("KeyboardInterrupt (Ctrl+C) shutting down")
 
         finally:
             for key in self.process3d_info.keys():