CURC-ROBOCON2026 R2代码解析

Robocon 2026 “功夫探秘”主题赛 — R2上位机代码

硬件平台

项目	型号	说明
LiDAR	Livox MID-360	360°×59° 非重复扫描，40m 量程，内置 IMU
深度相机	Intel RealSense D415	RGB-D，用于视觉检测与测距
USB 相机	任意 USB 相机	OpenCV 采集，自动重连
底盘	全向轮（地沟哎A板下位机）	CH340 USB 串口，115200 baud
计算平台	9300H + 1650	Ubuntu 24.04, ROS2 Jazzy

顶层目录结构

├── src/                          # ROS2 功能包
│   ├── spark_fast_lio/           # FAST-LIO2 LiDAR-IMU 里程计
│   ├── omni_chassis_bridge/      # 全向轮底盘串口桥接（Nav2 → STM32）
│   ├── usb_cam_node/             # USB 相机节点
│   ├── registration/             # 点云配准与全局重定位
│   │   ├── KISS-Matcher/         # 全局点云匹配（FPFH + TEASER++）
│   │   └── global_relocalization_kiss_matcher/
│   └── kfs_core/                 # KFS 推理核心（C++ ONNX YOLO）
├── launch/                       # 一键启动脚本
├── config/                       # 配置文件（LiDAR / Nav2 / URDF）
├── maps/                         # 建图输出（map.pgm + map.yaml）
├── scripts/                      # 工具脚本
└── docs/                         # 文档

ROS2 功能包（src/）

spark_fast_lio — FAST-LIO2 里程计

基于 MIT-SPARK 的 FAST-LIO2，使用 ESIKF（误差状态迭代卡尔曼滤波）结合 ikd-Tree 实现轻量级 LiDAR-IMU 里程计。核心算法（IKFoM、ikd-Tree、点云去畸变）保持原版不变，在预处理、后处理、配置和 Nav2 集成层面进行了大量定制。

• 输入：/livox/lidar（Livox CustomMsg，xfer_format=1）+ /imu/data（经 mid360_imu_converter.py 从 g 转换为 m/s²）
• 输出：/cloud_registered（世界帧配准点云）+ map → base TF + /odometry（含 twist 速度）
• 三套 Launch 配置：

模式	Launch 文件	配置文件	用途
mapping	mapping_mid360.launch.yaml	mid360.yaml	建图，累积点云，Ctrl-C 自动导出 PCD
loc	mapping_mid360_loc.launch.yaml	mid360_loc.yaml	加载先验 PCD 到 ikd-Tree，ICP 匹配纯定位
odom	mapping_mid360_odom.launch.yaml	mid360_odom.yaml	里程计模式，发布 odom → base TF，供 KISS-Matcher 全局重定位

代码架构

spark_fast_lio.h 中的 SPARKFastLIO2 类继承自 rclcpp::Node，核心成员：

• Preprocess（preprocess.h）：Livox CustomMsg 解析、点云去畸变、方向性盲区过滤、时间同步
• ImuProcess（imu_processing.hpp）：IMU 前向传播、ESIKF 预测步骤
• KD_TREE<PointType>（ikd_Tree.h）：增量 kd-tree，支持动态插入、删除和重平衡
• esekfom::esekf（esekfom.hpp）：误差状态迭代卡尔曼滤波器

主循环（1ms wall timer → main()）：syncPackages() 同步 LiDAR-IMU 数据 → IMU 前向传播 → 点云去畸变 + 降采样 → 滤波链（见下文）→ lasermapFovSegment() 提取局部地图 → calcHModel() 计算观测模型 → EKF 迭代更新 → mapIncremental() 增量更新 ikd-Tree。

50Hz 独立 odom 发布：主循环只在收到 LiDAR 帧时触发，但 Nav2 和动作组控制回路需要稳定的 odom 流。为此新增了 odom_timer_（20ms 周期），独立于主循环以 50Hz 持续发布最新的 map → base TF 和 odometry，避免下游控制回路更新频率过低。

数据预处理链路

FAST-LIO 的输入数据在到达主循环之前需要经过两个外部节点：

MID-360 硬件 ──CustomMsg──→ livox_ros_driver2 ──/livox/lidar──→ FAST-LIO
                                 └── /livox/imu ──→ mid360_imu_converter ──/imu/data──→ FAST-LIO

