# 多模态威胁融合系统 — 使用说明

## 概述

多模态威胁融合系统将热成像、闪光检测、声源检测三条感知管道的输出进行融合，生成综合威胁评估并输出避障指令。

系统提供两个版本：
- **C++ 版** (`cpp/`) — 直接编译运行，集成 Prometheus uav_control 框架，适用于 P600 无人机实机部署
- **Python 版** (`threat_fusion_node.py`) — 早期 ROS 1 原型，通过 subprocess 调用热成像检测器

## 系统架构

```
                          ┌─────────────────────────────────────────────┐
                          │         threat_fusion (C++ ROS 节点)        │
                          │                                             │
  SpireCV 吊舱 ──Image──► │  ThermalBridge (直接 C++ 调用检测器)        │
                          │         │                                   │
  闪光检测节点 ──ROS───►  │  ThreatCache (时间窗口缓存)                │
                          │         │                                   │
  声源检测节点 ──ROS───►  │  FusionEngine (融合引擎)                   │
                          │         │                                   │
  uav_control ──State───► │  AvoidanceController (避障控制器)           │
  (UAVState)              │         │                                   │
                          │  GimbalController (AT10 云台追踪)          │
                          └────┬──────────┬──────────┬────────────────┘
                               │          │          │
                    UAVCommand │   状态/威胁  TextInfo
                    (ENU 速度) │          │        (地面站)
                               ▼          ▼          ▼
                     Prometheus       rosbridge    QGC
                     uav_control      → 浏览器
                     → PX4
```

### Prometheus uav_control 框架集成

本系统通过 Prometheus `prometheus_msgs` 与 `uav_control` 模块通信，遵循其话题命名空间和控制接口规范：

```
/uav{N}/prometheus/state         → 读取无人机状态 (UAVState)
/uav{N}/prometheus/command       → 发送避障速度指令 (UAVCommand)
/uav{N}/prometheus/control_state → 读取控制状态机 (UAVControlState)
/uav{N}/prometheus/text_info     → 地面站告警消息 (TextInfo)
```

## 机载电脑部署指南 (Jetson Orin Nano)

### 目录结构

在机载电脑上，代码应部署到 Prometheus catkin 工作空间的以下位置：

```
~/Prometheus/
├── Modules/
│   ├── uav_control/                    # Prometheus 核心控制模块 (已存在)
│   │   ├── src/uav_control_node.cpp
│   │   ├── src/uav_controller.cpp
│   │   ├── src/uav_estimator.cpp
│   │   └── launch/
│   │       ├── uav_control_main_outdoor.launch    # P600 室外启动文件
│   │       └── uav_control_outdoor.yaml            # 室外参数配置
│   │
│   ├── common/
│   │   └── prometheus_msgs/            # Prometheus 自定义消息 (已存在)
│   │
│   ├── communication/                  # MAVLink 通信桥接 (已存在)
│   │
│   └── threat_fusion/                  # ★ 本系统 — 多模态威胁融合 ★
│       ├── CMakeLists.txt
│       ├── package.xml
│       ├── include/
│       │   ├── fusion_types.h          # 融合类型定义 (威胁等级/配置)
│       │   ├── fusion_engine.h         # 融合引擎
│       │   ├── threat_cache.h          # 时间窗口缓存
│       │   ├── avoidance_controller.h  # 避障控制器
│       │   └── gimbal_controller.h     # AT10 云台控制器
│       ├── src/
│       │   ├── main.cpp                # ROS 节点入口 (Prometheus 集成)
│       │   ├── fusion_engine.cpp       # 融合逻辑实现
│       │   ├── avoidance_controller.cpp # 避障算法实现
│       │   └── gimbal_controller.cpp   # 云台控制实现
│       ├── launch/
│       │   └── threat_fusion.launch    # 融合节点启动文件
│       └── config/
│           └── threat_fusion_params.yaml  # 参数配置
│
├── 热成像识别/                          # ★ 热成像检测模块 (依赖) ★
│   └── cpp/
│       ├── include/
│       │   ├── thermal_types.h
│       │   ├── thermal_detector.h
│       │   └── thermal_preprocessor.h
│       └── src/
│           ├── thermal_detector.cpp
│           └── thermal_preprocessor.cpp
│
├── Simulator/                          # Gazebo 仿真环境 (已存在)
├── Scripts/                            # 构建/测试脚本 (已存在)
└── compile_all.sh                      # 主构建脚本
```

### 安装步骤

#### 1. 复制代码到 catkin 工作空间

