996 lines
35 KiB
Python
996 lines
35 KiB
Python
import cv2
|
||
import threading
|
||
import sys, os, time
|
||
import argparse
|
||
import numpy as np
|
||
import serial
|
||
from collections import deque
|
||
from PyQt5.QtWidgets import (QApplication, QLabel, QWidget, QVBoxLayout,
|
||
QHBoxLayout, QMainWindow, QStackedWidget,
|
||
QSlider, QPushButton, QGridLayout, QFrame)
|
||
from PyQt5.QtGui import QImage, QPixmap, QFont
|
||
from PyQt5.QtCore import Qt, QTimer, pyqtSignal, QObject
|
||
from PIL import Image, ImageDraw, ImageFont
|
||
|
||
try:
|
||
from surround_view import FisheyeCameraModel, BirdView
|
||
import surround_view.param_settings as settings
|
||
except ImportError as e:
|
||
print("[ERROR] Cannot import surround_view modules: {}".format(e))
|
||
sys.exit(1)
|
||
|
||
|
||
# ==========================================================
|
||
# 1. 字体管理
|
||
# ==========================================================
|
||
class FontManager:
|
||
_fonts = {}
|
||
_lock = threading.Lock()
|
||
|
||
@classmethod
|
||
def get(cls, size=20):
|
||
with cls._lock:
|
||
if size not in cls._fonts:
|
||
candidates = [
|
||
"/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc",
|
||
"/usr/share/fonts/truetype/wqy/wqy-microhei.ttc",
|
||
"/usr/share/fonts/truetype/droid/DroidSansFallbackFull.ttf",
|
||
"/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc",
|
||
"/usr/share/fonts/truetype/noto/NotoSansCJK-Regular.ttc",
|
||
]
|
||
loaded = False
|
||
for p in candidates:
|
||
if os.path.exists(p):
|
||
try:
|
||
cls._fonts[size] = ImageFont.truetype(p, size)
|
||
loaded = True
|
||
break
|
||
except Exception:
|
||
continue
|
||
if not loaded:
|
||
cls._fonts[size] = ImageFont.load_default()
|
||
return cls._fonts[size]
|
||
|
||
|
||
# ==========================================================
|
||
# 2. 中文叠加
|
||
# ==========================================================
|
||
def add_texts_to_pil(pil_image, text_list):
|
||
draw = ImageDraw.Draw(pil_image)
|
||
for item in text_list:
|
||
if len(item) == 5:
|
||
x, y, text, color, size = item
|
||
font = FontManager.get(size)
|
||
else:
|
||
x, y, text, color = item
|
||
font = FontManager.get(20)
|
||
draw.text((x + 1, y + 1), text, font=font, fill=(0, 0, 0))
|
||
draw.text((x, y), text, font=font, fill=color)
|
||
|
||
|
||
# ==========================================================
|
||
# 3. 串口1：收发线程
|
||
# 发送帧格式：55 [前] [左] [后] [右] [00] [雷达阈值dm] [校验] AA
|
||
# 校验 = (前+左+后+右+00+雷达阈值) & 0xFF
|
||
# ==========================================================
|
||
SEND_INTERVAL_MS = 100
|
||
|
||
class SerialWorker(threading.Thread):
|
||
def __init__(self, port="/dev/ttyS0", baudrate=115200):
|
||
super().__init__(daemon=True)
|
||
self.port = port
|
||
self.baudrate = baudrate
|
||
self.running = True
|
||
|
||
# 接收状态
|
||
self._rx_lock = threading.Lock()
|
||
self._radar_dist = 0
|
||
self._stopped = False
|
||
self._active_dirs = []
|
||
|
||
# 接收防抖
|
||
self._rx_pending_dirs = []
|
||
self._rx_pending_radar = 0
|
||
self._rx_pending_stop = False
|
||
self._rx_pending_count = 0
|
||
RX_DEBOUNCE = 3
|
||
|
||
# 发送内容
|
||
self._tx_lock = threading.Lock()
|
||
self._tx_front = 0x00
|
||
self._tx_left = 0x00
|
||
self._tx_back = 0x00
|
||
self._tx_right = 0x00
|
||
self._tx_radar_thr = 0x00
|
||
|
||
# 发送防抖
|
||
self._stable_presence = [0x00, 0x00, 0x00, 0x00]
|
||
self._pending_presence = [0x00, 0x00, 0x00, 0x00]
|
||
self._pending_count = 0
|
||
self.DEBOUNCE_FRAMES = 9
|
||
|
||
# 打印计数（防刷屏）
|
||
self._tx_print_cnt = 0
|
||
|
||
def update_send(self, front, left, back, right, radar_thr):
|
||
"""外部调用，更新待发送的有人/无人状态及雷达阈值"""
|
||
presence = [front & 0xFF, left & 0xFF,
|
||
back & 0xFF, right & 0xFF]
|
||
|
||
# 发送防抖：连续 DEBOUNCE_FRAMES 次一致才更新稳定值
|
||
if presence == self._pending_presence:
|
||
self._pending_count += 1
|
||
else:
|
||
self._pending_presence = presence[:]
|
||
self._pending_count = 1
|
||
|
||
if self._pending_count >= self.DEBOUNCE_FRAMES:
|
||
self._stable_presence = presence[:]
|
||
|
||
with self._tx_lock:
|
||
self._tx_front = self._stable_presence[0]
|
||
self._tx_left = self._stable_presence[1]
|
||
self._tx_back = self._stable_presence[2]
|
||
self._tx_right = self._stable_presence[3]
|
||
self._tx_radar_thr = radar_thr & 0xFF
|
||
|
||
def _build_tx_frame(self):
|
||
"""按协议构建9字节发送帧"""
|
||
with self._tx_lock:
|
||
b1 = self._tx_front # 前
|
||
b2 = self._tx_left # 左
|
||
b3 = self._tx_back # 后
|
||
b4 = self._tx_right # 右
|
||
b5 = 0x00 # 固定00
|
||
b6 = self._tx_radar_thr # 雷达阈值dm
|
||
chk = (b1 + b2 + b3 + b4 + b5 + b6) & 0xFF
|
||
return bytes([0x55, b1, b2, b3, b4, b5, b6, chk, 0xAA])
|
||
|
||
def get_state(self):
|
||
with self._rx_lock:
|
||
return list(self._active_dirs), self._radar_dist, self._stopped
|
||
|
||
def _parse_rx_frame(self, frame: bytes):
|
||
if len(frame) != 9 or frame[0] != 0x55 or frame[8] != 0xAA:
|
||
return
|
||
|
||
# 打印接收原始帧
|
||
print("[RX ttyS0] {}".format(
|
||
" ".join("{:02X}".format(b) for b in frame)))
|
||
|
||
front = frame[1]; left = frame[2]
|
||
back = frame[3]; right = frame[4]
|
||
radar = frame[5]; stop = frame[6]
|
||
|
||
active = []
|
||
for name, val in [("front", front), ("left", left),
|
||
("back", back), ("right", right)]:
|
||
if val != 0x00:
|
||
active.append(name)
|
||
|
||
# 接收防抖
|
||
if (active == self._rx_pending_dirs and
|
||
radar == self._rx_pending_radar and
|
||
(stop != 0x00) == self._rx_pending_stop):
|
||
self._rx_pending_count += 1
|
||
else:
|
||
self._rx_pending_dirs = active
|
||
self._rx_pending_radar = radar
|
||
self._rx_pending_stop = (stop != 0x00)
|
||
self._rx_pending_count = 1
|
||
|
||
if self._rx_pending_count >= 3:
|
||
with self._rx_lock:
|
||
self._active_dirs = active
|
||
self._radar_dist = radar
|
||
self._stopped = (stop != 0x00)
|
||
|
||
def run(self):
|
||
try:
|
||
ser = serial.Serial(
|
||
port=self.port, baudrate=self.baudrate,
|
||
bytesize=serial.EIGHTBITS,
|
||
stopbits=serial.STOPBITS_ONE,
|
||
parity=serial.PARITY_NONE,
|
||
timeout=0.05)
|
||
except Exception as e:
|
||
print("[串口1 ERROR] 无法打开: {}".format(e))
|
||
return
|
||
|
||
# ── 发送子线程 ──
|
||
def _send_loop():
|
||
interval = SEND_INTERVAL_MS / 1000.0
|
||
while self.running:
|
||
try:
|
||
frame = self._build_tx_frame()
|
||
ser.write(frame)
|
||
|
||
# 每10次打印一次发送内容（约1秒）
|
||
self._tx_print_cnt += 1
|
||
if self._tx_print_cnt % 10 == 0:
|
||
print("[TX ttyS0] {} 含义: 前={:02X} 左={:02X} 后={:02X} 右={:02X} 雷达阈值={:02X}dm 校验={:02X}".format(
|
||
" ".join("{:02X}".format(b) for b in frame),
|
||
frame[1], frame[2], frame[3], frame[4],
|
||
frame[6], frame[7]))
|
||
|
||
except Exception as e:
|
||
if self.running:
|
||
print("[串口1 TX ERROR] {}".format(e))
|
||
time.sleep(interval)
|
||
|
||
tx_thread = threading.Thread(target=_send_loop, daemon=True)
|
||
tx_thread.start()
|
||
|
||
# ── 接收主循环 ──
|
||
buf = bytearray()
|
||
while self.running:
|
||
try:
|
||
byte = ser.read(1)
|
||
if not byte:
|
||
continue
|
||
b = byte[0]
|
||
if len(buf) == 0:
|
||
if b == 0x55:
|
||
buf.append(b)
|
||
continue
|
||
buf.append(b)
|
||
if len(buf) == 9:
|
||
self._parse_rx_frame(bytes(buf))
|
||
buf.clear()
|
||
elif len(buf) > 9:
|
||
buf.clear()
|
||
except Exception as e:
|
||
if self.running:
|
||
print("[串口1 RX ERROR] {}".format(e))
|
||
time.sleep(0.1)
|
||
ser.close()
|
||
|
||
def stop(self):
|
||
self.running = False
|
||
|
||
|
||
# ==========================================================
|
||
# 4. 串口2：红外遥控解析线程
|
||
# ==========================================================
|
||
REMOTE_HEADER = bytes([0x01, 0x52, 0xF0, 0xFF, 0x00, 0x00, 0xFF])
|
||
REMOTE_FRAME_LEN = 11
|
||
|
||
|
||
class RemoteSignals(QObject):
|
||
enter_settings = pyqtSignal()
|
||
exit_settings = pyqtSignal()
|
||
|
||
|
||
class RemoteWorker(threading.Thread):
|
||
def __init__(self, port="/dev/ttyS3", baudrate=115200):
|
||
super().__init__(daemon=True)
|
||
self.port = port
|
||
self.baudrate = baudrate
|
||
self.running = True
|
||
self.signals = RemoteSignals()
|
||
|
||
def _parse_frame(self, frame: bytes):
|
||
if len(frame) != REMOTE_FRAME_LEN:
|
||
return
|
||
if frame[:7] != REMOTE_HEADER:
|
||
return
|
||
user_code = frame[7]
|
||
print("[遥控] 用户码: 0x{:02X}".format(user_code))
|
||
if user_code == 0x05:
|
||
self.signals.enter_settings.emit()
|
||
elif user_code == 0x0A:
|
||
self.signals.exit_settings.emit()
|
||
|
||
def run(self):
|
||
try:
|
||
ser = serial.Serial(port=self.port, baudrate=self.baudrate,
|
||
bytesize=serial.EIGHTBITS,
|
||
stopbits=serial.STOPBITS_ONE,
|
||
parity=serial.PARITY_NONE, timeout=1)
|
||
except Exception as e:
|
||
print("[串口2 ERROR] 无法打开 {}: {}".format(self.port, e))
|
||
return
|
||
|
||
buf = bytearray()
|
||
while self.running:
|
||
try:
|
||
byte = ser.read(1)
|
||
if not byte:
|
||
continue
|
||
b = byte[0]
|
||
if len(buf) == 0:
|
||
if b == 0x01:
|
||
buf.append(b)
|
||
continue
|
||
buf.append(b)
|
||
if len(buf) <= 7:
|
||
if buf[len(buf) - 1] != REMOTE_HEADER[len(buf) - 1]:
|
||
buf.clear()
|
||
continue
|
||
if len(buf) == REMOTE_FRAME_LEN:
|
||
self._parse_frame(bytes(buf))
|
||
buf.clear()
|
||
elif len(buf) > REMOTE_FRAME_LEN:
|
||
buf.clear()
|
||
except Exception as e:
|
||
if self.running:
|
||
print("[串口2 ERROR] {}".format(e))
|
||
time.sleep(0.1)
|
||
ser.close()
|
||
|
||
def stop(self):
|
||
self.running = False
|
||
|
||
|
||
# ==========================================================
|
||
# 5. 轮播调度器
|
||
# ==========================================================
|
||
class CarouselScheduler:
|
||
INTERVAL = 4.0
|
||
|
||
def __init__(self):
|
||
self._queue = []
|
||
self._cur_dir = None
|
||
self._cur_start = 0.0
|
||
|
||
def update(self, active_dirs: list):
|
||
if not active_dirs:
|
||
self._queue = []
|
||
self._cur_dir = None
|
||
return None, 0
|
||
|
||
for d in active_dirs:
|
||
if d not in self._queue:
|
||
self._queue.append(d)
|
||
|
||
self._queue = [d for d in self._queue
|
||
if d in active_dirs or d == self._cur_dir]
|
||
|
||
if self._cur_dir is None or self._cur_dir not in self._queue:
|
||
self._cur_dir = self._queue[0] if self._queue else None
|
||
self._cur_start = time.time()
|
||
self._skip_gone(active_dirs)
|
||
|
||
if time.time() - self._cur_start >= self.INTERVAL:
|
||
self._advance(active_dirs)
|
||
|
||
remaining = max(0.0, self.INTERVAL - (time.time() - self._cur_start))
|
||
return self._cur_dir, remaining
|
||
|
||
def _advance(self, active_dirs):
|
||
if not self._queue:
|
||
self._cur_dir = None
|
||
return
|
||
try:
|
||
idx = self._queue.index(self._cur_dir)
|
||
except ValueError:
|
||
idx = -1
|
||
self._cur_dir = self._queue[(idx + 1) % len(self._queue)]
|
||
self._cur_start = time.time()
|
||
self._skip_gone(active_dirs)
|
||
|
||
def _skip_gone(self, active_dirs):
|
||
for _ in range(len(self._queue)):
|
||
if self._cur_dir in active_dirs:
|
||
break
|
||
self._advance(active_dirs)
|
||
|
||
|
||
# ==========================================================
|
||
# 6. 原始帧采集线程
|
||
# ==========================================================
|
||
class RawCaptureWorker(threading.Thread):
|
||
def __init__(self, caps, names):
|
||
super().__init__(daemon=True)
|
||
self.caps = caps
|
||
self.names = names
|
||
self.running = True
|
||
self.queues = {name: deque(maxlen=2) for name in names}
|
||
|
||
def get_frame(self, name):
|
||
q = self.queues.get(name)
|
||
if q:
|
||
try:
|
||
return q[-1]
|
||
except IndexError:
|
||
pass
|
||
return None
|
||
|
||
def run(self):
|
||
while self.running:
|
||
for cap, name in zip(self.caps, self.names):
|
||
ret, frame = cap.read()
|
||
if ret and frame is not None:
|
||
self.queues[name].append(frame)
|
||
|
||
def stop(self):
|
||
self.running = False
|
||
|
||
|
||
# ==========================================================
|
||
# 7. 鸟瞰处理线程
|
||
# ==========================================================
|
||
class BirdViewWorker(threading.Thread):
|
||
def __init__(self, raw_worker, camera_models, birdview, framestep=2):
|
||
super().__init__(daemon=True)
|
||
self.raw_worker = raw_worker
|
||
self.models = camera_models
|
||
self.names = raw_worker.names
|
||
self.birdview = birdview
|
||
self.framestep = framestep
|
||
self.running = True
|
||
self.frame_idx = 0
|
||
self._result_queue = deque(maxlen=2)
|
||
self._lock = threading.Lock()
|
||
|
||
def get_bird(self):
|
||
with self._lock:
|
||
try:
|
||
return self._result_queue[-1]
|
||
except IndexError:
|
||
return None
|
||
|
||
def run(self):
|
||
while self.running:
|
||
self.frame_idx += 1
|
||
if self.frame_idx % self.framestep != 0:
|
||
time.sleep(0.005)
|
||
continue
|
||
|
||
frames = []
|
||
for name, model in zip(self.names, self.models):
|
||
frame = self.raw_worker.get_frame(name)
|
||
if frame is None:
|
||
break
|
||
try:
|
||
proc = model.flip(model.project(model.undistort(frame)))
|
||
frames.append(proc)
|
||
except Exception as e:
|
||
print("[BIRD PROC ERROR] {}".format(e))
|
||
break
|
||
|
||
if len(frames) != 4:
|
||
time.sleep(0.01)
|
||
continue
|
||
|
||
try:
|
||
self.birdview.update_frames(frames)
|
||
self.birdview.stitch_all_parts()
|
||
self.birdview.make_white_balance()
|
||
self.birdview.copy_car_image()
|
||
bird = self.birdview.image.copy()
|
||
with self._lock:
|
||
self._result_queue.append(bird)
|
||
except Exception as e:
|
||
print("[BIRDVIEW ERROR] {}".format(e))
|
||
time.sleep(0.01)
|
||
|
||
def stop(self):
|
||
self.running = False
|
||
|
||
|
||
# ==========================================================
|
||
# 8. 多摄像头初始化
|
||
# ==========================================================
|
||
class MultiCameraBirdView:
|
||
def __init__(self):
|
||
self.running = True
|
||
self.names = settings.camera_names
|
||
self.yamls = [
|
||
os.path.join(os.getcwd(), "yaml", name + ".yaml")
|
||
for name in self.names
|
||
]
|
||
try:
|
||
self.camera_models = [
|
||
FisheyeCameraModel(f, n)
|
||
for f, n in zip(self.yamls, self.names)
|
||
]
|
||
except Exception as e:
|
||
print("[ERROR] {}".format(e))
|
||
self.running = False
|
||
return
|
||
|
||
self.caps = []
|
||
self.which_cameras = {"front": 0, "left": 1, "back": 2, "right": 3}
|
||
|
||
for name in self.names:
|
||
cap_id = self.which_cameras.get(name, 0)
|
||
cap = cv2.VideoCapture(cap_id, cv2.CAP_V4L2)
|
||
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV"))
|
||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
|
||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
|
||
if not cap.isOpened():
|
||
print("[ERROR] Cannot open {} (ID:{})".format(name, cap_id))
|
||
self.running = False
|
||
return
|
||
self.caps.append(cap)
|
||
|
||
self.birdview = BirdView()
|
||
self._initialize_weights()
|
||
|
||
def _initialize_weights(self):
|
||
try:
|
||
images = [
|
||
os.path.join(os.getcwd(), "images", name + ".png")
|
||
for name in self.names
|
||
]
|
||
static_frames = []
|
||
for img_path, model in zip(images, self.camera_models):
|
||
img = cv2.imread(img_path)
|
||
if img is None:
|
||
continue
|
||
static_frames.append(
|
||
model.flip(model.project(model.undistort(img))))
|
||
if len(static_frames) == 4:
|
||
self.birdview.get_weights_and_masks(static_frames)
|
||
print("[INFO] Weights initialized")
|
||
else:
|
||
print("[WARNING] Static images incomplete")
|
||
except Exception as e:
|
||
print("[ERROR] Weight init failed: {}".format(e))
|
||
|
||
|
||
# ==========================================================
|
||
# 9. 常量
|
||
# ==========================================================
|
||
DIR_CN = {"front": "前方", "left": "左方", "back": "后方", "right": "右方"}
|
||
WARN_COLOR = (255, 80, 80)
|
||
SAFE_COLOR = (100, 255, 100)
|
||
SCREEN_W = 1024
|
||
SCREEN_H = 600
|
||
BIRD_W = SCREEN_W // 3
|
||
FRONT_W = SCREEN_W - BIRD_W
|
||
|
||
|
||
# ==========================================================
|
||
# 10. 渲染线程（主视图）
|
||
# ==========================================================
|
||
class RenderWorker(threading.Thread):
|
||
TARGET_FPS = 30
|
||
FRAME_DELAY = 1.0 / TARGET_FPS
|
||
|
||
def __init__(self, bird_worker, raw_worker, serial_worker, carousel,
|
||
radar_thr_ref):
|
||
super().__init__(daemon=True)
|
||
self.bird_worker = bird_worker
|
||
self.raw_worker = raw_worker
|
||
self.serial_worker = serial_worker
|
||
self.carousel = carousel
|
||
self.radar_thr_ref = radar_thr_ref # list[int]，索引0为雷达阈值dm
|
||
self.running = True
|
||
self._out_queue = deque(maxlen=2)
|
||
self._lock = threading.Lock()
|
||
self._last_bird = None
|
||
for sz in (18, 20, 24, 32):
|
||
FontManager.get(sz)
|
||
|
||
def get_qimage(self):
|
||
with self._lock:
|
||
try:
|
||
return self._out_queue[-1]
|
||
except IndexError:
|
||
return None
|
||
|
||
def run(self):
|
||
while self.running:
|
||
t0 = time.time()
|
||
bird = self.bird_worker.get_bird()
|
||
if bird is not None:
|
||
self._last_bird = bird
|
||
|
||
if self._last_bird is None:
|
||
time.sleep(0.02)
|
||
continue
|
||
|
||
try:
|
||
active_dirs, radar_dist, is_stopped = \
|
||
self.serial_worker.get_state()
|
||
display_dir, remaining = self.carousel.update(active_dirs)
|
||
|
||
right_frame = None
|
||
if display_dir:
|
||
right_frame = self.raw_worker.get_frame(display_dir)
|
||
if right_frame is None:
|
||
right_frame = self.raw_worker.get_frame("front")
|
||
if right_frame is None:
|
||
time.sleep(0.01)
|
||
continue
|
||
|
||
bird_part = cv2.resize(self._last_bird, (BIRD_W, SCREEN_H),
|
||
interpolation=cv2.INTER_LINEAR)
|
||
front_part = cv2.resize(right_frame, (FRONT_W, SCREEN_H),
|
||
interpolation=cv2.INTER_LINEAR)
|
||
display = np.hstack((bird_part, front_part))
|
||
rgb = cv2.cvtColor(display, cv2.COLOR_BGR2RGB)
|
||
pil_img = Image.fromarray(rgb)
|
||
|
||
texts = [(10, 10, "鸟瞰视图", (0, 255, 100), 20)]
|
||
|
||
if display_dir:
|
||
dir_cn = DIR_CN.get(display_dir, display_dir)
|
||
texts.append((BIRD_W + 10, 10,
|
||
"{} 检测到人员".format(dir_cn),
|
||
WARN_COLOR, 32))
|
||
if len(active_dirs) > 1:
|
||
queue_cn = " → ".join(DIR_CN.get(d, d)
|
||
for d in self.carousel._queue)
|
||
texts.append((BIRD_W + 10, 55,
|
||
"轮播顺序: {}".format(queue_cn),
|
||
(255, 200, 0), 18))
|
||
texts.append((BIRD_W + 10, 82,
|
||
"当前: {} 切换倒计时: {:.1f}s".format(
|
||
dir_cn, remaining),
|
||
(200, 200, 200), 18))
|
||
else:
|
||
texts.append((BIRD_W + 10, 55,
|
||
"仅 {} 有人".format(dir_cn),
|
||
(200, 200, 200), 18))
|
||
else:
|
||
texts.append((BIRD_W + 10, 10,
|
||
"前视监控 (周围无人)", SAFE_COLOR, 24))
|
||
|
||
texts.append((BIRD_W + 10, SCREEN_H - 75,
|
||
"雷达距离: {} 分米".format(radar_dist),
|
||
(180, 220, 255), 20))
|
||
stop_text = "停机状态: 已停机" if is_stopped else "停机状态: 运行中"
|
||
stop_color = (255, 100, 100) if is_stopped else (100, 255, 100)
|
||
texts.append((BIRD_W + 10, SCREEN_H - 48,
|
||
stop_text, stop_color, 20))
|
||
texts.append((BIRD_W + 10, SCREEN_H - 22,
|
||
time.strftime("%Y-%m-%d %H:%M:%S"),
|
||
(160, 160, 160), 18))
|
||
|
||
add_texts_to_pil(pil_img, texts)
|
||
arr = np.ascontiguousarray(np.array(pil_img))
|
||
h, w, ch = arr.shape
|
||
qimg = QImage(arr.tobytes(), w, h, w * ch,
|
||
QImage.Format_RGB888)
|
||
with self._lock:
|
||
self._out_queue.append(qimg)
|
||
|
||
# ── 更新串口发送内容 ──
|
||
dir_map = {"front": 0, "left": 1, "back": 2, "right": 3}
|
||
presence = [0x00, 0x00, 0x00, 0x00]
|
||
for d in active_dirs:
|
||
idx = dir_map.get(d)
|
||
if idx is not None:
|
||
presence[idx] = 0x01
|
||
|
||
radar_thr = self.radar_thr_ref[0] # 从共享引用读取
|
||
self.serial_worker.update_send(
|
||
presence[0], presence[1],
|
||
presence[2], presence[3],
|
||
radar_thr)
|
||
|
||
except Exception as e:
|
||
print("[RENDER WORKER ERROR] {}".format(e))
|
||
|
||
elapsed = time.time() - t0
|
||
sleep_t = self.FRAME_DELAY - elapsed
|
||
if sleep_t > 0:
|
||
time.sleep(sleep_t)
|
||
|
||
def stop(self):
|
||
self.running = False
|
||
|
||
|
||
# ==========================================================
|
||
# 11. 设置界面
|
||
# ==========================================================
|
||
class SettingsPanel(QWidget):
|
||
DEFAULT_SETTINGS = {
|
||
"carousel_interval": 4,
|
||
"brightness": 50,
|
||
"radar_unit": 0,
|
||
"show_radar": 1,
|
||
"radar_alarm_dist": 50, # 雷达报警阈值，单位 dm
|
||
}
|
||
|
||
def __init__(self, parent=None):
|
||
super().__init__(parent)
|
||
self._cfg = dict(self.DEFAULT_SETTINGS)
|
||
self._build_ui()
|
||
|
||
def _build_ui(self):
|
||
self.setStyleSheet("""
|
||
QWidget { background-color: #1a1a2e; color: #e0e0e0; }
|
||
QLabel { font-size: 16px; }
|
||
QPushButton {
|
||
background-color: #16213e;
|
||
color: #e0e0e0;
|
||
border: 1px solid #0f3460;
|
||
border-radius: 6px;
|
||
padding: 8px 20px;
|
||
font-size: 15px;
|
||
}
|
||
QPushButton:hover { background-color: #0f3460; }
|
||
QPushButton:pressed{ background-color: #533483; }
|
||
QSlider::groove:horizontal {
|
||
height: 6px; background: #0f3460; border-radius: 3px;
|
||
}
|
||
QSlider::handle:horizontal {
|
||
width: 18px; height: 18px; margin: -6px 0;
|
||
background: #e94560; border-radius: 9px;
|
||
}
|
||
QFrame#sep { background-color: #0f3460; }
|
||
""")
|
||
|
||
root = QVBoxLayout(self)
|
||
root.setContentsMargins(40, 30, 40, 30)
|
||
root.setSpacing(20)
|
||
|
||
title = QLabel("⚙ 系统设置")
|
||
title.setAlignment(Qt.AlignCenter)
|
||
title.setFont(QFont("", 26, QFont.Bold))
|
||
title.setStyleSheet("color: #e94560; font-size: 26px;")
|
||
root.addWidget(title)
|
||
root.addWidget(self._make_sep())
|
||
|
||
grid = QGridLayout()
|
||
grid.setHorizontalSpacing(30)
|
||
grid.setVerticalSpacing(22)
|
||
|
||
# 轮播间隔
|
||
grid.addWidget(self._label("轮播间隔（秒）"), 0, 0)
|
||
self._sld_interval = self._slider(1, 10, self._cfg["carousel_interval"])
|
||
self._lbl_interval = self._value_label(str(self._cfg["carousel_interval"]))
|
||
self._sld_interval.valueChanged.connect(
|
||
lambda v: self._lbl_interval.setText(str(v)))
|
||
grid.addWidget(self._sld_interval, 0, 1)
|
||
grid.addWidget(self._lbl_interval, 0, 2)
|
||
|
||
# 亮度
|
||
grid.addWidget(self._label("画面亮度"), 1, 0)
|
||
self._sld_bright = self._slider(0, 100, self._cfg["brightness"])
|
||
self._lbl_bright = self._value_label(str(self._cfg["brightness"]))
|
||
self._sld_bright.valueChanged.connect(
|
||
lambda v: self._lbl_bright.setText(str(v)))
|
||
grid.addWidget(self._sld_bright, 1, 1)
|
||
grid.addWidget(self._lbl_bright, 1, 2)
|
||
|
||
# 雷达报警阈值（dm）
|
||
grid.addWidget(self._label("雷达报警阈值（dm）"), 2, 0)
|
||
self._sld_radar_thr = self._slider(1, 200,
|
||
self._cfg["radar_alarm_dist"])
|
||
self._lbl_radar_thr = self._value_label(
|
||
str(self._cfg["radar_alarm_dist"]))
|
||
self._sld_radar_thr.valueChanged.connect(
|
||
lambda v: self._lbl_radar_thr.setText(str(v)))
|
||
grid.addWidget(self._sld_radar_thr, 2, 1)
|
||
grid.addWidget(self._lbl_radar_thr, 2, 2)
|
||
|
||
# 雷达单位
|
||
grid.addWidget(self._label("雷达距离单位"), 3, 0)
|
||
self._btn_unit = QPushButton(
|
||
"分米" if self._cfg["radar_unit"] == 0 else "厘米")
|
||
self._btn_unit.clicked.connect(self._toggle_unit)
|
||
grid.addWidget(self._btn_unit, 3, 1)
|
||
|
||
# 显示雷达
|
||
grid.addWidget(self._label("显示雷达距离"), 4, 0)
|
||
self._btn_radar = QPushButton(
|
||
"开启" if self._cfg["show_radar"] else "关闭")
|
||
self._btn_radar.clicked.connect(self._toggle_radar)
|
||
grid.addWidget(self._btn_radar, 4, 1)
|
||
|
||
root.addLayout(grid)
|
||
root.addWidget(self._make_sep())
|
||
|
||
hint = QLabel("按遥控 0A 保存并退出设置")
|
||
hint.setAlignment(Qt.AlignCenter)
|
||
hint.setStyleSheet("color: #888; font-size: 14px;")
|
||
root.addWidget(hint)
|
||
root.addStretch()
|
||
|
||
@staticmethod
|
||
def _label(text):
|
||
lbl = QLabel(text)
|
||
lbl.setAlignment(Qt.AlignRight | Qt.AlignVCenter)
|
||
return lbl
|
||
|
||
@staticmethod
|
||
def _value_label(text):
|
||
lbl = QLabel(text)
|
||
lbl.setFixedWidth(60)
|
||
lbl.setAlignment(Qt.AlignLeft | Qt.AlignVCenter)
|
||
lbl.setStyleSheet("color: #e94560; font-size: 18px; font-weight:bold;")
|
||
return lbl
|
||
|
||
@staticmethod
|
||
def _slider(mn, mx, val):
|
||
s = QSlider(Qt.Horizontal)
|
||
s.setRange(mn, mx)
|
||
s.setValue(val)
|
||
s.setFixedWidth(400)
|
||
return s
|
||
|
||
@staticmethod
|
||
def _make_sep():
|
||
f = QFrame()
|
||
f.setObjectName("sep")
|
||
f.setFrameShape(QFrame.HLine)
|
||
f.setFixedHeight(1)
|
||
return f
|
||
|
||
def _toggle_unit(self):
|
||
self._cfg["radar_unit"] = 1 - self._cfg["radar_unit"]
|
||
self._btn_unit.setText(
|
||
"分米" if self._cfg["radar_unit"] == 0 else "厘米")
|
||
|
||
def _toggle_radar(self):
|
||
self._cfg["show_radar"] = 1 - self._cfg["show_radar"]
|
||
self._btn_radar.setText(
|
||
"开启" if self._cfg["show_radar"] else "关闭")
|
||
|
||
def load(self):
|
||
self._sld_interval.setValue(self._cfg["carousel_interval"])
|
||
self._sld_bright.setValue(self._cfg["brightness"])
|
||
self._sld_radar_thr.setValue(self._cfg["radar_alarm_dist"])
|
||
self._btn_unit.setText(
|
||
"分米" if self._cfg["radar_unit"] == 0 else "厘米")
|
||
self._btn_radar.setText(
|
||
"开启" if self._cfg["show_radar"] else "关闭")
|
||
print("[设置] 界面已加载")
|
||
|
||
def save(self):
|
||
self._cfg["carousel_interval"] = self._sld_interval.value()
|
||
self._cfg["brightness"] = self._sld_bright.value()
|
||
self._cfg["radar_alarm_dist"] = self._sld_radar_thr.value()
|
||
print("[设置] 已保存: {}".format(self._cfg))
|
||
|
||
def get_settings(self) -> dict:
|
||
return dict(self._cfg)
|
||
|
||
|
||
# ==========================================================
|
||
# 12. PyQt 主窗口
|
||
# ==========================================================
|
||
class SurroundViewWindow(QMainWindow):
|
||
PAGE_MAIN = 0
|
||
PAGE_SETTINGS = 1
|
||
|
||
def __init__(self, multi_cam, serial_worker, remote_worker):
|
||
super().__init__()
|
||
self.multi_cam = multi_cam
|
||
self.serial_worker = serial_worker
|
||
self.remote_worker = remote_worker
|
||
self.carousel = CarouselScheduler()
|
||
|
||
# 共享雷达阈值（list包装，方便跨线程引用）
|
||
self._radar_thr_ref = [50]
|
||
|
||
self.setWindowTitle("环视系统 - 串口联动")
|
||
self.resize(SCREEN_W, SCREEN_H)
|
||
|
||
self.stack = QStackedWidget()
|
||
self.setCentralWidget(self.stack)
|
||
|
||
# 主视图页
|
||
main_page = QWidget()
|
||
main_lay = QVBoxLayout(main_page)
|
||
main_lay.setContentsMargins(0, 0, 0, 0)
|
||
self.image_label = QLabel()
|
||
self.image_label.setAlignment(Qt.AlignCenter)
|
||
self.image_label.setStyleSheet("background-color: black;")
|
||
main_lay.addWidget(self.image_label)
|
||
self.stack.addWidget(main_page)
|
||
|
||
# 设置页
|
||
self.settings_panel = SettingsPanel()
|
||
self.stack.addWidget(self.settings_panel)
|
||
self.stack.setCurrentIndex(self.PAGE_MAIN)
|
||
|
||
# 工作线程
|
||
self.raw_worker = RawCaptureWorker(multi_cam.caps, multi_cam.names)
|
||
self.raw_worker.start()
|
||
|
||
self.bird_worker = BirdViewWorker(
|
||
self.raw_worker, multi_cam.camera_models,
|
||
multi_cam.birdview, framestep=2)
|
||
self.bird_worker.start()
|
||
|
||
self.render_worker = RenderWorker(
|
||
self.bird_worker, self.raw_worker,
|
||
serial_worker, self.carousel,
|
||
self._radar_thr_ref) # ← 传入共享阈值引用
|
||
self.render_worker.start()
|
||
|
||
self.timer = QTimer()
|
||
self.timer.timeout.connect(self._flush)
|
||
self.timer.start(16)
|
||
|
||
remote_worker.signals.enter_settings.connect(self._on_enter_settings)
|
||
remote_worker.signals.exit_settings.connect(self._on_exit_settings)
|
||
|
||
def _flush(self):
|
||
if self.stack.currentIndex() != self.PAGE_MAIN:
|
||
return
|
||
qimg = self.render_worker.get_qimage()
|
||
if qimg is None:
|
||
return
|
||
pixmap = QPixmap.fromImage(qimg)
|
||
lw = self.image_label.width()
|
||
lh = self.image_label.height()
|
||
if qimg.width() != lw or qimg.height() != lh:
|
||
pixmap = pixmap.scaled(lw, lh, Qt.KeepAspectRatio,
|
||
Qt.FastTransformation)
|
||
self.image_label.setPixmap(pixmap)
|
||
|
||
def _on_enter_settings(self):
|
||
if self.stack.currentIndex() == self.PAGE_SETTINGS:
|
||
return
|
||
self.settings_panel.load()
|
||
self.stack.setCurrentIndex(self.PAGE_SETTINGS)
|
||
print("[UI] 已切换到设置界面")
|
||
|
||
def _on_exit_settings(self):
|
||
if self.stack.currentIndex() != self.PAGE_SETTINGS:
|
||
return
|
||
self.settings_panel.save()
|
||
cfg = self.settings_panel.get_settings()
|
||
CarouselScheduler.INTERVAL = float(cfg["carousel_interval"])
|
||
self._radar_thr_ref[0] = int(cfg["radar_alarm_dist"])
|
||
self.stack.setCurrentIndex(self.PAGE_MAIN)
|
||
print("[UI] 已退出设置，雷达阈值: {}dm".format(self._radar_thr_ref[0]))
|
||
|
||
def closeEvent(self, event):
|
||
self.timer.stop()
|
||
self.render_worker.stop()
|
||
self.bird_worker.stop()
|
||
self.raw_worker.stop()
|
||
self.serial_worker.stop()
|
||
self.remote_worker.stop()
|
||
self.multi_cam.running = False
|
||
for cap in self.multi_cam.caps:
|
||
cap.release()
|
||
event.accept()
|
||
|
||
|
||
# ==========================================================
|
||
# 13. 主函数
|
||
# ==========================================================
|
||
def main():
|
||
parser = argparse.ArgumentParser(description="环视系统 + 串口联动")
|
||
parser.add_argument("--mode", type=str, required=True,
|
||
choices=["realtime", "static"])
|
||
parser.add_argument("--port", type=str, default="/dev/ttyS0")
|
||
parser.add_argument("--remote-port", type=str, default="/dev/ttyS3")
|
||
parser.add_argument("--baudrate", type=int, default=115200)
|
||
parser.add_argument("--framestep", type=int, default=2)
|
||
parser.add_argument("--interval", type=float, default=4.0)
|
||
args = parser.parse_args()
|
||
|
||
if args.mode == "realtime":
|
||
CarouselScheduler.INTERVAL = args.interval
|
||
|
||
serial_worker = SerialWorker(port=args.port, baudrate=args.baudrate)
|
||
serial_worker.start()
|
||
print("[INFO] 雷达串口已启动: {} @ {} 发送周期: {}ms".format(
|
||
args.port, args.baudrate, SEND_INTERVAL_MS))
|
||
|
||
remote_worker = RemoteWorker(port=args.remote_port,
|
||
baudrate=args.baudrate)
|
||
remote_worker.start()
|
||
print("[INFO] 遥控串口已启动: {} @ {}".format(
|
||
args.remote_port, args.baudrate))
|
||
|
||
multi_cam = MultiCameraBirdView()
|
||
if not multi_cam.running:
|
||
print("[ERROR] 摄像头初始化失败")
|
||
serial_worker.stop()
|
||
remote_worker.stop()
|
||
return
|
||
|
||
app = QApplication(sys.argv)
|
||
win = SurroundViewWindow(multi_cam, serial_worker, remote_worker)
|
||
win.bird_worker.framestep = args.framestep
|
||
win.show()
|
||
sys.exit(app.exec_())
|
||
|
||
elif args.mode == "static":
|
||
print("[INFO] Static mode not implemented.")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|