mid360_imu_converter.py：MID-360 内置 IMU 的加速度输出单位是 g（重力加速度），而 FAST-LIO 需要 m/s²。这个 35 行的节点纯粹做单位转换——订阅 /livox/imu，将 linear_acceleration 三轴分量乘以南京本地重力加速度 G = 9.7949 m/s²，重新发布到 /imu/data。代码中直接修改入参对象以复用内存，避免 200Hz IMU 流产生不必要的分配开销：

def cb(self, msg: Imu):
    msg.linear_acceleration.x *= G  # g → m/s²
    msg.linear_acceleration.y *= G
    msg.linear_acceleration.z *= G
    self.pub.publish(msg)

与原版 MIT-SPARK 的主要差异

1. Livox MID-360 传感器适配

• 预处理新增方向性盲区参数 blind_front（建图 0.85m / 定位 0.30m）和 blind_rear（建图 0.35m / 定位 0.50m），根据点的 X 坐标方向选择不同盲区距离——前方用小盲区保留障碍物检测，后方用大盲区过滤机器人自身结构
• is_from_pilot_zone() 新增 "livox" 模式，过滤动态点云和操作人员干扰（blind_for_human_pilots: 0.45）
• 配置 MID-360 专用外参：extrinsic_T = [0.011, 0.02329, -0.04412]（来自 MID-360 用户手册，IMU 相对 LiDAR 光心位置）
• 每套 Launch 文件通过 static_transform_publisher 硬编码 MID-360 安装位姿：
  • 建图模式：base → livox_frame 无偏移（x=0, z=0, pitch=0, yaw=0），便于直接从地图原点开始建图
  • 定位/里程计模式：base → livox_frame（x=-0.324, z=0.324, pitch=45°, yaw=180°），对应实际安装位姿

2. 先验地图重定位（Loc 模式）

原版 MIT-SPARK 不具备此功能，在 spark_fast_lio.cpp 的构造函数中实现：

• 启动时通过 prior_map_path 加载 PCD 先验地图，经体素降采样（prior_map_voxel: 0.3m）后直接 ikd_tree_.Build() 初始化
• 坐标系转换：PCD 保存在 map（base 世界帧 W_b），而 ICP 匹配的实时点云在 IMU 世界帧 W_i。加载时通过 offset_R_B_I = offset_R_L_I × lidar_R_wrt_base⁻¹ 将 PCD 点转换到 W_i 帧，才能与实时扫描正确匹配
• /initialpose 回调：将 Nav2 提供的 map 系位姿转换为 IMU 系位姿，膨胀 EKF 协方差（pos=0.25m, rot=0.1rad），让 ICP 在初始猜测附近收敛
• 定位模式下关闭所有非必要发布（scan_publish_en: false, dense_publish_en: false, path_en: false），节省 CPU 和网络带宽

3. 四种点云滤波器

原版完全没有这些滤波器，全部为本地新增：

滤波器	实现方式	参数	适用场景
动态点滤波	多帧一致性检测：维护最近 N 帧体素化历史，只保留至少在 K 帧中出现的体素	dyn_filter.{num_frames:5, min_hits:3, voxel_size:0.3}	建图时关闭（低纹理场景会过度删除），导航时可用
去顶滤波	Z 轴 PassThrough 滤波，去除 z > 3.0m 的天花板点	ceiling_filter.{enabled:true, max_z:3.0}	室内场地常开
离群点滤波	RadiusOutlierRemoval，搜索半径内邻居不足则移除	radius_filter.{enabled:false, radius:0.3, min_neighbors:2}	建图时关闭（稀疏场景易误删），复杂地形可开启
机器人本体滤波	在 livox_frame 坐标系中定义盒体，盒内点直接丢弃	robot_body_filter.{x:[-0.75,0.05], y:[-0.30,0.30], z:[-0.32,0.08]}	常开

滤波调用顺序：机器人本体滤波（body帧）→ 转世界帧 → 去顶 + 离群点滤波 → 动态点滤波 → 加入 ikd-Tree

此外，lidar_body_filter_node.py 还提供了一个独立的 Python ROS2 节点，使用 numpy 向量化在 livox_frame 中做相同的盒式过滤，发布 /livox/lidar_filtered 和 /robot_body_box（RViz Marker 可视化），可独立于 FAST-LIO 运行。

4. Nav2 集成适配

• Odometry 发布中新增 twist（速度）计算：线速度由位姿差分得到并转换到 base 帧，角速度由四元数差计算——Nav2 的 DWB 规划器依赖 twist 数据
• PCD 保存坐标系改为 base 世界帧（W_b = "map"），与 odometry 发布的 TF 保持一致
• 析构函数 flush：Ctrl-C 退出时自动将累积点云写入 PCD，同时修复了原版 interval <= 0 时不累积的 bug

