TractorVision初期移植
This commit is contained in:
CHAMPION923
211
lib/presenter/presenter.py
Normal file
211
lib/presenter/presenter.py
Normal file
@@ -0,0 +1,211 @@
|
||||
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()
|
||||
Reference in New Issue
Block a user