songhaoyu/opi-yolo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

2026.6.25小更新

Browse Source
This commit is contained in:
songhaoyu
2026-06-26 11:33:21 +08:00
parent 308ceb6c21
commit 1f3341d18b
2 changed files with 59 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
infer_camera_modular.py
+18 -8
View File
@@ -287,18 +287,24 @@ class UAVControlLoop:
- 无目标时 PID 自动复位 → 输出零速
"""
def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg):
def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg, proxy=None):
self.logger = logging.getLogger("UAVControlLoop")
# ---- 保存 config（供 start_uav 使用如 takeoff_alt 等） ----
self.fc_cfg = fc_cfg
# -----------------------------------------------------------
# ---- 使用 RouterProxy 替换 DroneController ----
self.proxy = RouterProxy(
req_endpoint=fc_cfg.get("req_endpoint", "tcp://127.0.0.1:5555"),
sub_endpoint=fc_cfg.get("sub_endpoint", "tcp://127.0.0.1:5556")
)
# ---- 使用 RouterProxy（支持外部注入共享实例） ----
if proxy is not None:
self.proxy = proxy
self._owns_proxy = False
self.logger.info("使用外部注入的共享 RouterProxy")
else:
self.proxy = RouterProxy(
req_endpoint=fc_cfg.get("req_endpoint", "tcp://127.0.0.1:5555"),
sub_endpoint=fc_cfg.get("sub_endpoint", "tcp://127.0.0.1:5556")
)
self._owns_proxy = True
# ---------------------------------------------
self.pid_y = PID(**pid_y_cfg)
@@ -679,8 +685,12 @@ class UAVControlLoop:
self.tracker.reset()
self._tracked_result = None
self.proxy.close()
self.logger.info("控制已停止，ZMQ 代理已关闭，跟踪器已重置")
if self._owns_proxy:
self.proxy.close()
self.logger.info("自有 RouterProxy 已关闭")
else:
self.logger.info("共享 RouterProxy 交由外部管理，跳过 close")
self.logger.info("控制已停止，跟踪器已重置")
# ==================== 主程序入口 ======================
run_mission.py
+41 -49
View File
@@ -5,19 +5,21 @@ run_mission.py — 基于 ZMQ Router 协议的主任务入口
本脚本是整个无人机任务系统的总装粘合剂。采用领域驱动模块化设计，
将各子系统按职责独立实例化，在主循环中进行控制权协调。
┌─ MissionManager ────────────┐
│ RouterProxy (own) │ 状态机管理 + 航点巡航
│ POSITION SETPOINT (非追踪) │ 控制权：NAVIGATING/HOLD_TASK/...
└─────────────────────────────┘
↕ 控制权协调
┌─ UAVControlLoop ────────────┐
│ RouterProxy (own) │ 20Hz VELOCITY 闭环追踪
│ TargetTracker + PID │ 控制权：仅 VISUAL_TRACKING
└─────────────────────────────┘
┌─ 感知模块 ───────────────────┐
│ VideoStreaming │ 摄像头/RTSP 采集
│ YOLO26UAVInfer │ 昇腾 NPU 推理
└─────────────────────────────┘
┌─ MissionOrchestrator ──────────┐
│ RouterProxy (shared) │ 单一共享 ZMQ 代理
└──────────┬─────────────────────┘
│ 注入
┌─────┴──────┐
▼ ▼
┌─ MissionManager ┌─ UAVControlLoop ──┐
│ 状态机 + 航点巡航 │ 20Hz VELOCITY │
│ POSITION SETPOINT │ 闭环追踪 │
│ (非追踪期间) │ (VISUAL_TRACKING) │
└───────────────────┴────────────────────┘
┌─ 感知模块 ─────────┐
│ VideoStreaming │ 摄像头/RTSP 采集
│ YOLO26UAVInfer │ 昇腾 NPU 推理
└───────────────────┘
【控制权协调规则】
同一时刻只有一个人持有「SETPOINT 发令权」：
@@ -27,7 +29,8 @@ run_mission.py — 基于 ZMQ Router 协议的主任务入口
【安全设计】
- try / except FailsafeTriggered / finally 全面资源释放
- 两个独立 RouterProxy，避免 REQ socket 线程竞争
- MissionManager 和 UAVControlLoop 共享同一 RouterProxy，
通过控制权协调避免 REQ socket 并发冲突
- 控制线程启停由状态机状态变化驱动，逻辑可预测
【可扩展的图像输入接口】
@@ -107,18 +110,17 @@ class MissionOrchestrator:
# ================================================================
# 🚁 MissionManager（状态机 + 航点巡航）
# ================================================================
# 拥有独立的 RouterProxy，用于读取 STATE 和发送 COMMAND /
# WAYPOINT / POSITION SETPOINT。
# 与 UAVControlLoop 共享同一个 RouterProxy（见下方）。
fc = cfg["flight_control"]
self.mission_proxy = RouterProxy(
self.proxy = RouterProxy(
req_endpoint=fc.get("req_endpoint", "tcp://127.0.0.1:5555"),
sub_endpoint=fc.get("sub_endpoint", "tcp://127.0.0.1:5556"),
)
self.mission_proxy.set_waypoints_callback(self._on_qgc_waypoints)
self.proxy.set_waypoints_callback(self._on_qgc_waypoints)
self.mission = MissionManager(
proxy=self.mission_proxy,
proxy=self.proxy,
waypoints=self.waypoints,
target_altitude=1.5,
arrival_radius=0.3,
@@ -129,10 +131,11 @@ class MissionOrchestrator:
# ================================================================
# 🛸 UAVControlLoop（20Hz VELOCITY 闭环追踪）
# ================================================================
# 拥有独立的 RouterProxy。其 20Hz 控制线程仅在 VISUAL_TRACKING
# 期间运行，其余时间处于停止状态，避免与 MissionManager 冲突。
# 注入与 MissionManager 相同的 RouterProxy 实例，避免两个独立
# SUB 线程和 REQ socket 竞争同一 Router。
self.controller = UAVControlLoop(
cfg["flight_control"], cfg["pid_y"], cfg["pid_z"],
proxy=self.proxy,
)
self._control_active = False # 控制线程运行标志
@@ -247,7 +250,7 @@ class MissionOrchestrator:
def _collect_and_enqueue_log(self):
"""采集当前飞行状态，推入日志队列（主线程执行，极轻量）"""
try:
state = self.mission_proxy.get_latest_state()
state = self.proxy.get_latest_state()
if state is None:
return
@@ -269,7 +272,7 @@ class MissionOrchestrator:
drone.get("battery", 0.0), # battery_percent
current_state, # state (string)
wp_idx, # wp_index
len(self._latest_detections) if hasattr(self, 'detections') else 0, # detect_count
len(self._latest_detections) if self._latest_detections is not None else 0, # detect_count
tracking_active # tracking_active
)
@@ -300,7 +303,7 @@ class MissionOrchestrator:
self.logger.info("📥 收到 %d 个航点，开始坐标转换...", len(raw_wps))
# ---- ① 获取参考点（起飞点） ----
state = self.mission_proxy.get_latest_state()
state = self.proxy.get_latest_state()
if state is None:
self.logger.error("无法获取 STATE，缺少参考经纬度，航点转换失败")
return
@@ -488,15 +491,9 @@ class MissionOrchestrator:
"""启动所有子系统"""
self.logger.info("正在启动所有子系统...")
# 1. 启动 MissionManager 的 ZMQ 代理（SUB 监听 + REQ 就绪）
self.mission_proxy.start()
self.logger.info("✔ MissionManager Proxy 已启动")
# 2. 启动 UAVControlLoop 的 ZMQ 代理
# start_uav() 会执行完整的起飞序列，我们不需要 ——
# MissionManager 负责起飞。这里只启动 SUB 线程收 STATE。
self.controller.proxy.start()
self.logger.info("✔ UAVControlLoop Proxy 已启动")
# 1. 启动共享 ZMQ 代理（MissionManager 与 UAVControlLoop 共用）
self.proxy.start()
self.logger.info("✔ 共享 RouterProxy 已启动（MissionManager + UAVControlLoop 共用）")
# 3. 等待收到第一条 Router STATE
self._wait_for_initial_state()
@@ -516,7 +513,7 @@ class MissionOrchestrator:
"""等待 Router 推送第一条 STATE"""
deadline = time.time() + timeout
while time.time() < deadline:
state = self.mission_proxy.get_latest_state()
state = self.proxy.get_latest_state()
if state is not None:
drone = state.get("drone", {})
self.logger.info(
@@ -642,7 +639,7 @@ class MissionOrchestrator:
self.logger.info("▶ [控制权] 启动 UAVControlLoop VELOCITY 线程")
# 记录进入追踪时的飞机状态
state = self.mission_proxy.get_latest_state()
state = self.proxy.get_latest_state()
if state:
d = state.get("drone", {})
self.logger.info("追踪初始状态: mode=%s alt=%.1fm armed=%s",
@@ -694,7 +691,7 @@ class MissionOrchestrator:
# ---- 状态面板 ----
state_name = self.mission.state.name
wp_text = f"WP: {self.mission.current_wp_index + 1}/{len(self.waypoints)}"
wp_text = f"WP: {self.mission.current_wp_index + 1}/{len(self.mission.waypoints)}"
ctrl_text = "CTRL:VELOCITY" if self._control_active else "CTRL:POSITION"
cv2.putText(frame, f"State: {state_name} | {wp_text}",
@@ -706,7 +703,7 @@ class MissionOrchestrator:
# ---- 每 100 帧打印详细状态日志 ----
if loop_count % 100 == 0:
state = self.mission_proxy.get_latest_state()
state = self.proxy.get_latest_state()
if state:
d = state.get("drone", {})
self.logger.info(
@@ -754,21 +751,16 @@ class MissionOrchestrator:
self._log_thread.join(timeout=2.0)
self.logger.info("✔ 飞行日志线程已停止")
# 2. 关闭 MissionManager 的 Proxy
# 2. 关闭共享 ZMQ 代理（MissionManager + UAVControlLoop 共用）
# 注意：UAVControlLoop 不拥有 proxy 生命周期（_owns_proxy=False），
# 仅在此处统一关闭一次。
try:
self.mission_proxy.close()
self.logger.info("✔ MissionManager Proxy 已关闭")
self.proxy.close()
self.logger.info("✔ 共享 RouterProxy 已关闭")
except Exception as e:
self.logger.warning("mission_proxy close 异常: %s", e)
self.logger.warning("proxy close 异常: %s", e)
# 3. 关闭 UAVControlLoop 的 Proxy（直接访问其内部 proxy）
try:
self.controller.proxy.close()
self.logger.info("✔ UAVControlLoop Proxy 已关闭")
except Exception as e:
self.logger.warning("control_proxy close 异常: %s", e)
# 4. 释放视频资源
# 3. 释放视频资源
try:
self.streamer.release()
self.logger.info("✔ VideoStreamer 已释放")