5. 工程改进

• timediff_lidar_wrt_imu 类型从 double（秒）改为 int64_t（纳秒），提高时间同步精度
• IMU 时间戳回环检测增强：回环时重置 kf_for_preintegration_ 并禁用时间同步偏移
• 支持多帧点云发布：/cloud_registered（世界帧）、/cloud_registered_lidar（livox_frame）、/cloud_registered_body（imu 帧）、/cloud_registered_base（base 帧），方便调试不同坐标系下的点云形态
• 通过 pcl_verbose: false 关闭 PCL 控制台输出，减少日志噪音

6. 配置参数对比

参数	原版 MIT-SPARK	本地版本	说明
lidar_type	2/3/4（Velodyne/Ouster）	1（AVIA/Livox）	MID-360 CustomMsg
scan_line	16/64	6	MID-360 扫描模组数
blind	0.3	0.4	近距离噪声适配
blind_front/rear	无	0.85/0.35（建图）	新增方向性盲区
det_range	300.0	40.0	MID-360 量程
extrinsic_T	Velodyne/Ouster	[0.011, 0.02329, -0.04412]	MID-360 手册值
prior_map_path	无	有（loc 模式）	新增先验地图重定位
pcd_save.interval	-1	100	修复原版不累积 PCD 的 bug
gravity_alignment	默认开启	关闭	否则静止时永远不发 TF
filter_size_map	0.5	0.3	更精细的体素分辨率

omni_chassis_bridge — 底盘桥接

原定功能为将 Nav2 的 /cmd_vel（Twist）转换为 23 字节串口协议帧，发送给 STM32 下位机。后因方案变更，该功能主要为动作组服务。

# 全向映射
vx（前后）→ ch1  # 28~228, 中位 128
vy（左右）→ ch2  # 28~228, 中位 128
ω （旋转）→ ch3  # 11~245, 中位 128

特性：

• 自动检测 /dev/ttyUSB0，不存在则进入模拟模式
• 50 Hz 发送频率
• 13 字节回传帧解析（帧头 0xAC + 数据 + 帧尾 0xAB）
• 调试 UI 已迁移至独立的 PyQt5 进程（见下文 GUI 调试界面）

registration — 点云配准与全局重定位

包含两个子包：

KISS-Matcher：无初值全局粗配准

• FPFH 特征描述子 + TEASER++ 鲁棒估计 + small_gicp 精配准
• 支持 C++ 和 Python 绑定（pybind11）

global_relocalization_kiss_matcher：ROS2 封装

• 累积 FAST-LIO 输出的 /cloud_registered 点云
• 与先验 PCD 地图进行全局匹配
• 成功后持续发布 map → odom TF
• 支持任意位置开机自动定位

在实际导航中，首先通过 KISS-Matcher 获得一个初始位姿，随后再进行后续导航动作。

kfs_core — KFS 推理核心

C++ 编写的 ROS2 节点，封装 ONNX Runtime + YOLO 检测：

// 话题接口
// 订阅: ~/enable (Bool)          - 推理使能
// 服务: ~/trigger (Trigger)      - 触发单次推理
// 发布: ~/result (InferResults)   - 检测结果数组
//       ~/debug_image (Image)     - 标注画面

支持 USB 相机和 RealSense 两种输入，可通过 rqt_reconfigure 动态调节置信度、IOU 阈值等参数。本节点的推理部分由其他队员（leezoneline）完成，实际运行使用 yolo11s 模型，显存占用不到 1G，在 1650 显卡下性能开销优秀。

usb_cam_node — USB 相机节点

基于 OpenCV V4L2 后端，发布 sensor_msgs/Image：

• 自动扫描 /dev/video*
• 断开后每 2 秒自动重连
• 相机不可用时发布黑色测试图以保持话题活跃
• 支持 OpenCV 实时预览窗口

SLAM 与导航

建图

python3 launch/start_mid360_fastlio_mapping.py

流程：MID-360 驱动 → IMU 转换 → FAST-LIO2 建图 → Ctrl-C 导出 PCD → 投影为 2D 占用栅格

导航（两种定位方案）

方案 A — FAST-LIO 先验地图定位（固定位置开机，不推荐使用）：

map ← FAST-LIO(loc) ← base ← livox_frame

需在 RViz 中手动设置初始位姿，偏差较大。

方案 B — KISS-Matcher 全局重定位（推荐，任意位置开机）：

map ← KISS-Matcher ← odom ← FAST-LIO ← base ← livox_frame