```bash
# 在机载电脑上
cd ~/Prometheus/Modules/

# 复制融合模块
cp -r /path/to/多模态融合/cpp/ threat_fusion/

# 复制热成像检测模块 (如果尚未存在)
mkdir -p 热成像识别/cpp
cp -r /path/to/热成像识别/cpp/ 热成像识别/
```

#### 2. 创建 ROS 包配置

如果 `threat_fusion` 还不是一个 catkin 包，创建 `package.xml`:

```xml
<package>
  <name>threat_fusion</name>
  <version>1.0.0</version>
  <description>多模态威胁融合 ROS 节点</description>
  <maintainer email="dev@example.com">developer</maintainer>
  <license>BSD</license>
  <buildtool_depend>catkin</buildtool_depend>
  <depend>roscpp</depend>
  <depend>std_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>diagnostic_msgs</depend>
  <depend>cv_bridge</depend>
  <depend>prometheus_msgs</depend>
  <depend>libopencv-dev</depend>
</package>
```

#### 3. 编译

```bash
cd ~/Prometheus
catkin_make --pkg threat_fusion

# 或使用 catkin build
catkin build threat_fusion
```

#### 4. 创建启动文件

`launch/threat_fusion.launch`:

```xml
<launch>
    <!-- 多模态威胁融合节点 -->
    <node pkg="threat_fusion" type="threat_fusion" name="threat_fusion"
          output="screen" respawn="true">
        <!-- 无人机编号 -->
        <param name="uav_id" value="1" />

        <!-- 模态开关 -->
        <param name="thermal_enabled" value="true" />
        <param name="flash_enabled" value="true" />
        <param name="acoustic_enabled" value="true" />

        <!-- 热成像相机参数 -->
        <param name="thermal_image_topic" value="/uav1/gimbal/image_original" />
        <param name="thermal_hfov" value="32.0" />

        <!-- 避障参数 -->
        <param name="avoidance_enabled" value="true" />
        <param name="avoidance_speed" value="2.0" />
        <param name="avoidance_decel_distance" value="10.0" />
        <param name="maximum_safe_vel_xy" value="5.0" />
        <param name="maximum_safe_vel_z" value="4.0" />

        <!-- AT10 吊舱参数 -->
        <param name="gimbal_port" value="/dev/ttyUSB1" />
        <param name="gimbal_kp" value="1.0" />
    </node>
</launch>
```

#### 5. 启动顺序

```bash
# 1. 启动 uav_control (必须先启动)
roslaunch prometheus_uav_control uav_control_main_outdoor.launch uav_id:=1 sim_mode:=false

# 2. 启动 SpireCV 吊舱服务
roslaunch spirecv at10_gimbal_server.launch

# 3. 启动融合节点
roslaunch threat_fusion threat_fusion.launch

# 4. (可选) 启动闪光/声源检测节点
rosrun flash_detector flash_detector_node
rosrun acoustic_analyzer acoustic_analyzer_node
```

## 接口适配说明

### 1. 避障指令接口

**原设计 (Python/MAVROS)**:
- 发布 `geometry_msgs/TwistStamped` 到 `/uav1/mavros/setpoint_velocity/cmd_vel`
- 直接控制 MAVROS 速度

**现设计 (C++/Prometheus)**:
- 发布 `prometheus_msgs/UAVCommand` 到 `/uav1/prometheus/command`
- 使用 `Move_mode = XYZ_VEL` (ENU 速度控制)
- `uav_control` 内部转换为 MAVROS `PositionTarget` 发送给 PX4
- 附加 `Command_ID` 单调递增，防止过期指令执行

```
UAVCommand:
  Agent_CMD = Move (4)
  Move_mode = XYZ_VEL (2)
  velocity_ref[0] = vx (East, m/s)
  velocity_ref[1] = vy (North, m/s)
  velocity_ref[2] = vz (Up, m/s)
  yaw_ref = 当前航向 (rad, 保持)
  Command_ID = 自增 ID
```

**适配要点**:
- 速度限制对齐 `uav_control_outdoor.yaml` 的 `maximum_safe_vel_xy/z`
- 地理围栏检查在发送前执行，防止指令超出安全区域
- 控制状态机须处于 `COMMAND_CONTROL(2)` 模式，指令才会被 `uav_control` 执行

### 2. 无人机状态接口

**原设计 (Python/MAVROS)**:
- 订阅 `/uav1/mavros/local_position/pose` (`geometry_msgs/PoseStamped`)
- 从四元数提取航向角

