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