自动累计点云与先验地图全局匹配，无需手动设置初始位姿，偏差取决于配准算法误差，实际运行中误差极小。

一键启动脚本

脚本	功能
start_mid360_fastlio_mapping.py	建图 + 自动导出 Nav2 地图
start_mid360_fastlio_loc.py	纯定位（不建图）
start_mid360_nav2_loc.py	定位 + 导航全栈（方案 A）
start_mid360_nav2_kiss.py	定位 + 导航全栈（方案 B）+ UI 调试界面
start_mid360_preview.py	点云预览

Launch 编排系统（Stage 模式）

所有 start_mid360_*.py 启动脚本共用一套自实现的 Stage 类来编排多进程 ROS2 系统，替代了 ROS2 原生的 launch 文件。核心设计思想是 分阶段启动 + 话题就绪检查 + 并行启动 + 优雅停止。

每个 Stage 封装一个子进程及其就绪条件：

class Stage:
    name: str          # 日志标签
    cmd: List[str]     # 启动命令
    ready: Callable    # 就绪条件函数（如 topic_exists('/odometry')）
    ready_timeout: float  # 就绪超时（默认 25s）
    proc: Popen        # 子进程句柄
    log_file: File     # 独立日志文件

启动流程（以 start_mid360_nav2_kiss.py 为例）：

1. cleanup_existing() 并行清理旧进程（pkill 所有已知节点名）
2. Group 1 并行启动: driver + chassis（无依赖）
3. Group 2: imu-conv（依赖 /livox/imu）
4. Group 3: fast-lio-odom（依赖 /imu/data）
5. Group 4 并行启动: 其余所有（kfs-infer + debug-executor + debug-ui + rviz + standby-trigger）

每个 Stage 启动后阻塞等待其就绪条件（通过 ros2 topic info 检查话题存在性或 tf2_echo 检查 TF），超时则抛出异常。无依赖的 Stage 通过 _start_group() 并行启动，减少总启动时间。

优雅停止：Ctrl-C 后逆序遍历 stages 列表，先发送 SIGINT（5s 超时），若进程未退出则 SIGKILL。每个 Stage 通过 os.setsid 创建独立进程组，os.killpg() 确保子进程的子进程也被一并杀死。

日志系统：每个 Stage 的 stdout/stderr 写入独立日志文件（logs/debug_stack/20250101_120000/stage_name.log），同时实时输出到终端（带彩色标签）。所有包含 ERROR、FATAL、TRACEBACK 等关键字的行自动汇总到 _errors.log，方便赛后排查。

环境变量模块化：所有模块默认开启，通过环境变量选择性关闭：

环境变量控制

变量	默认	作用
NO_RVIZ=1	开 RViz	关闭 RViz
NO_CHASSIS=1	开底盘	关闭底盘桥接
NO_LOGIC=1	开主逻辑	关闭航点逻辑
NO_UI=1	开调试 UI	关闭 PyQt 调试界面
LOGIC_DELAY	5.0	主逻辑启动前等待秒数

串口通信协议

23 字节定长帧，50 Hz 发送频率：

索引	含义	值范围
0	帧头	保密
1	ch1 左右平移	28~228 / 128
2	ch2 前后平移	28~228 / 128
3	ch3 自转	11~245 / 128
4	ch4 抬升/机械臂	0~255 / 0
5	校验位	保密
6-9	吸盘/夹爪/灵足	0/1
14	校验位	保密
15-18	滑轨/幅度/视觉切换	0/1
19	校验位	保密
20-21	灵足角度	int16
22	2006 电机位置	0~255

构建与运行

# 编译
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

# 建图
python3 launch/start_mid360_fastlio_mapping.py

# 导航（方案 B，推荐）
python3 launch/start_mid360_nav2_kiss.py

# 调试：仅 SLAM + Nav2，不动底盘
NO_CHASSIS=1 NO_LOGIC=1 NO_UI=1 python3 launch/start_mid360_nav2_kiss.py

GUI 调试界面

基于 PyQt5 的 ROS2 调试界面，采用三层解耦架构：

UI 层（PyQt5 QWidgets）
    ↕ Qt 信号/槽
ROS2Bridge（QObject）
    ↕ rclpy 订阅/发布
ROS2 执行层（debug_executor_node）

核心设计理念是 UI 与执行层完全解耦：UI 进程（debug_ui_node.py）只负责显示和发布控制请求，动作组执行由独立的 debug_executor_node.py 完成。即使 UI 卡死，已启动的动作也不会被终止。

项目结构