**现设计 (C++/Prometheus)**:
- 订阅 `/uav1/prometheus/state` (`prometheus_msgs/UAVState`)
- 直接读取 `position[0-2]` (ENU 位置)、`attitude[2]` (yaw rad)
- ENU yaw → 北基准顺时针: `heading = (90 - yaw_deg) % 360`

**适配要点**:
- UAVState 由 `uav_estimator` 统一发布，包含传感器融合后的位置/姿态
- 室外模式使用 GPS/RTK 定位 (location_source=4 或 5)
- 支持多种定位源自动切换 (GPS, RTK, VINS, FAST-LIO 等)

### 3. 热成像检测接口

**原设计 (Python)**:
- 通过 `subprocess` 调用 `thermal_detector.exe`，解析 JSON 输出
- 延迟高 (~500ms)、需要磁盘 I/O

**现设计 (C++)**:
- 直接调用 `ThermalPreprocessor::preprocess()` + `ThermalDetector::detect()`
- 纯内存操作，延迟低 (~50ms)
- 检测结果直接填充 `ThermalCacheEntry`，无需 JSON 序列化/反序列化

**适配要点**:
- 热成像检测源文件 (`thermal_detector.cpp`, `thermal_preprocessor.cpp`) 直接编译进融合节点
- CMakeLists.txt 中通过 `THERMAL_DETECTOR_DIR` 引用外部热成像模块路径
- 部署时确保两个模块在同一工作空间

### 4. 云台控制接口

**SpireCV 直接控制** (USE_SPIRECV 宏):
- 通过串口 (`/dev/ttyUSB1`) 直接控制 AT10 Pro 吊舱
- 使用 SpireCV SDK 的 `sv::Gimbal` API
- 适用于无 ROS 云台服务的场景

**SpireCV ROS 服务** (通过 ROS 话题):
- 订阅 `/uav1/gimbal/image_original` 获取吊舱图像
- 可通过 `prometheus_msgs/GimbalControl` 控制云台角度/速度

**适配要点**:
- P600 手册约定: pitch 下俯为正, yaw 左转为正
- 追踪参数: kp_gimbal=1.0, max_yaw_rate=40°/s
- 追踪时使用 `setAngleRateEuler()` 平滑旋转

### 5. 检测结果发布

**新增接口**: 将热成像检测结果发布为 `prometheus_msgs/MultiDetectionInfo`

```
MultiDetectionInfo:
  num_objs = 检测数量
  detect_or_track = 0 (检测模式)
  detection_infos[]:
    object_name = "Person" / "Hotspot" / "Barrel Overheat"
    pixel_position = [cx, cy]
    category = 1(Person), 2(Hotspot), 3(Barrel), 4(Weapon)
```

这使其他 Prometheus 模块 (如 `yolov5_tracking`) 可以复用检测结果进行目标追踪。

### 6. 地面站告警接口

**新增**: 通过 `prometheus_msgs/TextInfo` 向地面站发送威胁告警

```
TextInfo:
  MessageType = INFO(0) | WARN(1) | ERROR(2) | FATAL(3)
  Message = "ThreatFusion: WARNING | 2 threats | avoidance=ON"
```

告警等级映射:
- WARNING 威胁 → TextInfo.WARN
- DANGER 威胁 → TextInfo.ERROR
- CRITICAL 威胁 → TextInfo.FATAL

## 安全机制

### 地理围栏

避障指令在发送前检查地理围栏 (对齐 `uav_control_outdoor.yaml`):

```yaml
geo_fence:
  x_min: -1000.0, x_max: 1000.0
  y_min: -1000.0, y_max: 1000.0
  z_min: -50.0,   z_max: 1000.0
```

### 速度限制

避障速度限制在 `uav_control` 安全范围内:
- 水平: ±5.0 m/s (与 `maximum_safe_vel_xy` 对齐)
- 垂直: ±4.0 m/s (与 `maximum_safe_vel_z` 对齐)

### 控制状态检查

仅当无人机已解锁 (`UAVState.armed = true`) 时才发送避障指令。

## 坐标系

系统使用 ENU (East-North-Up) 坐标系，与 Prometheus uav_control 框架一致:

| 轴 | 方向 | 说明 |
|----|------|------|
| X | East (东) | ENU 水平 |
| Y | North (北) | ENU 水平 |
| Z | Up (上) | 垂直 |

航向约定:
- Prometheus `UAVState.attitude[2]`: yaw=0 指向东，逆时针为正 (rad)
- 融合引擎内部: heading=0 指向北，顺时针为正 (deg)
- 转换: `heading = (90 - yaw_rad × 180/π) % 360`

## 编译选项

