From 4dc5b484150232a5a7bec8b0c24e2153e134c6df Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Fri, 12 Jun 2026 18:01:26 +0800 Subject: [PATCH] =?UTF-8?q?=E5=9F=BA=E4=BA=8EZMQ=20Router=E9=87=8D?= =?UTF-8?q?=E6=9E=84=E7=8A=B6=E6=80=81=E6=9C=BA=E7=9B=B8=E5=85=B3=E4=BB=A3?= =?UTF-8?q?=E7=A0=81=E4=BB=A5=E5=8F=8Areadme=E5=92=8C.sh=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E7=9A=84=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 408 +++++++++++++- drone_controller/mission_manager.py | 822 ++++++++++++++++++++++------ run_mission.py | 457 ++++++++++++++-- start_npu_infer.sh | 2 +- 4 files changed, 1462 insertions(+), 227 deletions(-) diff --git a/README.md b/README.md index 8f5e001..71f0e65 100644 --- a/README.md +++ b/README.md @@ -1,27 +1,385 @@ -# 这是一个***小白瞎搞***的东西,仅供参考(我甚至觉得不能作为参考) +# 🚁 OPI-YOLO — 昇腾 NPU 无人机目标检测与闭环控制系统 +### *这是一个小白的个人项目，慎用* -这些会部署在香橙派AI Pro上，模型为*YOLOv26n* +> **硬件平台：** 香橙派 AI Pro（Orange Pi AI Pro）｜**加速单元：** 华为昇腾 Ascend NPU +> **通信协议：** ZeroMQ Router/Dealer 架构 ｜ **飞控协议：** MAVLink（PX4 / ArduPilot） +> **检测模型：** YOLOv26n ｜ **部署方式：** ais_bench (昇腾推理 SDK) -## 说明下各文件的作用 -#### *python部分* -#### **零散脚本** -- check.py:确认输入视频帧率，用于性能测试的; -- control.py:PID计算和MAVLink指令发送方法的定义(已废弃); -- fc_test:获取飞控信息的测试脚本; -- infer_camera_direct.py:通过摄像头检测目标并调用方法控制无人机; -- infer.py:使用图片检测目标，仅用于测试环境; -- infer_camera_modular.py:模块化处理过的摄像头检测与控制(以确认链路基本通顺，但是无人机追踪效果如何尚未测试) -- infer_camera_video.py:使用视频检测目标并调用方法控制无人机(用于测试,未调参) -- gent_mission.py:尝试将视觉追踪与自动巡点飞行相结合(未测试，这应该是本项目一阶段的最终预期) -- run_mission.py:自动巡点飞行的业务测试(未测试) -#### drone_controller:控制模块 -- base_control.py:定义了部分基本动作的方法(最新规划已改用路由，已弃置) -- pid_counter.py:抽离出来的PID计算器 -- mission_manager.py:自动巡点飞行的控制模块(最新规划已改用路由，已弃置) -- router_proxy.py:基于新路由的路由规则编写的相关代码 -- target_tracker.py:通过math库和numpy编写的简单目标跟踪 -#### 其他目录以及文件： -- om:推理使用的.om文件 -- asset:用于基础测试的文件 -- config.json:配置文件 -- start_npu_infer:一个.sh文件,计划后续将整个脚本作为一个服务 \ No newline at end of file +--- + +> [!IMPORTANT] +> **⚡ AI 开发者注意（AI Orientation）** +> 本项目已建立现成的 `.codegraph` 代码图谱索引（`.codegraph/codegraph.db`）。后续任何 AI 辅助工具接入时，请**优先使用 `mcp__codegraph` 系列工具**读取该数据库以获得全局代码感知能力（调用链追踪、依赖分析、符号定位），**切勿重复生成独立索引**。索引覆盖 25 个文件、329+ 个符号节点、458+ 条依赖边。 + +--- + +## 📋 目录 + +- [项目简介](#-项目简介) +- [系统架构](#-系统架构) +- [模块设计](#-模块设计) + - [VideoStreaming — 视频流采集与图传](#1-videostreaming--视频流采集与图传) + - [YOLO26UAVInfer — NPU 推理核心](#2-yolo26uavinfer--npu-推理核心) + - [TargetTracker — 目标追踪与卡尔曼滤波](#3-targettracker--目标追踪与卡尔曼滤波) + - [UAVControlLoop — 飞控闭环控制](#4-uavcontrolloop--飞控闭环控制) + - [RouterProxy — ZeroMQ 通信代理](#5-routerproxy--zeromq-通信代理) + - [MissionManager — 任务状态机](#6-missionmanager--任务状态机) +- [环境要求与依赖](#-环境要求与依赖) +- [配置文件](#-配置文件) +- [目录结构](#-目录结构) +- [快速开始](#-快速开始) +- [免责声明](#-免责声明) + +--- + +## 🎯 项目简介 + +本项目实现了一套**基于机载 NPU 硬件加速的无人机实时目标检测与视觉闭环控制系统**，在香橙派 AI Pro（昇腾平台）上全速运行。 + +**核心工作流：** + +``` +摄像头/RTSP 视频流 + │ + ▼ + ┌────────────────┐ ┌──────────────────┐ + │ VideoStreaming │ ──► │ YOLO26UAVInfer │ + │ 帧采集 + 图传 │ │ NPU 推理 + 后处理 │ + └────────────────┘ └────────┬─────────┘ + │ 检测结果 (bbox + score) + ▼ + ┌────────────────┐ ┌──────────────────┐ + │ MissionManager │ ◄── │ TargetTracker │ + │ 任务状态机 │ │ 卡尔曼滤波+Coast │ + └───────┬────────┘ └────────┬─────────┘ + │ │ 追踪坐标 + ▼ ▼ + ┌─────────────────────────────────────────┐ + │ UAVControlLoop (20Hz) │ + │ PID 计算机体速度 → RouterProxy → Router │ + └─────────────────────────────────────────┘ + │ + ▼ + PX4 / ArduPilot 飞控 +``` + +**关键设计理念：** + +- **解耦通信层**：通过 ZeroMQ Router 代理隔离 Python 控制层与 MAVLink 底层，REQ/SUB 双通道实现指令-推送分离 +- **异步流水线**：视频采集、NPU 推理、UDP 图传、飞控控制分属独立线程，互不阻塞 +- **安全优先**：防断流超时刹车（500ms）、卡尔曼 Coast 预测、PID 积分自动清空、Failsafe 人工接管熔断 + +--- + +## 🏗 系统架构 + +### 通信架构 + +``` +┌─────────────────────────────────────────────────────────┐ +│ 香橙派 AI Pro │ +│ │ +│ ┌──────────┐ ┌──────────────┐ ┌───────────────┐ │ +│ │ Python │──► │ ZeroMQ REQ │──► │ │ │ +│ │ 控制层 │ │ :5555 │ │ Router │ │ +│ │ │◄── │ ACK 回执 │◄── │ 代理 │ │ +│ │ │ └──────────────┘ │ │ │ +│ │ │ ┌──────────────┐ │ (C++/Rust) │──► PX4 +│ │ │◄── │ ZeroMQ SUB │◄── │ │ │ +│ │ │ │ :5556 │ │ │ │ +│ └──────────┘ └──────────────┘ └───────────────┘ │ +│ │ +│ ┌──────────┐ ┌──────────────┐ │ +│ │ RTSP │──► │ VideoStream │──► UDP :9999 → 地面站 │ +│ │ 摄像头 │ │ + 推理 │ │ +│ └──────────┘ └──────────────┘ │ +└─────────────────────────────────────────────────────────┘ +``` + +### 消息协议 + +| type | 方向 | 通道 | 频率 | 说明 | +|------|------|------|------|------| +| `SETPOINT` | Python → Router | REQ | 20Hz | 实时 Offboard 控制（NED 位置/速度） | +| `WAYPOINT` | Python → Router | REQ | 按需 | 航点飞行（GPS + 高度 + 速度 + 动作） | +| `COMMAND` | Python → Router | REQ | 按需 | ARM / OFFBOARD / LAND / RTL / RESUME | +| `QUERY` | Python → Router | REQ | 按需 | 查询 HOME_POSITION 等 | +| `STATE` | Router → Python | PUB | 10Hz | 飞机状态（模式、位置、电量等） | +| `ALERT` | Router → Python | PUB | 按需 | 紧急告警（低电量、GPS 丢失等） | +| `PX4_ACK` | Router → Python | PUB | 按需 | PX4 COMMAND_ACK 转发 | + +> 详细协议规范见 [`python_protocol.md`](./python_protocol.md) + +--- + +## 🔧 模块设计 + +### 1. VideoStreaming — 视频流采集与图传 + +**文件：** `infer_camera_modular.py:31` ｜ `class VideoStreaming` + +负责视频帧的**双线程异步采集**与**UDP 图传**。 + +| 特性 | 说明 | +|------|------| +| **独立采集线程** | 后台 `_capture_worker` 线程持续从摄像头/RTSP 读取帧，永不阻塞主循环 | +| **单帧缓冲区** | `queue.Queue(maxsize=1)` 仅保留最新帧，积压帧自动丢弃（避免延迟累积） | +| **RTSP 低延迟** | `CAP_PROP_BUFFERSIZE=1` + 自动追加 `?tcp` 传输模式 | +| **自动重连** | 读取失败 → 释放资源 → 1 秒后重新连接循环，断流后自主恢复 | +| **UDP 图传** | 独立 `sender_thread` 线程 JPEG 压缩后以 UDP 推流至地面站（默认 `:9999`），超 65KB 自动跳过 | + +### 2. YOLO26UAVInfer — NPU 推理核心 + +**文件：** `infer_camera_modular.py:187` ｜ `class YOLO26UAVInfer` + +基于华为昇腾 `ais_bench` SDK（`InferSession`）的推理封装。 + +| 阶段 | 说明 | +|------|------| +| **图像预处理** | `letterbox()` — 保持宽高比的缩放 + 灰边填充（640×640），适配 NPU 固定输入 | +| **张量转换** | `preprocess()` — HWC → CHW、归一化到 `[0,1]`、`float32`、添加 batch 维度 | +| **NPU 推理** | `InferSession(device_id, model_path).infer()` — 异步硬件推理 | +| **坐标回映射** | `postprocess()` — 置信度阈值过滤 → Letterbox 逆变换 → 原图坐标裁剪 | +| **可视化** | `draw_boxes()` — 在帧上渲染检测框 + 置信度标签 | + +### 3. TargetTracker — 目标追踪与卡尔曼滤波 + +**文件：** `drone_controller/target_tracker.py:42` ｜ `class _KalmanBoxFilter` / `class TargetTracker` + +基于恒定速度卡尔曼滤波实现单目标追踪，解决 YOLO 帧间抖动和短时遮挡问题。 + +| 组件 | 说明 | +|------|------| +| **卡尔曼滤波器** | 8 维状态 `[cx, cy, w, h, vx, vy, vw, vh]`，过程/观测噪声与目标尺寸动态关联 | +| **Tracklet 轨道** | 记录 `track_id`、连续丢失计数、匹配命中次数、生存帧数 | +| **数据关联** | 基于 IOU 的匈牙利匹配（`linear_assignment`），支持 `max_lost_frames` 帧内 coast | +| **Coast 预测** | 目标短暂丢失时靠卡尔曼预测持续输出位置，超阈值后判定目标死亡 | +| **命中验证** | `min_hits` 击发机制：新目标需连续匹配 N 帧才确认为有效轨道 | + +### 4. UAVControlLoop — 飞控闭环控制 + +**文件：** `infer_camera_modular.py:266` ｜ `class UAVControlLoop` + +**20Hz 高频控制线程**，通过 ZeroMQ RouterProxy 间接操控飞控。 + +``` +┌─ 主循环 ─────────────────┐ +│ update_detections() │──→ 写入共享检测结果（锁保护） +└───────────────────────────┘ + │ +┌─ 控制线程 (20Hz) ─────────┐ +│ _compute_velocity() │──→ PID 计算机体速度 (vy, vz) +│ proxy.send_setpoint() │──→ ZMQ REQ → Router → PX4 +└───────────────────────────┘ +``` + +**安全设计：** + +| 机制 | 说明 | +|------|------| +| **20Hz 保活心跳** | 控制线程以固定 20Hz 循环发送 SETPOINT，满足 Offboard 协议 ≥20Hz 要求 | +| **防断流刹车** | `_last_update_time` 超过 **500ms** 未更新 → `_brake_requested=True` → 强制零速刹车 | +| **PID 积分清空** | 目标丢失时调用 `pid_y.reset()` / `pid_z.reset()`，防止积分 windup 导致失控 | +| **起飞序列自动化** | `start_uav()`：启动 ZMQ → 等待 STATE → ARM → TAKEOFF → OFFBOARD 预热 → 启动控制线程 | + +### 5. RouterProxy — ZeroMQ 通信代理 + +**文件：** `drone_controller/router_proxy.py:49` ｜ `class RouterProxy` + +Python 控制层与外部 Router 进程之间的通信桥梁。 + +| 功能 | 说明 | +|------|------| +| **REQ 通道** | `send_setpoint()` / `send_command()` / `send_query()` / `send_waypoint()` 四种指令接口 | +| **SUB 通道** | 后台 `_sub_worker` 持续监听 STATE / ALERT / PX4_ACK 推送 | +| **STATE 缓存** | `_latest_state` 受锁保护的线程安全缓存 + 深度拷贝读取 | +| **新鲜度检测** | `is_state_fresh()` — 500ms 新鲜度阈值 + 2s 警告阈值（协议 §3.8.1） | +| **异常重建** | `_recover_req()` — REQ 超时或异常时自动重建 ZMQ socket（含 REQ_RELAXED+CORRELATE 兼容） | +| **回调钩子** | `set_state_callback()` / `set_alert_callback()` — 可注册外部回调处理推送消息 | + +### 6. MissionManager — 任务状态机 + +**文件：** `drone_controller/mission_manager.py:24` ｜ `class MissionManager` + +面向航点巡航 + 视觉追踪融合任务的复合状态机。 + +``` +INIT → ARMING → TAKEOFF → NAVIGATING → HOLD_TASK ──→ VISUAL_TRACKING ──→ (继续巡航) + │ │ │ + └── (无目标超时) ────────┘ │ + ▼ + RETURNING → LANDING → FINISHED + 或 + HOLD_FINAL (不回航时) ─── FINISHED +``` + +| 特性 | 说明 | +|------|------| +| **Failsafe 熔断** | 检测到飞控模式被切出 GUIDED/OFFBOARD 时立即抛出 `FailsafeTriggered` 异常，暴力熔断主循环 | +| **视觉追踪超时** | 追踪持续 5 秒视为任务完成（`_is_tracking_task_complete`，可自定义） | +| **悬停检索** | 到达航点后进入 HOLD_TASK，在规定时间内等待视觉发现目标 | +| **返航策略** | 支持 `return_to_home=True/False`，可选择返航降落或末端悬停 | + +--- + +## 📦 环境要求与依赖 + +### 硬件平台 + +| 组件 | 规格 | +|------|------| +| **主控** | 香橙派 AI Pro（Orange Pi AI Pro）或兼容昇腾平台 | +| **NPU** | 华为昇腾 Ascend 310B / 同等算力单元 | +| **摄像头** | USB 摄像头 或 RTSP 网络摄像头 | +| **飞控** | PX4 / ArduPilot 固件兼容飞控（串口连接） | + +### 软件依赖 + +| 依赖 | 用途 | 最低版本 | +|------|------|----------| +| `ais_bench` | 昇腾 NPU 推理 SDK | 最新稳定版 | +| `opencv-python` | 视频采集、图像预处理、可视化渲染 | ≥4.5 | +| `pyzmq` | ZeroMQ 通信（REQ/SUB 协议） | ≥22 | +| `numpy` | 张量操作、卡尔曼滤波矩阵运算 | ≥1.21 | +| `pymavlink` | MAVLink 协议（DroneController 模式） | 最新版 | + +### 昇腾环境变量 + +```bash +# 香橙派 AI Pro 需加载（脚本已自动处理） +source /usr/local/Ascend/ascend-toolkit/set_env.sh +``` + +--- + +## ⚙ 配置文件 + +`config.json` 集中管理系统配置： + +```json +{ + "model": { + "model_path": "./om/yolo26n-balloon2.om", // OM 模型路径 + "conf_threshold": 0.25, // 置信度阈值 + "device_id": 0 // NPU 设备 ID + }, + "video": { + "video_source": "rtsp://192.168.144.25:8554/main.264", // 视频源（0=USB 摄像头） + "jpeg_quality": 75 // 图传 JPEG 质量 + }, + "network": { + "ground_station_ip": "192.168.31.221", // 地面站 IP + "udp_port": 9999 // 图传 UDP 端口 + }, + "flight_control": { + "req_endpoint": "tcp://127.0.0.1:5555", // ZMQ REQ 地址 + "sub_endpoint": "tcp://127.0.0.1:5556", // ZMQ SUB 地址 + "takeoff_alt": 5.0, // 起飞目标高度 (m) + "control_hz": 20, // 控制频率 (Hz) + "target_tracker": { + "max_lost_frames": 8, // 最大连续丢失帧数 + "max_asscociation_dist": 200.0, // 最大关联距离 (px) + "min_hits": 3 // 最小命中确认数 + } + }, + "pid_y": { "kp": 1.0, "ki": 0.02, "kd": 0.3, "max_out": 0.8, "min_out": -0.8 }, + "pid_z": { "kp": 1.0, "ki": 0.02, "kd": 0.3, "max_out": 0.4, "min_out": -0.4 } +} +``` + +--- + +## 📁 目录结构 + +``` +opi-yolo/ +├── infer_camera_modular.py # 🚀 主入口：模块化视觉追踪（推荐运行入口） +├── infer_camera_direct.py # 直接调用控制（旧版，走 MAVLink 直连） +├── gent_mission.py # 航点巡航 + 视觉追踪融合版 +├── run_mission.py # 自动巡点飞行业务测试 +├── control.py # PID + MAVLink 指令（旧版，直连飞控模式） +├── config.json # 项目全局配置文件 +├── python_protocol.md # Python ↔ Router 通信协议规范 +├── start_npu_infer.sh # 🔁 NPU 推理开机自启脚本（崩溃自动重启） +├── videotest.py # 视频源连通性测试 +├── README.md # 本文件 +│ +├── drone_controller/ +│ ├── router_proxy.py # ZMQ REQ/SUB 通信代理（当前主力通信方式） +│ ├── target_tracker.py # 卡尔曼滤波目标追踪器 +│ ├── mission_manager.py # 航点任务状态机 +│ ├── base_control.py # MAVLink 底层驱动（旧版，已弃置） +│ └── pid_counter.py # PID 计算器（抽离版） +│ +├── base_test/ +│ ├── check.py # 视频帧率测试 +│ ├── fc_test.py # 飞控通信测试 +│ ├── infer.py # 图片推理测试 +│ └── receiver.py # UDP 图传接收模拟 +│ +├── om/ # 昇腾 OM 模型文件存放目录 +│ └── yolo26n-balloon2.om +│ +└── asset/ # 基础测试资源文件 +``` + +--- + +## 🚀 快速开始 + +### 1. 环境准备 + +```bash +# 加载昇腾环境（香橙派 AI Pro 必须） +source /usr/local/Ascend/ascend-toolkit/set_env.sh + +# 激活 Python 环境 +conda activate base + +# 安装依赖 +pip install opencv-python pyzmq numpy pymavlink ais_bench +``` + +### 2. 配置 + +编辑 `config.json`，根据实际部署修改： +- `video.video_source`：摄像头设备号（`0`）或 RTSP 地址 +- `flight_control.req/sub_endpoint`：ZeroMQ 端点地址 +- `network.ground_station_ip`：地面站 IP（图传接收端） + +### 3. 运行 + +```bash +# 模块化视觉追踪（推荐） +python infer_camera_modular.py + +# 或使用自启脚本（崩溃自动重启） +chmod +x start_npu_infer.sh +./start_npu_infer.sh + +# 或航点巡航 + 视觉追踪融合 +python gent_mission.py +``` + +### 4. 地面站接收图传 + +在 `ground_station_ip` 对应的机器上运行 UDP 接收端（参考 `base_test/receiver.py`）查看实时图传画面。 + +--- + +## ⚠️ 免责声明 + +> **安全警告**：本系统涉及无人机自主飞行控制。在实际部署前，请务必： +> 1. 在仿真环境（如 Gazebo / SITL）中充分测试全部功能 +> 2. 在固定台架上验证 PID 参数和控制逻辑 +> 3. 确保遥控器手动接管通道可用且优先级高于自主控制 +> 4. 准备物理急停开关 +> 5. 遵守当地无人机飞行法规 +> +> 本项目作者不对因使用本软件造成的任何财产损失或人身伤害承担责任。 + +--- + +> [!IMPORTANT] +> **⚡ AI 开发者注意（AI Orientation）** +> 本项目已建立现成的 `.codegraph` 代码图谱索引（`.codegraph/codegraph.db`）。后续任何 AI 辅助工具接入时，请**优先使用 `mcp__codegraph` 系列工具**读取该数据库以获得全局代码感知能力（调用链追踪、依赖分析、符号定位），**切勿重复生成独立索引**。索引覆盖 25 个文件、329+ 个符号节点、458+ 条依赖边。 diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py index 760b016..1adde4d 100644 --- a/drone_controller/mission_manager.py +++ b/drone_controller/mission_manager.py @@ -1,208 +1,714 @@ +""" +mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机 + +【重构背景】 + 系统底层架构已升级为 ZMQ Router 代理通信协议。本模块彻底摒弃 + 旧版 DroneController 直连方式，全面迁移至 RouterProxy。 + +【核心改动】 + 1. self.drone → self.proxy（注入的 RouterProxy 实例） + 2. 所有状态读取：proxy.get_latest_state()["drone"][...] + 3. 所有指令发送：proxy.send_command() / send_waypoint() / send_setpoint() + 4. 新增 STATE 新鲜度校验（协议 §3.8.1），过期数据保守处理 + 5. 新增 Failsafe 模式断流保护，退出 OFFBOARD 即熔断 + 6. VISUAL_TRACKING 状态下完全让渡控制权，不发送任何 SETPOINT + +【位置估计】 + Router 协议未暴露局部 NED 坐标，本模块使用速度积分 + （vx × dt 累加）推算相对起飞点的水平位置，用于航点到 + 达判定。垂直轴直接使用 state["drone"]["alt_rel"]。 + +【控制权协调】 + MissionManager 通过自有 proxy 发送指令。连续 20Hz SETPOINT + 流由 UAVControlLoop 的独立线程负责。run_mission.py 负责协调： + 在 VISUAL_TRACKING 外停止 UAVControlLoop 线程，避免两套 + SETPOINT 流同时激活产生冲突。 +""" + import time import math +import logging from enum import Enum +logger = logging.getLogger(__name__) + class MissionState(Enum): - INIT = 0 # 初始化/等待连接 - ARMING = 1 # 解锁中 - TAKEOFF = 2 # 自动起飞中 - NAVIGATING = 3 # 航点巡航导航 - HOLD_TASK = 4 # 到达航点后的原地搜索/悬停 - VISUAL_TRACKING = 5 # 🎯 视觉追踪中（由视觉主循环接管控制权） - RETURNING = 6 # 返航中 - LANDING = 7 # 自动降落中 - FINISHED = 8 # 任务结束/已上锁 - HOLD_FINAL = 9 # 结束巡点后原地悬停（当 return_to_home 为 False 时使用） + """无人机任务状态枚举""" + INIT = 0 # 初始化 / 等待连接 + ARMING = 1 # 解锁中 + TAKEOFF = 2 # 自动起飞中 + NAVIGATING = 3 # 航点巡航导航 + HOLD_TASK = 4 # 到达航点后的原地搜索 / 悬停 + VISUAL_TRACKING = 5 # 🎯 视觉追踪中（控制权让渡给 UAVControlLoop） + RETURNING = 6 # 返航中 + LANDING = 7 # 自动降落中 + FINISHED = 8 # 任务结束 / 已上锁 + HOLD_FINAL = 9 # 结束巡点后原地悬停（return_to_home=False） class FailsafeTriggered(Exception): - """自定义安全异常：当安全员人工介入切出自控模式时抛出，用于暴力熔断主循环""" + """ + 自定义安全异常：当安全员人工介入切出自控模式时抛出， + 用于暴力熔断主循环。 + """ pass +# ================================================================ +# 内部工具 +# ================================================================ + +def _dist_3d(x1, y1, z1, x2, y2, z2): + """三维欧氏距离""" + return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2) + + +# ================================================================ +# 航点巡航管理器 +# ================================================================ + class MissionManager: - def __init__(self, drone, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0, - return_to_home=True): + """ + 🛸 ZMQ 航点巡航状态机管理器 + + 职责： + - 封装完整的无人机任务生命周期（初始化 → 解锁 → 起飞 → + 航点巡航 → 视觉追踪 → 返航 → 降落） + - 所有底层通信通过注入的 RouterProxy 完成 + - 在 VISUAL_TRACKING 状态下让渡控制权给 UAVControlLoop + - Failsafe 熔断保护 + + 使用示例： + proxy = RouterProxy() + proxy.start() + manager = MissionManager(proxy, waypoints=[...], target_altitude=1.5) + + while True: + manager.update(target_detected=some_bool) + time.sleep(0.05) + """ + + # ---- Failsafe：自主飞行的合法模式白名单 ---- + AUTONOMOUS_MODES = frozenset({ + "OFFBOARD", "GUIDED", "AUTO", "HOLD", "LAND", "RTL", + }) + + # ---- 位置估计：速度积分有效时间窗口 ---- + MAX_POSITION_DT = 1.0 + + # ---- 目标丢失超时（秒） ---- + DEFAULT_TARGET_LOST_TIMEOUT = 3.0 + + # ---- 各阶段超时 ---- + TAKEOFF_TIMEOUT = 15.0 + OFFBOARD_TIMEOUT = 5.0 + ARM_TIMEOUT = 5.0 + + def __init__(self, proxy, waypoints, target_altitude=1.5, + arrival_radius=0.3, hold_duration=5.0, + return_to_home=True, target_lost_timeout=None): """ - :param drone: 实例化的 DroneController 底层驱动对象 - :param waypoints: 航点列表 [{'x': 1.0, 'y': 2.0, 'z': -1.5, 'yaw': 0}, ... ] - :param target_altitude: 默认起飞高度（米，NED中向上为负值） - :param arrival_radius: 航点到达判定半径（米） - :param hold_duration: 抵达巡点后，没有发现目标时的最大搜寻/悬停时间（秒） - :param return_to_home: 航点巡完后是否返航，False则在最后一个点原地悬停 + Args: + proxy: RouterProxy 实例（调用方负责 start/close） + waypoints: 航点列表 [{'x':1,'y':2,'z':-1.5,'yaw':0}, ...] + target_altitude: 默认起飞高度（米，正值=向上） + arrival_radius: 航点到达判定半径（米） + hold_duration: 巡点无目标的最大悬停搜索时间（秒） + return_to_home: 航点巡完后是否返航，False 则终点悬停 + target_lost_timeout: VISUAL_TRACKING 目标丢失超时（秒） """ - self.drone = drone - self.waypoints = waypoints - self.target_altitude = target_altitude if target_altitude < 0 else -target_altitude + # ---- 依赖注入 ---- + self.proxy = proxy + + # ---- 任务参数 ---- + # 航点列表（外部队列已经使用 NED 坐标系，Z 负值向上） + self.waypoints = list(waypoints) if waypoints else [] + self.target_altitude = -abs(target_altitude) # NED Z（负值向上） self.arrival_radius = arrival_radius self.hold_duration = hold_duration self.return_to_home = return_to_home + self.target_lost_timeout = ( + target_lost_timeout if target_lost_timeout is not None + else self.DEFAULT_TARGET_LOST_TIMEOUT + ) + # ---- 状态机 ---- self.state = MissionState.INIT self.current_wp_index = 0 - # 计时器定义 + # ---- 当前目标航点（共享给外部主循环 / UAVControlLoop） ---- + self.current_target = { + "x": 0.0, "y": 0.0, + "z": self.target_altitude, "yaw": 0.0, + } + + # ================================================================ + # 本地位置估计（速度积分法） + # ================================================================ + self._est_x = 0.0 # NED 北向（相对起飞点） + self._est_y = 0.0 # NED 东向 + self._pos_time = 0.0 # 上次位置更新时间戳 + self._home_set = False # 首次 STATE 标记 + + # ---- 计时器 ---- self.hold_start_time = 0.0 self.tracking_start_time = 0.0 + self._target_lost_start = None + self._state_start_time = time.time() - # 起始点（Home）坐标缓存 - self.home_x = 0.0 - self.home_y = 0.0 - self.home_z = 0.0 + # ================================================================ + # 追踪完成信号（外部设置） + # ================================================================ + # UAVControlLoop 或 run_mission.py 在判定视觉追踪完成时置 True + self.tracking_completed = False + # 追踪最大时长兜底 + self.tracking_max_duration = 30.0 - def _is_tracking_task_complete(self): - """ - 🛠️ 预留接口：判断视觉追踪任务是否完成 - 你可以根据需求修改内部逻辑。例如： - - 当前：假设持续追踪 5 秒视为完成任务。 - - 后续扩展：可以引入与目标的物理距离、传感器状态等。 - """ - tracking_duration = time.time() - self.tracking_start_time - target_hold_time = 5.0 # 默认设定5秒 + logger.info("MissionManager 初始化: %d 个航点, 目标高度=%.1fm, " + "到达半径=%.1fm, 搜寻时长=%.1fs, 返航=%s", + len(self.waypoints), -self.target_altitude, + self.arrival_radius, self.hold_duration, + self.return_to_home) - if tracking_duration >= target_hold_time: - return True - return False + # ================================================================ + # 主入口 + # ================================================================ def update(self, target_detected=False): """ - 状态机高频核心业务逻辑，由外部主循环驱动 - :param target_detected: 当前帧是否发现了视觉追踪目标 + 状态机高频核心业务逻辑，由外部主循环驱动（建议 ≥20Hz） + + Args: + target_detected: 当前帧视觉检测是否有目标 + + Raises: + FailsafeTriggered: 检测到非法模式切换时抛出 """ - # 0. 越权检查（最强人肉兜底）：只要切出自主控制模式，立刻暴力熔断 - if self.state not in [MissionState.INIT, MissionState.FINISHED]: - if self.drone.current_mode not in ['GUIDED', 'OFFBOARD']: - print(f"\n⚠️ [Failsafe] 检测到飞控模式被切为: {self.drone.current_mode}! 外部人工强行接管，熔断任务！") - self.state = MissionState.FINISHED - raise FailsafeTriggered("安全员强行接管") + # --------------------------------------------------------------- + # 步骤 0：获取最新 STATE + 新鲜度校验 + Failsafe + # --------------------------------------------------------------- + state = self.proxy.get_latest_state() - # 1. INIT: 等待飞控连接并获取当前位置作为 Home 点 - if self.state == MissionState.INIT: - if self.drone.is_connected: - print("[INIT] 飞控已连上。获取当前坐标为起始点...") - self.home_x, self.home_y, self.home_z = self.drone.local_position - print(f"[INIT] 起始点坐标: ({self.home_x:.2f}, {self.home_y:.2f}, {self.home_z:.2f})") - print("[INIT] 正在下发 GUIDED/OFFBOARD 模式并尝试解锁...") + # ---- 新鲜度校验（协议 §3.8.1） ---- + if self.state not in (MissionState.INIT, MissionState.FINISHED): + if state is not None: + if not self.proxy.is_state_fresh(state): + age_us, _ = self.proxy.get_state_freshness(state) + age_s = (age_us or 0) / 1_000_000 + logger.warning("⚠️ STATE 严重过期 (age=%.2fs)，" + "本轮保守跳过", age_s) + return + else: + logger.warning("⚠️ 尚未收到 STATE，跳过本轮处理") + return - target_mode = "OFFBOARD" if "OFFBOARD" in self.drone.master.mode_mapping() else "GUIDED" - self.drone.set_mode(target_mode) - self.drone.arm() - self.state = MissionState.ARMING + # ---- Failsafe 越权检查 ---- + if self.state not in (MissionState.INIT, MissionState.FINISHED): + if state is not None: + mode = state["drone"].get("mode", "") + if mode not in self.AUTONOMOUS_MODES: + logger.warning( + "\n⚠️ [Failsafe] 飞控模式被切为: %s！" + "人工接管，熔断任务！", mode + ) + self.state = MissionState.FINISHED + raise FailsafeTriggered( + f"安全员接管，当前模式: {mode}" + ) - # 2. ARMING: 监测解锁状态 - elif self.state == MissionState.ARMING: - if self.drone.is_armed and self.drone.current_mode in ['GUIDED', 'OFFBOARD']: - print("\n[解锁成功] 正在下发自动起飞指令...") - # 下发位置到期望高度 - self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0) - self.state = MissionState.TAKEOFF + # ---- 无 STATE 时仅 INIT 可容忍 ---- + if state is None: + if self.state == MissionState.INIT: + logger.info("[INIT] 等待 Router 状态推送...") + return - # 3. TAKEOFF: 自动起飞爬升判定 - elif self.state == MissionState.TAKEOFF: - current_z = self.drone.local_position[2] - # 持续发送起飞点位置，激活控制链路 - self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0) + # --------------------------------------------------------------- + # 步骤 1：更新本地位置估计 + # --------------------------------------------------------------- + self._update_local_position(state) - # NED 坐标系下 Z 为负数，判断是否接近目标高度 - if abs(current_z - self.target_altitude) < self.arrival_radius: - print("\n[起飞完成] 达到预定高度。开始前往第 1 个巡航航点...") + # --------------------------------------------------------------- + # 步骤 2：状态机分支 + # --------------------------------------------------------------- + try: + if self.state == MissionState.INIT: + self._handle_init(state) + elif self.state == MissionState.ARMING: + self._handle_arming(state) + elif self.state == MissionState.TAKEOFF: + self._handle_takeoff(state) + elif self.state == MissionState.NAVIGATING: + self._handle_navigating(state) + elif self.state == MissionState.HOLD_TASK: + self._handle_hold_task(state, target_detected) + elif self.state == MissionState.VISUAL_TRACKING: + self._handle_visual_tracking(state, target_detected) + elif self.state == MissionState.RETURNING: + self._handle_returning(state) + elif self.state == MissionState.LANDING: + self._handle_landing(state) + elif self.state == MissionState.HOLD_FINAL: + self._handle_hold_final(state) + except FailsafeTriggered: + raise + except Exception as e: + logger.error("状态机异常 (state=%s): %s", self.state.name, e, + exc_info=True) + + # ================================================================ + # 位置估计 + # ================================================================ + + def _update_local_position(self, state): + """通过速度积分更新本地 NED 位置估计""" + try: + drone = state["drone"] + now = time.time() + + if not self._home_set: + self._est_x = 0.0 + self._est_y = 0.0 + self._pos_time = now + self._home_set = True + logger.info("[位置] 初始位置记录完成") + + dt = now - self._pos_time + if 0 < dt < self.MAX_POSITION_DT: + vx = drone.get("vx", 0.0) + vy = drone.get("vy", 0.0) + self._est_x += vx * dt + self._est_y += vy * dt + # dt ≥ MAX_POSITION_DT 时跳过积分防跳变 + + self._pos_time = now + except (KeyError, TypeError) as e: + logger.warning("[位置] 更新异常: %s", e) + + def _get_current_z(self, state): + """从 STATE 获取当前 NED Z（负值向上）""" + try: + return -state["drone"].get("alt_rel", 0.0) + except (KeyError, TypeError): + return self.target_altitude + + # ================================================================ + # 状态处理器 + # ================================================================ + + # ---- 初始化 ---- + + def _handle_init(self, state): + """ + [INIT] 等待飞控连接，判断是否需要执行完整起飞序列 + + 三种分支： + A) 已解锁 + OFFBOARD → 直接跳 NAVIGATING + B) 未解锁 → 发 ARM 指令 + C) 已解锁但非 OFFBOARD → 进入 ARMING 等待后续 TAKEOFF 序列 + """ + try: + drone = state["drone"] + armed = drone.get("armed", False) + mode = drone.get("mode", "") + alt_rel = drone.get("alt_rel", 0.0) + + logger.info("[INIT] mode=%s armed=%s alt_rel=%.1fm", + mode, armed, alt_rel) + + # A) 已在 OFFBOARD + 已解锁 → 直接巡航 + if armed and mode == "OFFBOARD": + logger.info("[INIT] 已在 OFFBOARD，直接进入航点巡航") self.state = MissionState.NAVIGATING self.current_wp_index = 0 - - # 4. NAVIGATING: 航点导航巡航 - elif self.state == MissionState.NAVIGATING: - if not self.waypoints or self.current_wp_index >= len(self.waypoints): - # 航点全部飞完 - if self.return_to_home: - print("\n[巡航完成] 所有航点已巡视完毕。正在返回起始点上方...") - self.state = MissionState.RETURNING - else: - print("\n[巡航完成] 所有航点已巡视完毕。根据配置切换至末端就地悬停...") - self.state = MissionState.HOLD_FINAL + self._set_current_target_from_wp(0) return - # 获取当前目标航点 - wp = self.waypoints[self.current_wp_index] - self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw']) - - # 计算水平及高度距离 - dx = wp['x'] - self.drone.local_position[0] - dy = wp['y'] - self.drone.local_position[1] - dz = wp['z'] - self.drone.local_position[2] - dist_xyz = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2) - - print( - f"\n\r[巡航中] 前往航点 [{self.current_wp_index + 1}/{len(self.waypoints)}] | 距离目标: {dist_xyz:.2f}m", - end="", flush=True) - - if dist_xyz < self.arrival_radius: - print(f"\n[到达航点 {self.current_wp_index + 1}] 进入原地悬停与视觉检索阶段...") - self.state = MissionState.HOLD_TASK - self.hold_start_time = time.time() - - # 5. HOLD_TASK: 抵达巡点后的搜索悬停阶段 - elif self.state == MissionState.HOLD_TASK: - # 优先判定视觉：如果在巡点悬停搜寻期间，视觉发现了目标 - if target_detected: - print("\n🎯 [目标锁定] 视觉检测到目标！立刻中断悬停，切入 [视觉追踪] 状态！") - self.state = MissionState.VISUAL_TRACKING - self.tracking_start_time = time.time() + # B) 未解锁 + if not armed: + logger.info("[INIT] 执行解锁...") + ok, ack = self.proxy.send_command("ARM") + if ok: + self.state = MissionState.ARMING + self._state_start_time = time.time() + else: + logger.error("[INIT] ARM 失败: %s", ack) return - # 如果没有发现目标，则保持航点位置，并检查悬停是否超时 - wp = self.waypoints[self.current_wp_index] - self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw']) + # C) 已解锁但非 OFFBOARD + if armed and mode != "OFFBOARD": + logger.info("[INIT] 已解锁但 mode=%s，进入起飞序列", mode) + self.state = MissionState.ARMING + self._state_start_time = time.time() - elapsed_time = time.time() - self.hold_start_time - print(f"\r[检索中] 保持航点中... 剩余搜寻时间: {max(0.0, self.hold_duration - elapsed_time):.1f}s", end="", - flush=True) + except (KeyError, TypeError) as e: + logger.warning("[INIT] 解析异常: %s", e) - if elapsed_time >= self.hold_duration: - print(f"\n[未发现目标] 搜寻超时。放弃当前点，前往下一个航点...") - self.current_wp_index += 1 - self.state = MissionState.NAVIGATING + # ---- 解锁 ---- - # 6. VISUAL_TRACKING: 🎯 视觉闭环追踪状态（核心新增） - elif self.state == MissionState.VISUAL_TRACKING: - # 在此状态下，高层状态机【不主动发送】绝对位置坐标，把控制权完全让渡给主循环的 PID 速度控制 - # 我们只需要在这里进行任务结束判定 - print(f"\r[追踪中] 正在执行AI视觉闭环精准追踪... 计时: {time.time() - self.tracking_start_time:.1f}s", - end="", flush=True) + def _handle_arming(self, state): + """ + [ARMING] 等待解锁确认，成功后进入 TAKEOFF + """ + try: + armed = state["drone"].get("armed", False) - if self._is_tracking_task_complete(): - print("\n✅ [任务完成] 当前目标追踪任务圆满完成！重置追踪器，继续执行航点巡航...") - self.current_wp_index += 1 # 切换到下一个航点 - self.state = MissionState.NAVIGATING - - # 7. RETURNING: 自动返航至起始点上方 - elif self.state == MissionState.RETURNING: - self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0) - - dist_to_home = math.sqrt((self.home_x - self.drone.local_position[0]) ** 2 + - (self.home_y - self.drone.local_position[1]) ** 2) - print(f"\r[返航中] 正在飞回起始点位置 | 剩余水平距离: {dist_to_home:.2f}m", end="", flush=True) - - if dist_to_home < self.arrival_radius: - print("\n[到达] 已安全返回起始点上方。正在下发自动降落指令 (LAND)...") - if self.drone.set_mode('LAND'): - self.state = MissionState.LANDING + if not armed: + elapsed = time.time() - self._state_start_time + if elapsed > self.ARM_TIMEOUT: + logger.warning("[ARMING] 超时 (%.1fs)，重试 ARM", elapsed) + self.proxy.send_command("ARM") + self._state_start_time = time.time() else: - print("⚠️ 降落模式切换失败，正在重新下发...") + logger.info("[ARMING] 等待解锁... (%.1fs)", elapsed) + return - # 8. LANDING: 自动降落中 - elif self.state == MissionState.LANDING: - print(f"\r[降落中] 当前高度: {self.drone.local_position[2]:+.2f}m | 解锁状态: {self.drone.is_armed}", - end="", flush=True) - if not self.drone.is_armed: - print("\n[完结] 无人机已安全着陆并自动上锁。整个任务流结束。") + # ---- 解锁成功 → 起飞 ---- + takeoff_alt = -self.target_altitude # 正值（协议 altitude 为正） + logger.info("\n[解锁成功] 发送起飞指令 (alt=%.1fm)", takeoff_alt) + + ok, ack = self.proxy.send_waypoint( + action="TAKEOFF", alt=takeoff_alt, + alt_frame="RELATIVE", speed=2.0, + ) + if ok: + self.state = MissionState.TAKEOFF + self._state_start_time = time.time() + else: + logger.warning("[TAKEOFF] WAYPOINT 失败: %s，尝试 SETPOINT 爬升", ack) + ok2, _ = self.proxy.send_setpoint( + x=0, y=0, z=self.target_altitude, yaw=0.0, + control_mode="POSITION", + ) + if ok2: + self.state = MissionState.TAKEOFF + self._state_start_time = time.time() + else: + logger.error("[TAKEOFF] 降级爬升也失败") + + except (KeyError, TypeError) as e: + logger.warning("[ARMING] 异常: %s", e) + + # ---- 起飞 ---- + + def _handle_takeoff(self, state): + """ + [TAKEOFF] 持续发送 POSITION SETPOINT 爬升，到达后进入 OFFBOARD + + 流程： + ① 每帧发送 POSITION 悬停/爬升 SETPOINT（预热 + 保持流） + ② 高度到达目标 90% → 发送 OFFBOARD 指令 + ③ 模式切换确认 → 跳转到 NAVIGATING + """ + try: + drone = state["drone"] + alt_rel = drone.get("alt_rel", 0.0) + mode = drone.get("mode", "") + target_up = -self.target_altitude # 正值 + + # ---- ① 持续发送 POSITION SETPOINT（预热 OFFBOARD 流） ---- + # 通过 MissionManager 自有 proxy 发送，此阶段 UAVControlLoop + # 的 20Hz 线程会被 run_mission.py 关闭，不会冲突。 + # NED 坐标系：x=0,y=0,z=target_altitude 表示飞到正上方目标高度 + self.proxy.send_setpoint( + x=0, y=0, z=self.target_altitude, yaw=0.0, + control_mode="POSITION", + ) + + # ---- ② 到达目标高度 → 进入 OFFBOARD ---- + reached_alt = alt_rel >= target_up * 0.9 + + if reached_alt and mode != "OFFBOARD": + logger.info("[TAKEOFF] 高度 %.1f/%.1f，切换 OFFBOARD...", + alt_rel, target_up) + # 先预热悬停（协议 §3.3.1 ③→④） + self.proxy.send_setpoint( + x=0, y=0, z=-alt_rel, yaw=0.0, + control_mode="POSITION", + ) + time.sleep(0.2) + + ok, ack = self.proxy.send_command("OFFBOARD") + if ok: + # 等待模式切换 + for _ in range(int(self.OFFBOARD_TIMEOUT / 0.1)): + st = self.proxy.get_latest_state() + if st and st["drone"]["mode"] == "OFFBOARD": + logger.info("[TAKEOFF] 已进入 OFFBOARD") + break + time.sleep(0.1) + else: + logger.warning("[TAKEOFF] OFFBOARD 切换超时") + else: + logger.error("[TAKEOFF] OFFBOARD 指令失败: %s", ack) + + # ---- ③ 高度到位 + OFFBOARD 模式 → 跳转巡航 ---- + if reached_alt and mode == "OFFBOARD": + logger.info("\n[起飞完成] 高度 %.1fm，进入航点巡航", alt_rel) + self.state = MissionState.NAVIGATING + self.current_wp_index = 0 + self._set_current_target_from_wp(0) + return + + # ---- 爬升超时保护 ---- + elapsed = time.time() - self._state_start_time + if elapsed > self.TAKEOFF_TIMEOUT: + logger.warning("[TAKEOFF] 超时 (%.1fs)，强行进入巡航", elapsed) + self.state = MissionState.NAVIGATING + self.current_wp_index = 0 + self._set_current_target_from_wp(0) + return + + logger.info("[起飞中] alt=%.1f/%.1f mode=%s 耗时=%.1fs", + alt_rel, target_up, mode, elapsed) + + except (KeyError, TypeError) as e: + logger.warning("[TAKEOFF] 异常: %s", e) + + # ---- 航点导航 ---- + + def _handle_navigating(self, state): + """ + [NAVIGATING] 每帧发送 POSITION SETPOINT 前往当前航点 + + 到达判定后推进索引；所有航点飞完后根据 return_to_home + 决定返航或悬停。 + """ + if not self.waypoints or self.current_wp_index >= len(self.waypoints): + self._finish_navigation() + return + + wp = self.waypoints[self.current_wp_index] + wp_x = wp["x"] + wp_y = wp["y"] + wp_z = wp.get("z", self.target_altitude) + wp_yaw = wp.get("yaw", 0.0) + + # ---- 发送 POSITION SETPOINT（通过自有 proxy） ---- + # 此 SETPOINT 让 PX4 飞向目标航点 + self.proxy.send_setpoint( + x=wp_x, y=wp_y, z=wp_z, yaw=wp_yaw, + control_mode="POSITION", + ) + + # ---- 共享目标 ---- + self.current_target = {"x": wp_x, "y": wp_y, "z": wp_z, "yaw": wp_yaw} + + # ---- 到达判定 ---- + current_z = self._get_current_z(state) + dist = _dist_3d(self._est_x, self._est_y, current_z, + wp_x, wp_y, wp_z) + + if dist < self.arrival_radius: + logger.info("\n✅ [到达航点 %d/%d] dist=%.2fm", + self.current_wp_index + 1, len(self.waypoints), dist) + self.state = MissionState.HOLD_TASK + self.hold_start_time = time.time() + else: + logger.info("[巡航中] 航点 [%d/%d] 距离=%.2fm", + self.current_wp_index + 1, len(self.waypoints), dist) + + def _finish_navigation(self): + """所有航点飞完后的跳转""" + if self.return_to_home: + logger.info("\n[巡航完成] 返回起始点...") + self.state = MissionState.RETURNING + self.current_target = {"x": 0, "y": 0, + "z": self.target_altitude, "yaw": 0} + else: + logger.info("\n[巡航完成] 末端悬停...") + self.state = MissionState.HOLD_FINAL + self._state_start_time = time.time() + + # ---- 悬停搜索 ---- + + def _handle_hold_task(self, state, target_detected): + """ + [HOLD_TASK] 抵达航点后的悬停搜索 + + 策略： + - 检测到目标 → VISUAL_TRACKING + - 超时无目标 → 下一个航点 + - 持续发送 POSITION SETPOINT 悬停 + """ + # ---- 发现目标 ---- + if target_detected: + logger.info("\n🎯 [目标锁定] 切入视觉追踪状态！") + self.state = MissionState.VISUAL_TRACKING + self.tracking_start_time = time.time() + self._target_lost_start = None + self.tracking_completed = False + return + + # ---- 保持当前位置悬停 ---- + wp = self.waypoints[self.current_wp_index] + self.proxy.send_setpoint( + x=wp["x"], y=wp["y"], + z=wp.get("z", self.target_altitude), + yaw=wp.get("yaw", 0), + control_mode="POSITION", + ) + self.current_target = { + "x": wp["x"], "y": wp["y"], + "z": wp.get("z", self.target_altitude), + "yaw": wp.get("yaw", 0), + } + + # ---- 超时判定 ---- + elapsed = time.time() - self.hold_start_time + if elapsed >= self.hold_duration: + logger.info("\n[未发现目标] 搜寻超时 (%.1fs)，前往下一航点", + self.hold_duration) + self.current_wp_index += 1 + self.state = MissionState.NAVIGATING + else: + logger.info("[检索中] 航点 %d 剩余 %.1fs", + self.current_wp_index + 1, + self.hold_duration - elapsed) + + # ---- 视觉追踪 ---- + + def _handle_visual_tracking(self, state, target_detected): + """ + [VISUAL_TRACKING] 🎯 完全让渡控制权给 UAVControlLoop + + 【核心设计】 + MissionManager 在此状态下 **不发送任何 SETPOINT**。 + UAVControlLoop (20Hz 独立线程) 通过其自有 proxy 发送 + VELOCITY SETPOINT 进行视觉闭环追踪。 + + 本模块仅负责追踪退出条件的判定和状态切换。 + + 【退出条件】 + 1. self.tracking_completed == True（外部模块设置） + 2. 目标丢失超过 target_lost_timeout 秒 + 3. 追踪总时长超过 tracking_max_duration（兜底） + """ + # ---- 不发送任何 SETPOINT — 控制权已让渡 ---- + + tracking_elapsed = time.time() - self.tracking_start_time + + # ---- 兜底超时 ---- + if tracking_elapsed > self.tracking_max_duration: + logger.info("\n⏰ [追踪超时] %.1fs ≥ %.1fs", + tracking_elapsed, self.tracking_max_duration) + self._exit_tracking() + return + + # ---- 外部完成信号 ---- + if self.tracking_completed: + logger.info("\n✅ [追踪完成] 外部模块报告完成，返回巡航") + self.tracking_completed = False + self._exit_tracking() + return + + # ---- 目标丢失超时 ---- + if not target_detected: + if self._target_lost_start is None: + self._target_lost_start = time.time() + else: + lost_elapsed = time.time() - self._target_lost_start + if lost_elapsed >= self.target_lost_timeout: + logger.info("\n⏰ [丢失超时] %.1fs，放弃追踪", + lost_elapsed) + self._exit_tracking() + return + logger.info("[追踪中] 丢失 %.1fs/%.1fs", + lost_elapsed, self.target_lost_timeout) + else: + self._target_lost_start = None + + logger.info("[追踪中] 进行中... %.1fs", tracking_elapsed) + + def _exit_tracking(self): + """退出 VISUAL_TRACKING 的统一出口""" + self.current_wp_index += 1 + self.state = MissionState.NAVIGATING + self._target_lost_start = None + logger.info("[状态切换] VISUAL_TRACKING → NAVIGATING，" + "前往航点 %d", self.current_wp_index + 1) + + # ---- 返航 ---- + + def _handle_returning(self, state): + """ + [RETURNING] 发送 POSITION SETPOINT 到起始点 (0,0,z) + """ + self.proxy.send_setpoint( + x=0, y=0, z=self.target_altitude, yaw=0, + control_mode="POSITION", + ) + self.current_target = {"x": 0, "y": 0, + "z": self.target_altitude, "yaw": 0} + + current_z = self._get_current_z(state) + dist = _dist_3d(self._est_x, self._est_y, current_z, + 0.0, 0.0, self.target_altitude) + logger.info("[返航中] 距离=%.2fm", dist) + + if dist < self.arrival_radius: + logger.info("\n[到达] 已返回起始点，发送 LAND 指令...") + ok, ack = self.proxy.send_command("LAND") + if ok: + self.state = MissionState.LANDING + else: + logger.warning("[返航] LAND 失败: %s", ack) + + # ---- 降落 ---- + + def _handle_landing(self, state): + """ + [LANDING] 监控 disarm 状态判断着陆 + """ + try: + armed = state["drone"].get("armed", False) + alt_rel = state["drone"].get("alt_rel", 0.0) + logger.info("[降落中] 高度=%.1fm 已解锁=%s", + alt_rel, not armed) + + if not armed: + logger.info("\n[完结] 已着陆上锁，任务结束") self.state = MissionState.FINISHED + except (KeyError, TypeError) as e: + logger.warning("[LANDING] 异常: %s", e) - # 9. HOLD_FINAL: 末端就地悬停 - elif self.state == MissionState.HOLD_FINAL: - final_wp = self.waypoints[-1] if self.waypoints else {'x': self.home_x, 'y': self.home_y, - 'z': self.target_altitude, 'yaw': 0} - self.drone.send_target_position(final_wp['x'], final_wp['y'], final_wp['z'], final_wp['yaw']) - print( - f"\r[挂起] 无人机保持在最终航点悬停。坐标:({final_wp['x']}, {final_wp['y']}, {final_wp['z']}) | 等待人工接管中...", - end="", flush=True) \ No newline at end of file + # ---- 末端悬停 ---- + + def _handle_hold_final(self, state): + """ + [HOLD_FINAL] 末端悬停，保持最终航点位置 + """ + if self.waypoints: + final_wp = self.waypoints[-1] + else: + final_wp = {"x": 0, "y": 0, + "z": self.target_altitude, "yaw": 0} + + self.proxy.send_setpoint( + x=final_wp["x"], y=final_wp["y"], + z=final_wp.get("z", self.target_altitude), + yaw=final_wp.get("yaw", 0), + control_mode="POSITION", + ) + self.current_target = { + "x": final_wp["x"], "y": final_wp["y"], + "z": final_wp.get("z", self.target_altitude), + "yaw": final_wp.get("yaw", 0), + } + logger.info("[挂起] 保持 (%.1f, %.1f, %.1f) 等待接管", + final_wp["x"], final_wp["y"], + final_wp.get("z", self.target_altitude)) + + # ================================================================ + # 辅助 + # ================================================================ + + def _set_current_target_from_wp(self, index): + """从航点列表更新 current_target""" + if self.waypoints and index < len(self.waypoints): + wp = self.waypoints[index] + self.current_target = { + "x": wp["x"], "y": wp["y"], + "z": wp.get("z", self.target_altitude), + "yaw": wp.get("yaw", 0), + } diff --git a/run_mission.py b/run_mission.py index 46b387a..42ba1fe 100644 --- a/run_mission.py +++ b/run_mission.py @@ -1,55 +1,426 @@ +""" +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 推理 + └─────────────────────────────┘ + +【控制权协调规则】 + 同一时刻只有一个人持有「SETPOINT 发令权」： + - VISUAL_TRACKING 之外 → MissionManager 发 POSITION SETPOINT + - VISUAL_TRACKING 之中 → UAVControlLoop 线程发 VELOCITY SETPOINT + - 切换由主循环检测 MissionManager.state 变化时触发 + +【安全设计】 + - try / except FailsafeTriggered / finally 全面资源释放 + - 两个独立 RouterProxy，避免 REQ socket 线程竞争 + - 控制线程启停由状态机状态变化驱动，逻辑可预测 + +【可扩展的图像输入接口】 + 如需替换视频源（如模拟器/录播文件），替换 VideoStreaming 实例 + 即可，保持 read_frame() → (ret, frame) 的协议不变。 +""" + +import os +import sys import time import json -from drone_controller.base_control import DroneController -from drone_controller.mission_manager import MissionManager, FailsafeTriggered +import logging +import threading -# 1. 定义室内台架或室内试验场的相对局部航点 (X=北, Y=东, Z=地, yaw=偏航角) -# 注意：NED 坐标系下，Z轴向上为负数！ -flight_waypoints = [ - {'x': 1.0, 'y': 0.0, 'z': -1.5, 'yaw': 0}, # 航点 1：向前 1 米，维持高度 1.5 米 - {'x': 1.0, 'y': 1.0, 'z': -1.5, 'yaw': 90}, # 航点 2：向右平移 1 米，机头转向正东 - {'x': 0.0, 'y': 0.0, 'z': -1.2, 'yaw': 0}, # 航点 3：回到原点上方，高度降到 1.2 米 -] +import cv2 -def load_config(config_path="./config.json"): - with open(config_path, "r", encoding="utf-8") as f: - return json.load(f) +# ---- ZMQ Router 代理与状态机 ---- +from drone_controller.router_proxy import RouterProxy +from drone_controller.mission_manager import ( + MissionManager, + MissionState, + FailsafeTriggered, +) +# ---- 视觉闭环控制（复用 infer_camera_modular 现有实现） ---- +# 注意：此处不修改 UAVControlLoop 任何内部逻辑。 +# run_mission.py 在外部协调其控制线程的启停。 +from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer + +logger = logging.getLogger("RunMission") + + +class MissionOrchestrator: + """ + 🎯 全任务编排器 + + 职责： + 1. 创建并初始化所有子系统 + 2. 在主循环中协调 MissionManager 状态机与 UAVControlLoop 的控制权 + 3. 处理 Failsafe 熔断、键盘中断等异常退出 + 4. 提供标准视频源接口（可替换） + """ + + def __init__(self, config_path="./config.json"): + self.logger = logging.getLogger("Orchestrator") + self.logger.info("正在加载配置: %s", config_path) + + with open(config_path, "r", encoding="utf-8") as f: + cfg = json.load(f) + self.cfg = cfg + + # ---- 感知子系统 ---- + self.streamer = VideoStreaming(cfg["video"], cfg["network"]) + self.detector = YOLO26UAVInfer(cfg["model"]) + + # ---- 航线定义（NED 坐标系，Z 向上为负） ---- + self.waypoints = [ + {'x': 1.0, 'y': 0.0, 'z': -1.5, 'yaw': 0}, + {'x': 1.0, 'y': 1.0, 'z': -1.5, 'yaw': 90}, + {'x': 0.0, 'y': 0.0, 'z': -1.2, 'yaw': 0}, + ] + + # ================================================================ + # 🚁 MissionManager（状态机 + 航点巡航） + # ================================================================ + # 拥有独立的 RouterProxy，用于读取 STATE 和发送 COMMAND / + # WAYPOINT / POSITION SETPOINT。 + fc = cfg["flight_control"] + self.mission_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 = MissionManager( + proxy=self.mission_proxy, + waypoints=self.waypoints, + target_altitude=1.5, + arrival_radius=0.3, + hold_duration=8.0, + return_to_home=False, + ) + + # ================================================================ + # 🛸 UAVControlLoop（20Hz VELOCITY 闭环追踪） + # ================================================================ + # 拥有独立的 RouterProxy。其 20Hz 控制线程仅在 VISUAL_TRACKING + # 期间运行，其余时间处于停止状态，避免与 MissionManager 冲突。 + self.controller = UAVControlLoop( + cfg["flight_control"], cfg["pid_y"], cfg["pid_z"], + ) + self._control_active = False # 控制线程运行标志 + + # ---- 主循环参数 ---- + self.running = False + self.loop_interval = 0.05 # 20Hz + + self.logger.info("MissionOrchestrator 初始化完成") + + # ================================================================ + # 🔌 启动 + # ================================================================ + + def start(self): + """启动所有子系统""" + 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 已启动") + + # 3. 等待收到第一条 Router STATE + self._wait_for_initial_state() + + self.running = True + self.logger.info("🚀 所有子系统就绪，主循环启动") + + def _wait_for_initial_state(self, timeout=5.0): + """等待 Router 推送第一条 STATE""" + deadline = time.time() + timeout + while time.time() < deadline: + state = self.mission_proxy.get_latest_state() + if state is not None: + drone = state.get("drone", {}) + self.logger.info( + "✔ 收到 Router 状态: mode=%s armed=%s alt=%.1fm", + drone.get("mode"), drone.get("armed"), + drone.get("alt_rel", 0), + ) + return True + time.sleep(0.1) + self.logger.warning("⚠ 未收到 Router 状态 (%ds 超时)，继续启动...", timeout) + return False + + # ================================================================ + # 🔁 主循环 + # ================================================================ + + def run(self): + """主业务循环（20Hz）""" + self.logger.info("主循环开始 (20Hz)") + + prev_state = MissionState.INIT + loop_count = 0 + + try: + while self.running: + loop_start = time.time() + + # ---- ① 读取视频帧 ---- + ret, frame = self.streamer.read_frame() + if not ret: + # 无新帧时短暂等待，不浪费 CPU + time.sleep(0.005) + continue + + loop_count += 1 + orig_shape = frame.shape[:2] + + # ---- ② NPU 推理 + 后处理 ---- + input_tensor, ratio, dwdh = self.detector.preprocess(frame) + outputs = self.detector.session.infer([input_tensor]) + detections = self.detector.postprocess( + outputs, ratio, dwdh, orig_shape, + ) + target_detected = len(detections) > 0 + + # ---- ③ 控制权协调 ---- + # 根据当前状态机 state 决定 UAVControlLoop 控制线程启停 + current_state = self.mission.state + self._coordinate_control(current_state, prev_state, + detections, orig_shape) + prev_state = current_state + + # ---- ④ 状态机更新 ---- + self.mission.update(target_detected=target_detected) + + # ---- ⑤ HUD 渲染 + 图传 ---- + frame = self._render_hud(frame, loop_count, detections) + self.streamer.push_to_stream(frame) + + # ---- 频率控制 ---- + elapsed = time.time() - loop_start + sleep_time = self.loop_interval - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + except FailsafeTriggered as e: + # Failsafe 熔断：安全员切出手动模式 → 程序安全退出 + self.logger.warning("🚨 [Failsafe] %s", e) + except KeyboardInterrupt: + self.logger.info("⌨ 用户键盘中断，正在安全退出...") + except Exception as e: + self.logger.exception("💥 主循环异常: %s", e) + finally: + self.shutdown() + + # ================================================================ + # 控制权协调 + # ================================================================ + + def _coordinate_control(self, current_state, prev_state, + detections, orig_shape): + """ + 控制权协调：基于状态机状态启停 UAVControlLoop 线程 + + ┌──────────────┬─────────────────────┬──────────────────────┐ + │ 状态机状态 │ MissionManager │ UAVControlLoop │ + ├──────────────┼─────────────────────┼──────────────────────┤ + │ 非 TRACKING │ proxy → POSITION │ 控制线程已停止 │ + │ │ SETPOINT │ │ + ├──────────────┼─────────────────────┼──────────────────────┤ + │ TRACKING │ 不发 SETPOINT │ 20Hz VELOCITY 线程 │ + │ │ (控制权已让渡) │ TargetTracker + PID │ + └──────────────┴─────────────────────┴──────────────────────┘ + """ + is_tracking = current_state == MissionState.VISUAL_TRACKING + was_tracking = prev_state == MissionState.VISUAL_TRACKING + + if is_tracking: + # ---- 进入 VISUAL_TRACKING → 启动/保持控制线程 ---- + if not self._control_active: + self._start_control_thread() + + # 不断喂入检测数据（TargetTracker 在主线程更新） + self.controller.update_detections(detections, orig_shape) + + else: + # ---- 非 VISUAL_TRACKING → 停止控制线程 ---- + if self._control_active: + self._stop_control_thread() + + # 非追踪期间也喂入空检测，保持数据结构新鲜 + self.controller.update_detections([], orig_shape) + + def _start_control_thread(self): + """启动 UAVControlLoop 的 20Hz VELOCITY 控制线程""" + self.logger.info("▶ [控制权] 启动 UAVControlLoop VELOCITY 线程") + + # 记录进入追踪时的飞机状态 + state = self.mission_proxy.get_latest_state() + if state: + d = state.get("drone", {}) + self.logger.info("追踪初始状态: mode=%s alt=%.1fm armed=%s", + d.get("mode"), d.get("alt_rel", 0), + d.get("armed")) + + # 直接复用 UAVControlLoop 已有的内部 _control_loop 方法 + # （不修改任何内部逻辑，仅从外部启动其线程） + self.controller.running = True + self.controller.control_thread = threading.Thread( + target=self.controller._control_loop, + daemon=True, + name="uav-vel-control", + ) + self.controller.control_thread.start() + self._control_active = True + + def _stop_control_thread(self): + """安全停止 UAVControlLoop 的控制线程""" + if not self._control_active: + return + + self.logger.info("⏹ [控制权] 停止 UAVControlLoop 线程") + self.controller.running = False + + if self.controller.control_thread: + self.controller.control_thread.join(timeout=2.0) + self.controller.control_thread = None + + # 复位 PID 积分项和跟踪器，避免下次启动时积分暴冲 + self.controller.pid_y.reset() + self.controller.pid_z.reset() + self.controller.tracker.reset() + self._control_active = False + self.logger.info("✔ 控制线程已停止，PID/跟踪器已重置") + + # ================================================================ + # 渲染 + # ================================================================ + + def _render_hud(self, frame, loop_count, detections): + """ + 在视频帧上叠加 HUD 信息 + + 包含：状态机阶段、航点索引、控制权归属、检测结果 + """ + # ---- 检测框 ---- + frame = self.detector.draw_boxes(frame, detections) + + # ---- 状态面板 ---- + state_name = self.mission.state.name + wp_text = f"WP: {self.mission.current_wp_index + 1}/{len(self.waypoints)}" + ctrl_text = "CTRL:VELOCITY" if self._control_active else "CTRL:POSITION" + + cv2.putText(frame, f"State: {state_name} | {wp_text}", + (20, 40), cv2.FONT_HERSHEY_COMPLEX, 0.6, + (0, 255, 255), 2) + cv2.putText(frame, ctrl_text, + (20, 65), cv2.FONT_HERSHEY_SIMPLEX, 0.5, + (0, 255, 0) if self._control_active else (255, 255, 0), 2) + + # ---- 每 100 帧打印详细状态日志 ---- + if loop_count % 100 == 0: + state = self.mission_proxy.get_latest_state() + if state: + d = state.get("drone", {}) + self.logger.info( + "📡 mode=%s armed=%s alt=%.1fm spd=%.1f bat=%.0f%% " + "%s %s detect=%d", + d.get("mode"), d.get("armed"), + d.get("alt_rel", 0), d.get("ground_speed", 0), + d.get("battery", 0), + state_name, ctrl_text, + len(detections), + ) + + return frame + + # ================================================================ + # 🧹 资源释放 + # ================================================================ + + def shutdown(self): + """ + 安全关闭所有子系统 + + 释放顺序（逆初始化顺序）： + 1. 停止控制线程 + 2. 关闭 MissionManager 的 ZMQ 代理 + 3. 关闭 UAVControlLoop 的 ZMQ 代理 + 4. 释放视频流资源 + """ + self.logger.info("====== 安全关闭所有子系统 ======") + self.running = False + + # 1. 停止控制线程 + self._stop_control_thread() + + # 2. 关闭 MissionManager 的 Proxy + try: + self.mission_proxy.close() + self.logger.info("✔ MissionManager Proxy 已关闭") + except Exception as e: + self.logger.warning("mission_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. 释放视频资源 + try: + self.streamer.release() + self.logger.info("✔ VideoStreamer 已释放") + except Exception as e: + self.logger.warning("streamer release 异常: %s", e) + + self.logger.info("🏁 所有资源已安全释放，程序退出") + + +# ================================================================ +# 入口 +# ================================================================ def main(): - # 加载配置 - config = load_config() - cfg = config["flight_control"] - # 2. 初始化底层 SDK 驱动 - uav = DroneController(connection_string=cfg.get("connection_string"), baud=cfg.get("baud_rate")) - if not uav.connect(): - print("物理串口打开失败，请检查数传连接或端口权限。") - return + """主入口：日志配置 → 编排器创建 → 启动 → 运行""" + # 创建日志目录 + os.makedirs("./log", exist_ok=True) - # 3. 实例化任务管理器 (设置到达半径为0.3米，每个点悬停3秒) - manager = MissionManager(drone=uav, waypoints=flight_waypoints, target_altitude=1.5, arrival_radius=0.3, - hold_duration=3.0) + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s - %(name)s - %(levelname)s - %(message)s", + handlers=[ + logging.StreamHandler(), + logging.FileHandler("./log/mission_run.log", mode="a"), + ], + ) - print("=================== 巡航控制系统启动 ===================") - try: - # 高频业务控制主循环 (10Hz) - while True: - # 轮询状态机 - is_finished = manager.update() - if is_finished: - print("=================== 整个巡航任务已安全退出 ===================") - break + # 允许命令行指定配置文件路径 + config_file = sys.argv[1] if len(sys.argv) > 1 else "./config.json" - time.sleep(0.1) # 严格的 10Hz 轮询，配合管理器内部 5Hz 下发，完美防失控 - - except FailsafeTriggered as e: - # 4. 最强安全保护：一旦遥控器切出模式，代码在这里捕获，直接结束整个 Python 进程 - print(f"\n🚨 [主循环紧急熔断] 系统安全退出: {e}。香橙派已处于安全挂起状态，不占用串口输出。") - except KeyboardInterrupt: - print("\n用户手动通过 Ctrl+C 终止了上位机脚本。") - finally: - uav.running = False # 销毁并关闭底层遥测监听守护线程 + orchestrator = MissionOrchestrator(config_path=config_file) + orchestrator.start() + orchestrator.run() -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/start_npu_infer.sh b/start_npu_infer.sh index afee5a3..07cd54f 100644 --- a/start_npu_infer.sh +++ b/start_npu_infer.sh @@ -20,7 +20,7 @@ conda activate base # ==================================================================== WORK_DIR="/home/HwHiAiUser/opi-yolo" -PYTHON_SCRIPT="infer_camera_direct.py" +PYTHON_SCRIPT="infer_camera_modular.py" LOG_FILE="/tmp/npu_infer.log" PYTHON_BIN="python3"