ros2_debug_gui/
├── main.py                    # 程序入口
├── requirements.txt           # 依赖
├── config/
│   └── config_manager.py      # 配置管理（JSON 持久化）
├── ros_interface/
│   └── ros_bridge.py          # ROS2 桥接层（Qt 信号 ↔ ROS2 话题）
├── styles/
│   └── themes.py              # 主题样式（淡色/深色 + 透明度）
└── ui/
    ├── main_window.py         # 主窗口（QMainWindow）
    └── tabs/
        ├── main_tab.py        # 主界面（里程计 + 动作组 + 诊断 + 日志）
        ├── test_tab.py        # 测试（开环测试 + PID 调参）
        ├── vision_tab.py      # 视觉（实时画面 + 手动控制 + 录制）
        ├── motor_tab.py       # 电机（实时力矩/位置曲线）
        ├── topics_tab.py      # 话题（话题列表 + 节点关系图）
        └── preferences_tab.py # 首选项（主题/背景/透明度/强调色）

窗口结构

MainWindow（QMainWindow, 1200×750, 最小 900×600）
├── 菜单栏: 文件（退出 Ctrl+Q）/ 视图（首选项 Ctrl+,）/ 帮助（关于）
├── QTabWidget（6 个选项卡）
│   ├── 主界面（MainTab）      - 动作组 + 里程计 + 诊断 + 日志
│   ├── 测试（TestTab）         - 开环测试 + PID 调参 + 机械臂/灵足/2006 控制
│   ├── 视觉（VisionTab）       - 实时画面 + 3×3 手动方向控制 + 视觉触发 + 视频录制
│   ├── 电机（MotorTab）        - 力矩/位置曲线 + 峰值统计
│   ├── 话题（TopicsTab）       - 话题列表 + 节点-话题关系图 + 话题详情
│   └── 首选项（PreferencesTab） - 主题/背景/透明度/强调色/背景图片
├── 状态栏: "就绪"
└── paintEvent: 支持背景图片（可调不透明度）

选项卡功能详解

主界面 Tab（main_tab.py）：水平 QSplitter 分左右两栏，支持拖拽调整大小并自动保存

• 左栏：里程计状态（x/y/z/yaw）+ 里程计归零 + 动作组选择（单选按钮，选中后显示详细步骤描述）+ 执行/停止/急停按钮
• 右栏：节点诊断面板（节点状态、更新频率、机械臂、灵足、2006 电机、灵足力矩等 9 项，500ms 刷新）+ 实时日志（按级别着色：INFO 蓝 / WARN 橙 / ERROR 红，带时间戳）

测试 Tab：开环测试 + PID 实时调参

• 开环控制：前进/后退/左转/右转（速度 × 时长）+ 机械臂高度/灵足角度/2006 电机/夹爪/吸盘/气阀/继电器

• 5 组 PID 参数，运行时通过 /debug_ui/open_loop 话题发送 type=set_pid;... 格式化字符串到执行端动态修改：

  控制对象	参数	默认值
  机械臂	KP / KD / MaxRate / RxTol	80 / 1.0 / 60 / 7
  灵足	KP / KD / MaxRate / RxTol	12 / 0.5 / 120 / 0.1
  底盘前进	KP / KD	1.2 / 0.05
  底盘旋转	KP / KD	2.0 / 0.03
  2006 电机	KP / KD / MaxRate	50 / 1.0 / 100

  “应用 PID” 立即生效，”保存 PID” 持久化到 JSON config。

视觉 Tab（vision_tab.py）：左侧控制面板 + 右侧实时识别画面

• 3×3 手动方向控制：十字布局（前进 ↑ / 后退 ↓ / 左移 ← / 右移 → / 旋左 ↺ / 旋右 ↻），使用 HoldButton（按住移动、松开停止），速度 ch 128±80（平移 0.397m/s）/ 128±100（旋转 0.93rad/s）
• 键盘控制：WSAD 前后左右 + QE 左右旋转，通过 eventFilter 拦截，支持多键同时按下合并速度
• 2006 电机：步进按钮（±10）
• 夹爪切换：ON/OFF 按钮，颜色联动（红色 = ON / 灰色 = OFF）
• 视觉识别控制：
  • KFS 识别 / Weapon 识别：独立触发按钮，发布到 /kfs_infer_node/trigger_kfs 和 /kfs_infer_node/trigger_weapon
  • 连续推理：模型选择（KFS / Weapon / Both）+ 开关按钮，控制 /kfs_infer_node/enable 和 /kfs_infer_node/set_continuous_mode
  • 相机重连：在线程中调用 /kfs_infer_node/reconnect_camera 服务