| 选项 | 说明 | 默认 |
|------|------|------|
| `USE_ROS` | 启用 ROS 节点模式 (需要 catkin) | OFF |
| `USE_SPIRECV` | 启用 SpireCV 吊舱控制 (需要 SpireCV SDK) | OFF |

```bash
# 独立测试模式 (无 ROS)
mkdir build && cd build
cmake ..
cmake --build . --config Release

# ROS 节点模式
cd ~/Prometheus
catkin_make --pkg threat_fusion --cmake-args -DUSE_ROS=ON -DUSE_SPIRECV=ON

# 仅 ROS, 不含 SpireCV
catkin_make --pkg threat_fusion --cmake-args -DUSE_ROS=ON
```

## 独立测试 (无 ROS)

```bash
# 单图检测
./threat_fusion ../test/test_images/20260510170010_1.jpg --json --no-show

# 模拟融合
./threat_fusion ../test/test_images/20260510170010_1.jpg --simulate

# 云台测试 (需要 SpireCV)
./threat_fusion --gimbal-test

# 批量处理
./threat_fusion ../test/test_images/ --json --no-show
```

## 参数配置

### ROS 参数

| 参数 | 类型 | 默认值 | 说明 |
|------|------|--------|------|
| `~uav_id` | int | 1 | 无人机编号 (用于话题命名空间) |
| `~thermal_image_topic` | string | `/uav1/gimbal/image_original` | 热成像图像话题 |
| `~thermal_enabled` | bool | true | 启用热成像模态 |
| `~thermal_hfov` | float | 32.0 | 热成像水平视场角 (度) |
| `~flash_enabled` | bool | true | 启用闪光模态 |
| `~acoustic_enabled` | bool | true | 启用声源模态 |
| `~avoidance_enabled` | bool | true | 启用避障 |
| `~avoidance_speed` | float | 2.0 | 避障最大速度 (m/s) |
| `~avoidance_decel_distance` | float | 10.0 | 开始减速的距离 (m) |
| `~maximum_safe_vel_xy` | float | 5.0 | 最大安全水平速度 (m/s) |
| `~maximum_safe_vel_z` | float | 4.0 | 最大安全垂直速度 (m/s) |
| `~gimbal_port` | string | `/dev/ttyUSB1` | AT10 吊舱串口 |
| `~gimbal_kp` | float | 1.0 | 云台追踪比例增益 |

### 融合算法参数

| 参数 | 默认值 | 说明 |
|------|--------|------|
| 热成像权重 | 0.35 | 热成像检测置信度权重 |
| 闪光权重 | 0.30 | 闪光检测置信度权重 |
| 声源权重 | 0.35 | 声源检测置信度权重 |
| 跨模态2确认奖励 | +0.25 | 两种模态确认同一威胁 |
| 跨模态3确认奖励 | +0.40 | 三种模态确认同一威胁 |
| 空间关联阈值 | 15° | 不同模态方位角差小于此值合并 |
| 时间窗口 | 0.5s | 融合数据时间窗口 |

## 文件说明

### C++ 版本

```
多模态融合/cpp/
├── CMakeLists.txt                   # 编译配置 (支持 ROS/非ROS 双模式)
├── include/
│   ├── fusion_types.h               # 融合类型定义 (威胁等级/配置/Prometheus 话题)
│   ├── fusion_engine.h              # 融合引擎接口
│   ├── threat_cache.h               # 线程安全时间窗口缓存
│   ├── avoidance_controller.h       # 避障控制器接口
│   └── gimbal_controller.h          # AT10 云台控制器接口
└── src/
    ├── main.cpp                     # 入口: ROS 节点 (Prometheus 集成) / 独立测试
    ├── fusion_engine.cpp            # 融合算法 (空间关联/置信度融合/等级升级)
    ├── avoidance_controller.cpp     # 避障算法 (斥力/合成/平滑/冷却)
    └── gimbal_controller.cpp        # 云台控制 (SpireCV AT10 Pro)
```

### Python 版本 (已弃用)

```
多模态融合/
└── threat_fusion_node.py            # Python ROS 1 原型 (921 行)
```

## 错误处理

| 故障 | 行为 |
|------|------|
| uav_control 未启动 | 等待 10s 后继续运行，航向为 0 |
| 热成像检测连续 5 次失败 | 自动禁用热成像模态 |
| 避障指令超出地理围栏 | 拦截指令，不发送 |
| SpireCV 未启用 | 云台模拟模式，输出日志 |
| 全部模态失效 | 输出 SAFE 状态，不触发避障 |
| 无人机未解锁 | 计算避障但不发送指令 |
