zhaozhihao/hirres_tractor_vision
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

obstacle detection function complete

Browse Source
This commit is contained in:
CHAMPION923
2025-06-02 19:52:03 +08:00
parent de417c4522
commit 7ed2131c3a
3 changed files with 95 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
lib/io/process2d.py
View File

@@ -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):

6
lib/io/process3d.py
View File

@@ -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

142
lib/presenter/presenter.py
View File

@@ -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["sn"], in_q, out_q)
pro = Process3D(cfg, in_q, self.fifo_3d)
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["sn"], in_q, out_q)
pro = Process2D(cfg, in_q, self.fifo_2d)
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):
pass
def handle_obstacle_data(self, img_data):
"""通过算法处理图像数据，并返回分析结果"""
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):
pass
nearest, _ = detect_obstacles_in_box(pcd, box[0], box[1], 640, 480)
def obstacle_mode_data_handle(self, pkt: OrderedDict):
"""1.获取所有3d避障相机的数据"""
obstacle_depth_img = {}
if nearest:
d = float(np.linalg.norm(nearest["position"]))
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. 通过算法处理图像数据 这段算法应该跑在进程里 """
for key in obstacle_depth_img.keys():
intrinsic = o3d.camera.PinholeCameraIntrinsic(640, 480, 474, 505, 320, 240)
img = o3d.geometry.Image(obstacle_depth_img[key].astype(np.float32))
pcd = o3d.geometry.PointCloud.create_from_depth_image(
img, intrinsic, depth_scale=1000.0, depth_trunc=8.0
# 3. 等待所有算法任务完成（阻塞）
for future in as_completed(futures):
result = future.result()
# 更新历史状态（主线程写入更安全）
self.hist_ok[MAPPING[result["title"]]].append(result["ok"])
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
# 清空发送包
pkt.clear()
pkt["time_str"] = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
# TODO: 清空发送包
self.pkt.clear()
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():