• 视频录制：支持录制原始图像 / 识别图像 / 同时录制，保存为 mp4（mp4v 编码，30fps），按时间戳命名到 logs/vision_recordings/

电机 Tab（motor_tab.py）：自绘 CurveCanvas（QPainter + 滑动时间窗口）

• 灵足力矩曲线（红色，-2~10 N·m，仅 |力矩| ≥ 2 时绘制）
• 位置数据曲线（可选灵足角度/机械臂/2006 电机），最多 5000 点滑动窗口
• 峰值统计：力矩 > 4N 时记录，显示最近 10 秒峰值

话题 Tab：话题列表 + 节点-话题关系图（QGraphicsView 绘制，支持拖拽平移 + Ctrl+滚轮缩放）+ 话题详情（类型/频率/发布者/订阅者）

首选项 Tab：淡色/深色主题（themes.py 中的 ThemeManager，使用 rgba 透明度实现亚克力效果）、自定义背景颜色/图片（支持不透明度调节）、强调色、UI 透明度。设置自动保存到 ~/.ros2_debug_gui/config.json，下次启动恢复。

ROS2 桥接层

ros_bridge.py 中的 ROS2Bridge(QObject) 是实现 UI 与 ROS2 解耦的关键，通过 Qt 信号与 UI 通信保证线程安全。

发布话题（UI → 执行端）：

话题	类型	用途
/debug_ui/trigger	String	触发动作组（内容为动作组名或 “stop”）
/debug_ui/open_loop	String	开环/PID/手动/夹爪指令（type=xxx;value=xxx;duration=xxx）
/cmd_vel_smoothed	Twist	底盘速度（BEST_EFFORT, depth=1）
/chassis/aux	UInt8MultiArray	辅助通道 [arm, foot_hi, foot_lo, m2006, gripper, relay]
/kfs_infer_node/trigger_kfs	Bool	单次 KFS 推理
/kfs_infer_node/trigger_weapon	Bool	单次 Weaponhead 推理
/kfs_infer_node/enable	Bool	连续推理开关
/kfs_infer_node/set_continuous_mode	String	连续推理模型选择（kfs/weapon/both）
/initialpose	PoseWithCovarianceStamped	触发 KISS-Matcher 全局重定位

订阅话题（执行端 → UI）：

话题	信号	用途
/odometry	odom_signal(x, y, z, yaw)	位姿反馈（支持显示归零偏移）
/debug_executor/status	diagnostic_signal(dict)	执行端诊断（忙碌/空闲/进度/桥接状态）
/chassis/status	内部状态	桥接状态（real/sim）
/standby_trigger/status	standby_signal(dict)	待命状态（phase/remaining/action/team/switches）
/kfs_infer_node/debug_image	image_signal(cv_img)	识别标注画面（cv_bridge 转换）
/kfs_infer_node/raw_image	raw_image_signal(cv_img)	原始画面（用于视频录制）

通信机制

1. connect_ros() 创建独立 ROS2 节点 debug_gui_client_{pid}，启动 daemon 线程循环执行 rclpy.spin_once()
2. ROS2 回调通过 pyqtSignal.emit() 将数据传到 UI 线程，保证线程安全
3. 两个 QTimer 定时刷新：话题列表（1000ms）+ 诊断信息（500ms）
4. rclpy 导入失败时自动进入模拟模式：用 QTimer 模拟里程计数据、话题列表和节点图
5. 断开时严格按顺序清理：停止所有 QTimer → 等待 spin 线程退出 → 发布零速停止 → 销毁节点 → shutdown rclpy

动作组系统

action_groups.py 定义了 14 个预设动作组，UI 和执行层共用此文件，避免界面描述与实际动作不同步。每个动作组是步骤序列，支持子动作组嵌套（通过 action_group 步骤类型引用其他动作组）。

