212 lines
8.1 KiB
Python
212 lines
8.1 KiB
Python
from ctypes import set_errno
|
||
import datetime
|
||
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
|
||
import numpy as np
|
||
import socket
|
||
from lib.camera.ArenaCamera import ArenaCamera
|
||
from lib.tcp.tcp_server import TcpServer
|
||
import open3d as o3d
|
||
from lib.cfg.cfg import (
|
||
CAMERA_2D_CFFS,
|
||
CAMERA_3D_CFGS,
|
||
CameraControl,
|
||
VisionMode,
|
||
HOST,
|
||
PORT,
|
||
OBSTACLE_KEYS,
|
||
MAPPING,
|
||
RAIL_KEYS,
|
||
TITLE2D_KEY,
|
||
PERIOD,
|
||
)
|
||
|
||
|
||
# NOTE: Presentre类
|
||
# 1. 管理相机进程，通过往相机进程的输入队列中发送数据控制相机采集数据，没发送一次1，相机通过输出队列返回一个图片数据
|
||
# 2. 接收Tcp客户端发送过来的数据来切换模式，根据不同的模式采集不同的图像并解析成数据
|
||
# 3. 将解析好的数据通过TCP服务发送出去
|
||
class Presenter:
|
||
def __init__(self) -> None:
|
||
# TODO: 初始化进程队列
|
||
self.mode = VisionMode.OBSTACLE_RECO
|
||
self.process3d_info = {}
|
||
self.process2d_info = {}
|
||
mgr = Manager()
|
||
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)
|
||
self.process3d_info[cfg["title"]] = pro
|
||
pro.start()
|
||
|
||
with open(CAMERA_2D_CFFS, encoding="utf-8") as f:
|
||
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)
|
||
self.process2d_info[cfg["title"]] = pro
|
||
pro.start()
|
||
|
||
# NOTE: 障碍物状态历史队列
|
||
# 前左前右障碍物距离检测
|
||
# 轨道检测数据历史队列
|
||
# 轨道数据数值历史队列
|
||
# 2d检测数值队列
|
||
self.hist_ok = {k: deque(maxlen=10) for k in OBSTACLE_KEYS}
|
||
self.last_d = {k: None for k in OBSTACLE_KEYS}
|
||
self.hist_rail = {k: deque(maxlen=5) for k in RAIL_KEYS}
|
||
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):
|
||
pass
|
||
|
||
def handle_camera_3d_data(self):
|
||
pass
|
||
|
||
def handle_camera_2d_data(self):
|
||
pass
|
||
|
||
def front_mode_data_handle(self, pkt: OrderedDict):
|
||
pass
|
||
|
||
def rear_mode_data_handle(self, pkt: OrderedDict):
|
||
pass
|
||
|
||
def obstacle_mode_data_handle(self, pkt: OrderedDict):
|
||
"""1.获取所有3d避障相机的数据"""
|
||
obstacle_depth_img = {}
|
||
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()
|
||
|
||
"""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
|
||
)
|
||
# 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
|
||
|
||
def send_tcp_data(self):
|
||
pass
|
||
|
||
# TODO: 对tcp发回的数据进行按行处理，并且返回一个数组(不返回了，它接收数据只是为了切换模式)
|
||
def rec_tcp_data_handle(self, data):
|
||
if data:
|
||
data += data.decode("utf-8", errors="ignore")
|
||
while "\n" in data:
|
||
line, data = data.split("\n", 1)
|
||
if not line.strip():
|
||
continue
|
||
try:
|
||
cmd = json.loads(line)
|
||
print(f"[SERVER] Cmd: {cmd}")
|
||
front = cmd.get("FrontCouplerSignal", False)
|
||
rear = cmd.get("RearCouplerSignal", False)
|
||
obs = cmd.get("ObstacleDetection", False)
|
||
if obs:
|
||
self.mode = VisionMode.OBSTACLE_DETECTION
|
||
elif front:
|
||
self.mode = VisionMode.FRONT_2D_DETECTION
|
||
elif rear:
|
||
self.mode = VisionMode.REAR_2D_DETECTION
|
||
except json.JSONDecodeError:
|
||
pass
|
||
|
||
def run(self):
|
||
# TODO: 初始化TCP服务和收收数据缓存
|
||
server = TcpServer(host="0.0.0.0", port=65444)
|
||
tcp_rec_buf = ""
|
||
tcp_send_buf = ""
|
||
pkt = OrderedDict() # tcp发送数据包
|
||
try:
|
||
server.accept_client()
|
||
while True:
|
||
# TODO: next_time 记录时钟控制帧率
|
||
next_time = time.perf_counter() + PERIOD
|
||
try:
|
||
# TODO: 接收tcp接收的数据，根据数据并转换模式
|
||
tcp_rec_buf = server.recv_data()
|
||
if tcp_rec_buf:
|
||
self.rec_tcp_data_handle(tcp_rec_buf)
|
||
elif not tcp_rec_buf:
|
||
print("Client disconnected gracefully")
|
||
continue
|
||
except ConnectionResetError:
|
||
print("Warring: clietn force disconnect!!! ")
|
||
break
|
||
except socket.error as e:
|
||
print(f"Net Error: {e}")
|
||
break
|
||
|
||
# 清空发送包
|
||
pkt.clear()
|
||
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)
|
||
elif self.mode == VisionMode.FRONT_2D_DETECTION:
|
||
self.front_mode_data_handle(pkt)
|
||
elif self.mode == VisionMode.REAR_2D_DETECTION:
|
||
self.rear_mode_data_handle(pkt)
|
||
|
||
# TODO: tcp发送数据
|
||
try:
|
||
tcp_send_buf = (json.dumps(pkt, ensure_ascii=False) + "\n").encode()
|
||
except TypeError as e:
|
||
print(f"JSON encode failed: {e}")
|
||
tcp_send_buf = b"{}"
|
||
server.send_data(tcp_send_buf)
|
||
|
||
# TODO: 控制帧率
|
||
now = time.perf_counter()
|
||
wait = next_time - now
|
||
if wait > 0:
|
||
time.sleep(wait)
|
||
next_time += PERIOD
|
||
except KeyboardInterrupt:
|
||
print("KeyboardInterrupt（Ctrl+C） shutting down")
|
||
|
||
finally:
|
||
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()
|