LOADING

加载过慢请开启缓存 浏览器默认开启

CURC-ROBOCON2026 R2代码

2026/8/1 robocon tutorial robocon

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 启动。这也导致我并没有写任何手动梅林方案
歌单