动作组	关键流程
红方启动	纠正前进 → 左转 90° + 机械臂 100 + 灵足 178° → 纠正到 (0.18, 0.75, 90°) → 视觉夹取 → 对接准备 → 捡取下一组
蓝方启动	纠正前进 → 右转 90° + 机械臂 100 + 灵足 178° → 纠正到 (0.4, -0.9, -90°) → 视觉夹取 → 对接准备 → 捡取下一组 → 蓝方梅林准备识别
对接准备	夹爪关闭 → 后退 65cm → 旋转 180° + 灵足 180° → 机械臂 85→72 → 灵足 255° + 夹爪闭合 → 等待力矩 > 6.5N → 松开
视觉夹取	2006 视觉对准（12px 容差，增益 0.5，连续 2 帧）→ 机械臂降 5 → 夹爪闭合 → 机械臂升 30
捡取下一组	左转 180° + 灵足 178° → 左移 20cm → 前进 65cm → 开环前进 2s → 视觉夹取 → 对接准备
归零	机械臂 → 60 → 气阀关 → 灵足 → 275° → 机械臂 → 0（跳过互锁）→ 2006 → 0
蓝方梅林准备	纠正到 (2m, 0.45m, 0°) → 机械臂 → 110 + 灵足 → 200° + 2006 → 128
蓝方梅林准备识别	识别姿态 → KFS 扫描 1s → 若 T 则拿取（臂 → 80 + 足 → 90° → 吸盘气阀 → 继电器 → 前进堵转）
红方梅林准备	纠正到 (2m, -0.45m, 0°) → 机械臂 → 110 + 灵足 → 200° + 2006 → 128
红方梅林准备识别	同蓝方对称，识别姿态 → KFS 扫描 → 若 T 则拿取
红方上梅林	2006 中位 → 横移堵转到边缘 → 机械臂 40 + 灵足 180 + 归零 → 前进直到上坡（Z + 10cm）
蓝方上梅林	同红方对称，横移方向相反（ch=178）
上梅林	通用版：2006 中位 → 机械臂 40 + 灵足 180 + 归零 → 前进直到上坡
下梅林	以 ch=103 后退，直到 Z 下降 ≥ 10cm 且车身水平
前进下梅林	以 ch=143 前进，直到 Z 下降 ≥ 10cm 且车身水平
高 200KFS 获取	机械臂 120→110 + 灵足 200 + 2006→128 → KFS 扫描 → 若 T 则前进堵转 + 臂 180 + 吸盘气阀 + 降臂至力矩 0.7N·m + 抬升返回
拿取高 200KFS	前进 0.6m → 继电器伸长 → 前进堵转 → 臂 180 + 2006 中位 → 降臂至力矩 → 吸盘气阀 → 臂 200 + 足 270 → 臂归零 → 返回
识别高 200KFS	机械臂 → 120→110 + 灵足 200 + 2006→128 → KFS 扫描 1s
识别低 200KFS	机械臂 → 120 → 继电器伸长 → 机械臂 → 110 + 灵足 130 + 2006→128 → KFS 扫描 1s
武器对接测试	机械臂 85→62 → 灵足 255° + 夹爪闭合 → 等待力矩 > 6.5N 后回落 → 松开夹爪
蓝方动作组	全流程组合：蓝方启动 → 视觉夹取 → 对接准备 → 梅林准备 → 识别（含横向移动 2.4m + 二次识别）

红蓝方对称设计：红方和蓝方的启动动作、梅林准备、上梅林等动作组通过镜像坐标和方向实现对称场地适应（红方目标 (0.18, 0.75, 90°) vs 蓝方 (0.40, -0.90, -90°)，上梅林横移方向相反）。

支持的步骤类型（共 18 种）：

步骤类型	说明	示例
forward/backward/strafe	PID 控制平移（距离 + 容差 + 超时）	{'forward': 0.65, 'desc': '前进 0.65m'}
left/right	PID 控制旋转（角度 + 容差），支持并行机械臂/灵足	{'left': 180.0, 'arm': 100, 'foot_angle': 178.0}
arm	机械臂位置控制（支持串口反馈容差、慢速模式、互锁跳过）	{'arm': 85, 'use_rx_feedback': True, 'arm_slow': True}
arm_delta	机械臂相对位移	{'arm_delta': 40, 'desc': '抬升 40'}
foot_angle	灵足角度控制（支持串口反馈校正）	{'foot_angle': 255.0, 'use_rx_feedback': True}
m2006_pos	2006 电机位置控制	{'m2006_pos': 128, 'use_rx_feedback': True}
open_forward	开环前进（通道值 + 时长）	{'open_forward': 148, 'open_duration': 2.0}
correct_pose	全向 PID 位置纠正（支持相对/绝对模式、禁止后退）	{'correct_pose': True, 'target_x': 2.0, 'target_yaw': 0.0}
forward_until_level	前进直到上坡（Z 上升检测 + pitch/roll 水平判断）	{'forward_until_level': True, 'z_thresh': 0.10}
backward_until_level	后退直到下坡	{'backward_until_level': True, 'ch_val': 103}
forward_until_stall	前进直到堵转（位移停滞检测 + 俯仰保护）	{'forward_until_stall': True, 'ch_val': 148, 'stall_time': 2.0}
strafe_until_stall	横移直到堵转（顶到边缘检测，限制最大距离）	{'strafe_until_stall': True, 'ch_val': 108, 'max_dist': 3.0}
wait_torque_drop	力矩对接检测（等待力矩超过阈值后回落）	{'wait_torque_drop': True, 'torque_high': 6.5}
arm_lower_until_torque	机械臂下降直到灵足力矩达标（接触桌面检测）	{'arm_lower_until_torque': 0.3, 'rate': 10.0}
align_2006	视觉对齐：2006 电机对准目标中心（连续 n 帧稳定）	{'align_2006': True, 'align_tol_px': 12, 'align_gain': 0.5}
kfs_scan	KFS 识别扫描（采集标签）	{'kfs_scan': True, 'duration': 1.0}
skip_if_label_not	条件跳过：KFS 结果不匹配则跳过后续步骤	{'skip_if_label_not': 'T'}
gripper/suction/valve/relay	夹爪/吸盘/气阀/继电器控制	{'gripper': 1, 'suction': 1, 'valve': 1}
wait	等待指定秒数	{'wait': 0.3}
action_group	子动作组嵌套调用	{'action_group': '对接准备'}

执行层

debug_executor_node.py 中的 ControlNode 类：

• 50Hz 控制回路（_loop timer），按步骤顺序执行动作组
• PID 控制系统：底盘前进/旋转（带累积 yaw 跟踪解决 ±π 翻转问题）、机械臂（支持慢速模式 + 互锁保护）、灵足角度、2006 电机
• 底盘死区补偿：MIN_LIN_CH=15（0.034m/s）、MIN_ANG_CH=15（0.045rad/s），末端蹭进模式（误差 < 3×TOL 时以恒定最低速度推进）
• 串口回传反馈（/chassis/rx）：抬升高度、灵足真实角度、灵足力矩、2006 电机位置，用作闭环容差判断
• 视觉反馈：订阅 /kfs_infer_node/result（InferResults），align_2006 步骤通过检测框中心像素误差驱动 2006 电机
• 桥接失联保护：/chassis/status 使用独立 ReentrantCallbackGroup + MultiThreadedExecutor，避免串口阻塞反向波及 ROS spin 线程
• 诊断发布：200ms 周期发布 /debug_executor/status（含 busy/progress/desc/bridge/rx_lift/rx_m2006/rx_foot/rx_torque 等字段）
• 外部触发接口：/debug_ui/trigger（String）+ /debug_ui/open_loop（String），允许任意外部节点控制

互锁保护

• 机械臂下降到 < 45 时，灵足必须 < 215°
• 灵足旋转时，机械臂必须 ≥ 35

待命触发节点

standby_trigger_node.py 是比赛中的”最后一公里”——它连接了下位机遥控器与上位机自动执行，让操作手无需触碰电脑即可触发动作组。

工作原理：监控 STM32 回传帧 /chassis/rx 中的三个预留位，检测边沿变化：

预留位	索引	功能
预留位 1	raw[3]	触发启动：0→1 边沿触发倒计时（3s），结束后自动执行动作组
预留位 2	raw[4]	动作组选择：0 = 红/蓝方启动动作，1 = 上梅林（红方/蓝方自动适配）
预留位 3	raw[5]	暂停/恢复：0→1 暂停当前动作，1→0 归零恢复初始状态

阵营识别通过 raw[2]（0=红方, 1=蓝方）自动适配。

倒计时与状态发布：10Hz timer（_tick）实时计算剩余时间，通过 /standby_trigger/status 发布给 UI 显示倒计时进度。倒计时期间若预留位 1 变回 0 则取消。倒计时归零后通过 /debug_ui/trigger 发布动作组名到执行层，与 UI 手动触发走完全相同的通道。

对外接口：

话题	方向	用途
/chassis/rx	订阅	下位机回传帧（UInt8MultiArray）
/standby_trigger/status	发布	倒计时状态（phase/remaining/action/team/switches）
/debug_ui/trigger	发布	触发动作组（String，复用 UI 的执行通道）

这个节点在比赛中的实际意义是：操作手通过遥控器拨动开关即可触发完整的自动流程（启动 → 视觉夹取 → 对接准备 → 梅林识别），无需触碰电脑，大幅降低了操作复杂度。节点独立于 UI 运行，即使 PyQt 界面未启动也能正常工作。不过得益于传奇机械组减重能力，我最终还是用了一台 MacBook neo 运行 VNC 控制 R2 启动。这也导致我并没有写任何手动梅林方案