[release] 规范 ROS 2 三代目录并校准训练说明

2026-07-27 18:43:29 +08:00
parent 196044b993
commit 117b02bec7
171 changed files with 12741 additions and 6472 deletions
@@ -0,0 +1,102 @@
+# sim2real_ros2_v3 架构说明
+
+## 设计目标
+
+- 保留已验证的 RL 部署契约不变
+- 将低延迟循环从 Python 迁移至 C++
+- 暴露标准 ROS 2 接口用于导航和系统集成
+- 保持安全边界独立于策略正确性
+
+## 各包职责
+
+### `sim2real_interfaces`（接口消息）
+
+定义最小化的运行时消息：
+
+- `RuntimeState`
+  硬件桥接发布的归一化运行时状态快照
+- `RuntimeTarget`
+  策略运行时发送至硬件桥接的最新策略目标
+
+### `sim2real_common`（共享常量）
+
+存储编译期常量和部署契约辅助：
+
+- 观测维度和字段布局
+- 动作维度和轮子索引
+- 关节顺序和默认站姿
+- 动作缩放因子和默认循环频率
+- Mahony 姿态滤波器
+- 站立平衡控制器
+- 安全监控器（SafetyMonitor / RuntimeGuard）
+
+### `sim2real_hw`（硬件桥接）
+
+拥有硬件侧执行循环和安全边界：
+
+- RobStride CAN 收发
+- IMU 与 Odin 状态采集
+- 电机丢帧检测与保活逻辑（holdover）
+- 看门狗与阻尼刹车
+- 发布 `RuntimeState`
+- 订阅 `RuntimeTarget`
+- 订阅 `/odom` 里程计数据
+
+目标热路径：
+
+- 以 `200Hz` 频率读取状态
+- 应用最新安全目标
+- 超时或安全违规时立即停机
+
+### `sim2real_runtime`（策略运行时）
+
+拥有策略侧执行：
+
+- 订阅 `RuntimeState`
+- 按当前部署契约精确构建 `53D` 观测
+- 以 `50Hz` 运行 ONNXRuntime 推理
+- 对 raw_action 做 `[-10, 10]` 安全裁剪
+- 发布 `RuntimeTarget`
+- 仲裁命令来源：estop > safety_hold > startup > navigation > web
+
+同时包含：
+- `odom_relay_node`：将 `/odin1/odometry` 中继为 `/odom`，帧名 `odin1_base_link` → `base_link`，并广播 TF
+
+### `sim2real_nav2`（导航配置）
+
+拥有：
+
+- Nav2 参数文件（planner、controller、costmap、AMCL、behavior）
+- Nav2 启动文件（含 AMCL、costmap 生命周期节点、pointcloud_to_laserscan）
+
+### `sim2real_bringup`（启动管理）
+
+拥有：
+
+- 参数文件
+- 启动组合
+- 运行时模式选择
+- 集成 odin_ros_driver、sim2real_nav2 的条件启动
+
+## 迁移规则
+
+1. 优化之前先冻结当前契约
+2. 先迁移传输和循环结构，再调整控制算法
+3. C++ 运行时未达到影子模式一致性前，保留 Python 运行时可用
+4. 按段测量延迟：
+   - 观测延迟
+   - 策略推理延迟
+   - 目标传输延迟
+   - 执行器响应延迟
+
+## 首个里程碑
+
+首个里程碑不是"机器人在 ROS 2 下行走"，而是：
+
+1. `sim2real_hw` 发布稳定的 `RuntimeState`
+2. `sim2real_runtime` 从该状态构建正确的 `53D` 观测
+3. `sim2real_runtime` 以 `50Hz` 发布 `RuntimeTarget`
+4. `sim2real_hw` 消费最新目标并执行超时刹车
+5. `cmd_vel` 可通过 ROS 2 注入而不改变策略契约
+
+> ✅ 以上里程碑已全部完成。
@@ -0,0 +1,80 @@
+# 迁移计划
+
+## Phase 1: 硬件核心迁移 ✅ 已完成
+
+将当前高频热路径从 Python 迁出。
+
+吸收的源文件：
+
+- `sim2real/interface/motor_driver.py`
+- `sim2real/interface/motor_mapping.py`
+- `sim2real/interface/imu_client.py`
+- `sim2real/safety/runtime_guard.py`
+- `sim2real/web/session.py`
+
+交付物：
+
+- C++ SocketCAN 电机总线封装
+- C++ 状态缓存
+- target 超时保活（timeout hold）
+- 阻尼刹车 / 急停通路
+- 发布 `RuntimeState`
+
+## Phase 2: 策略运行时迁移 ✅ 已完成
+
+吸收的源文件：
+
+- `sim2real/policy/policy_runner.py`
+- `sim2real/interface/real_io.py`
+- `sim2real/web/session.py`
+
+交付物：
+
+- 精确的 `53D` 观测构造器
+- ONNXRuntime C++ 推理封装
+- `50Hz` 策略定时器
+- 命令平滑与来源仲裁
+- raw_action `[-10, 10]` 安全裁剪
+- 发布 `RuntimeTarget`
+
+## Phase 3: ROS 2 系统集成 ✅ 已完成
+
+参考的源项目：
+
+- `00_ reference/odin_ros_driver`
+- `00_ reference/EDULITE_A3/el_a3_ros`
+- `00_ reference/rl_sar`
+
+交付物：
+
+- `cmd_vel` / `cmd_vel_stamped` 输入（支持 Twist 和 TwistStamped）
+- `odom_relay_node`：里程计中继 + TF 广播（odom → base_link）
+- 诊断话题
+- rosbag/foxglove 可观测性
+
+## Phase 4: 导航集成 ✅ 已完成
+
+目标：
+
+- 导航通过 ROS 2 发送身体速度指令
+- RL 运行时保持为 locomotion 控制器
+- 看门狗和安全边界始终在导航之下
+
+规则：
+
+- 导航绝不直接写电机指令
+- 策略契约在重新训练前保持不变
+- 任何新增历史项或里程计项必须版本化
+
+## 当前状态
+
+所有 4 个 Phase 已全部完成。以下为已实现的关键组件：
+
+| 组件 | 节点 | 说明 |
+|------|------|------|
+| 硬件桥接 | `sim2real_hw_node` | 200Hz CAN 收发 + IMU + Mahony + 安全 |
+| 策略运行时 | `sim2real_runtime_node` | 50Hz ONNX 推理 + 53D 观测 + raw_action clip |
+| 里程计中继 | `odom_relay_node` | /odin1/odometry → /odom + odom→base_link TF |
+| 导航栈 | Nav2 全套节点 | AMCL + costmap + DWB + Navfn + BT + lifecycle |
+| 传感器驱动 | `odin_ros_driver` | IMU + 点云 + 里程计原始发布 |
+| 点云转换 | `pointcloud_to_laserscan` | /odin1/cloud_slam → /scan (供 AMCL 使用) |
@@ -0,0 +1,499 @@
+# sim2real_ros2_v3 遥控器调用说明
+
+本文档说明如何在 `sim2real_ros2_v3` 中调用已接入的 SBUS UART 遥控器节点，以及执行后系统会产生什么效果。
+
+## 1. 当前接入关系
+
+遥控器节点位于：
+
+```text
+src/sim2real_runtime/src/remote_uart_node.py
+```
+
+该节点读取 SBUS 串口数据，并发布标准 ROS 2 控制话题：
+
+| 输入 | 输出 | 作用 |
+|---|---|---|
+| SBUS UART 遥控器 | `/cmd_vel` | 给策略运行时发送速度命令 |
+| SBUS CH7 高位 | `/safety/estop` | 触发软件急停 |
+
+策略节点 `sim2real_runtime_node` 已经订阅 `/cmd_vel` 和 `/safety/estop`，所以遥控器不直接控制电机，而是通过 ROS 2 标准速度接口进入策略控制链路。
+
+## 2. 通道映射
+
+通道映射与本仓库第一代 Python Sim2Real 实现中的遥控器配置保持一致。
+
+| 遥控器通道 | ROS 2 输出 | 含义 | 默认最大值 |
+|---|---|---|---:|
+| `CH2` | `cmd_vel.linear.x` | 前后速度 `vx` | `0.8 m/s` |
+| `CH4` | `cmd_vel.linear.y` | 左右速度 `vy` | `0.3 m/s` |
+| `CH1` | `cmd_vel.angular.z` | 转向角速度 `yaw` | `0.5 rad/s` |
+| `CH7 HIGH` | `/safety/estop = true` | 软件急停 | - |
+
+默认方向反转配置：
+
+| 参数 | 默认值 | 含义 |
+|---|---:|---|
+| `remote_invert_vx` | `true` | 反转前后方向 |
+| `remote_invert_vy` | `false` | 不反转横移方向 |
+| `remote_invert_yaw` | `true` | 反转转向方向 |
+
+## 3. 参数位置
+
+遥控器参数在：
+
+```text
+src/sim2real_bringup/config/runtime.yaml
+```
+
+当前默认参数：
+
+```yaml
+remote_enabled: true
+remote_port: "/dev/ttyACM0"
+remote_baudrate: 100000
+remote_timeout: 0.02
+remote_axis_deadzone: 50
+remote_active_threshold: 50
+remote_axis_full_scale: 660.0
+remote_max_vx: 0.8
+remote_max_vy: 0.3
+remote_max_yaw_rate: 0.5
+remote_invert_vx: true
+remote_invert_vy: false
+remote_invert_yaw: true
+remote_publish_inactive_zero: true
+remote_estop_latch: true
+remote_estop_channel: 7
+remote_estop_level: "high"
+remote_estop_debounce_frames: 3
+remote_estop_require_remote_mode: true
+remote_poll_hz: 50.0
+```
+
+如果遥控器串口不是 `/dev/ttyACM0`，需要修改：
+
+```yaml
+remote_port: "/dev/ttyUSB0"
+```
+
+或改成实际设备路径。
+
+## 4. 启动前检查
+
+### 4.1 确认串口存在
+
+```bash
+ls /dev/ttyACM* /dev/ttyUSB*
+```
+
+如果使用默认配置，应能看到：
+
+```bash
+/dev/ttyACM0
+```
+
+### 4.2 确认串口权限
+
+如果节点提示串口权限不足，可以临时执行：
+
+```bash
+sudo chmod 666 /dev/ttyACM0
+```
+
+更推荐的长期方式是把当前用户加入 `dialout` 组：
+
+```bash
+sudo usermod -aG dialout $USER
+```
+
+然后重新登录。
+
+### 4.3 确认 Python serial 依赖
+
+节点依赖 `pyserial`。如果系统没有安装：
+
+```bash
+sudo apt update
+sudo apt install -y python3-serial
+```
+
+## 5. 构建
+
+如果刚修改过代码或参数，建议重新构建相关包：
+
+```bash
+cd /path/to/sim2real_ros2_v3
+source /opt/ros/humble/setup.bash
+colcon build --packages-select sim2real_runtime sim2real_bringup --symlink-install --merge-install
+```
+
+构建完成后 source 环境：
+
+```bash
+source install/setup.bash
+```
+
+确认可执行节点存在：
+
+```bash
+ros2 pkg executables sim2real_runtime
+```
+
+应包含：
+
+```text
+sim2real_runtime remote_uart_node.py
+```
+
+## 6. 推荐启动方式
+
+### 6.1 启动完整系统，不启动 Nav2
+
+这是你当前常用方式：
+
+```bash
+cd /path/to/sim2real_ros2_v3
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
+```
+
+默认情况下，`launch_remote:=true`，所以上面命令会同时启动遥控器节点。
+
+等价完整写法：
+
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=true
+```
+
+### 6.2 不启动遥控器
+
+如果只想用手动 `ros2 topic pub` 或其他上位机发 `/cmd_vel`，可以关闭遥控器节点：
+
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_remote:=false
+```
+
+## 7. 单独启动遥控器节点
+
+如果系统已经在运行，只想单独测试遥控器节点：
+
+```bash
+cd /path/to/sim2real_ros2_v3
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
+```
+
+如果要临时指定串口：
+
+```bash
+ros2 run sim2real_runtime remote_uart_node.py --ros-args \
+  --params-file src/sim2real_bringup/config/runtime.yaml \
+  -p remote_port:=/dev/ttyUSB0
+```
+
+## 8. 执行后会产生什么效果
+
+启动以下命令后：
+
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
+```
+
+系统会产生以下效果。
+
+### 8.1 启动硬件桥接节点
+
+节点：
+
+```text
+/sim2real_hw_node
+```
+
+效果：
+
+1. 打开 `can0` 和 `can1`。
+2. 如果 `dry_run: false` 且 CAN 初始化成功，会使能 16 个 RobStride 电机。
+3. 设置电机 MIT 模式。
+4. 设置电机速度限制和力矩限制。
+5. 以 `200Hz` 运行硬件读写循环。
+6. 发布 `/runtime/state`。
+7. 订阅 `/runtime/target` 执行策略目标。
+
+### 8.2 启动策略运行节点
+
+节点：
+
+```text
+/sim2real_runtime_node
+```
+
+效果：
+
+1. 加载 ONNX 策略模型。
+2. 订阅 `/runtime/state`。
+3. 订阅 `/cmd_vel`。
+4. 订阅 `/safety/estop`。
+5. 执行启动站立流程：
+   - `boot_hold`
+   - `startup_soft_hold`
+   - `startup_hold`
+   - `runtime_zero_hold`
+   - `runtime_policy`
+6. 以 `50Hz` 发布 `/runtime/target`。
+
+### 8.3 启动遥控器节点
+
+节点：
+
+```text
+/sim2real_remote_uart_node
+```
+
+效果：
+
+1. 打开默认串口 `/dev/ttyACM0`。
+2. 以 `50Hz` 轮询 SBUS 数据。
+3. 遥控器摇杆居中时持续发布零速度：
+
+```text
+/cmd_vel:
+  linear.x = 0.0
+  linear.y = 0.0
+  angular.z = 0.0
+```
+
+4. 推动遥控器时发布非零速度，例如：
+
+```text
+/cmd_vel:
+  linear.x = vx
+  linear.y = vy
+  angular.z = yaw
+```
+
+5. 当 CH7 打到高位时发布：
+
+```text
+/safety/estop: true
+```
+
+由于当前 `remote_estop_latch: true`，急停是锁存式行为：在 `REMOTE` 模式下，CH7 连续 3 帧有效高位后，节点会发布急停，并保持内部急停已触发状态。恢复运行通常需要重启系统或手动发布复位信号，并确认机器人安全。
+
+### 8.4 机器人行为效果
+
+正常启动后，机器人不会立即按策略行走，而是按阶段执行：
+
+1. 电机使能。
+2. 读取当前关节位置。
+3. 软保持当前姿态。
+4. 平滑过渡到默认站立姿态。
+5. 稳定后进入 runtime。
+6. 遥控器无输入时保持站立平衡，即 `runtime_zero_hold`。
+7. 遥控器有输入时进入策略控制，即 `runtime_policy`。
+
+也就是说：
+
+| 遥控器状态 | 机器人效果 |
+|---|---|
+| 摇杆居中 | 站立保持，不主动行走 |
+| CH2 前后推动 | 前进/后退 |
+| CH4 左右推动 | 横向移动 |
+| CH1 左右推动 | 原地转向 |
+| CH7 高位 | 软件急停，进入安全刹车 |
+
+## 9. 如何确认遥控器已经生效
+
+### 9.1 查看节点是否存在
+
+```bash
+ros2 node list
+```
+
+应看到：
+
+```text
+/sim2real_remote_uart_node
+/sim2real_runtime_node
+/sim2real_hw_node
+```
+
+### 9.2 查看 `/cmd_vel`
+
+```bash
+ros2 topic echo /cmd_vel
+```
+
+摇动遥控器时应看到 `linear.x`、`linear.y` 或 `angular.z` 变化。
+
+### 9.3 查看 `/safety/estop`
+
+```bash
+ros2 topic echo /safety/estop
+```
+
+CH7 高位时应看到：
+
+```yaml
+data: true
+```
+
+### 9.4 查看策略目标阶段
+
+```bash
+ros2 topic echo /runtime/target --field target_source
+```
+
+常见输出含义：
+
+| `target_source` | 含义 |
+|---|---|
+| `boot_hold` | 刚启动，保持初始姿态 |
+| `startup_soft_hold` | 启动软保持 |
+| `startup_hold` | 正在站立或站立后保持 |
+| `runtime_zero_hold` | 已进入 runtime，遥控器无有效输入 |
+| `runtime_policy` | 遥控器有输入，策略已经介入 |
+| `safety_brake` | 安全刹车 |
+| `timeout_hold` | 目标超时，硬件保持默认姿态 |
+
+### 9.5 查看完整目标状态
+
+```bash
+ros2 topic echo --once /runtime/target
+```
+
+重点关注字段：
+
+```yaml
+target_source:
+zero_command:
+runtime_released:
+release_alpha:
+command:
+raw_command:
+```
+
+如果遥控器摇杆有输入，通常会看到：
+
+```yaml
+target_source: runtime_policy
+zero_command: false
+runtime_released: true
+release_alpha: 1.0
+```
+
+## 10. 常见问题
+
+### 10.1 启动后提示无法打开串口
+
+可能原因：
+
+1. 串口路径不对。
+2. 权限不足。
+3. 设备没有插好。
+4. 设备被其他程序占用。
+
+检查：
+
+```bash
+ls /dev/ttyACM* /dev/ttyUSB*
+```
+
+修改 `runtime.yaml`：
+
+```yaml
+remote_port: "/dev/ttyUSB0"
+```
+
+### 10.2 `/cmd_vel` 没有变化
+
+检查：
+
+```bash
+ros2 node list
+ros2 topic echo /cmd_vel
+```
+
+如果节点存在但无变化，可能是：
+
+1. 遥控器没有输出 SBUS。
+2. 串口波特率不对。
+3. SBUS 接线错误。
+4. 遥控器通道未校准。
+5. 死区 `remote_axis_deadzone` 或 `remote_active_threshold` 太大。
+
+### 10.3 摇杆方向反了
+
+修改：
+
+```yaml
+remote_invert_vx: true
+remote_invert_vy: false
+remote_invert_yaw: true
+```
+
+例如前后方向反了，就切换：
+
+```yaml
+remote_invert_vx: false
+```
+
+### 10.4 急停后不恢复
+
+当前配置：
+
+```yaml
+remote_estop_latch: true
+```
+
+这表示急停锁存。触发后建议：
+
+1. 先确认机器人物理安全。
+2. 停止 launch。
+3. 将 CH7 打回安全位置。
+4. 重新启动系统。
+
+如果需要非锁存模式，可以改为：
+
+```yaml
+remote_estop_latch: false
+```
+
+但实机调试时更建议使用锁存模式。
+
+## 11. 快速验证命令清单
+
+```bash
+cd /path/to/sim2real_ros2_v3
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
+```
+
+另开终端：
+
+```bash
+cd /path/to/sim2real_ros2_v3
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+ros2 node list
+ros2 topic echo /cmd_vel
+ros2 topic echo /runtime/target --field target_source
+```
+
+如果只测遥控器，不启动电机系统：
+
+```bash
+ros2 run sim2real_runtime remote_uart_node.py --ros-args --params-file src/sim2real_bringup/config/runtime.yaml
+```
+
+另开终端：
+
+```bash
+ros2 topic echo /cmd_vel
+ros2 topic echo /safety/estop
+```
@@ -0,0 +1,246 @@
+# sim2real_ros2_v3 Web UDP 调试说明
+
+本文档说明本次新增的最小 Web 调试链路。
+
+## 1. 架构
+
+```text
+Windows 本地浏览器/HTTP 服务
+        |
+        | UDP JSON
+        v
+Nano: sim2real_web_udp_bridge_node.py
+        |
+        | ROS 2 topics
+        v
+sim2real_cmd_mux_node.py -> /cmd_vel -> sim2real_runtime_node
+```
+
+Web 页面在 Windows 本地渲染，Nano 只运行轻量 UDP bridge 和 ROS2 节点。
+
+## 2. 新增 ROS2 节点
+
+### `remote_uart_node.py`
+
+遥控器节点现在发布：
+
+```text
+/cmd_vel_remote
+```
+
+不再直接发布 `/cmd_vel`。
+
+通道触发阈值改为：
+
+```yaml
+remote_axis_deadzone: 40
+remote_active_threshold: 40
+```
+
+只有通道归一化值绝对值大于 `40` 才认为是有效输入。
+
+### `cmd_mux_node.py`
+
+输入：
+
+```text
+/cmd_vel_remote
+/cmd_vel_web
+/cmd_vel_nav
+/control/mode
+/remote/enabled
+/web/enabled
+/nav/enabled
+/safety/estop
+```
+
+输出：
+
+```text
+/cmd_vel
+/control/mode_state
+/control/mux_status
+```
+
+控制模式：
+
+```text
+DISABLED
+REMOTE
+WEB
+NAV
+```
+
+急停 `/safety/estop=true` 会强制进入 `DISABLED`，并输出零速度。
+
+### `web_udp_bridge_node.py`
+
+Nano 端 UDP 监听：
+
+```text
+0.0.0.0:15000
+```
+
+发布：
+
+```text
+/cmd_vel_web
+/safety/estop
+/control/mode
+/web/enabled
+/remote/enabled
+/nav/enabled
+```
+
+订阅并回传状态：
+
+```text
+/runtime/state
+/runtime/target
+/cmd_vel
+/safety/estop
+/control/mode_state
+/control/mux_status
+```
+
+## 3. Nano 启动
+
+```bash
+cd /path/to/sim2real_ros2_v3
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false
+```
+
+默认会启动：
+
+```text
+sim2real_remote_uart_node
+sim2real_cmd_mux_node
+sim2real_web_udp_bridge_node
+```
+
+如果不想启动 Web UDP bridge：
+
+```bash
+ros2 launch sim2real_bringup sim2real_system.launch.py launch_nav2:=false launch_web_bridge:=false
+```
+
+## 4. Windows 本地 Web 启动
+
+把目录复制到 Windows 或通过共享目录访问：
+
+```text
+tools/win_web_debug
+```
+
+在 Windows 上安装 Python 3 后运行：
+
+```bash
+python server.py --nano-host <Nano_IP> --http-port 8088 --udp-port 15001
+```
+
+浏览器打开：
+
+```text
+http://127.0.0.1:8088
+```
+
+## 5. UDP 命令格式
+
+### 切换模式
+
+```json
+{"type":"mode","mode":"REMOTE"}
+```
+
+```json
+{"type":"mode","mode":"WEB"}
+```
+
+```json
+{"type":"mode","mode":"DISABLED"}
+```
+
+### Web 速度控制
+
+```json
+{
+  "type": "cmd_vel",
+  "linear": {"x": 0.2, "y": 0.0, "z": 0.0},
+  "angular": {"x": 0.0, "y": 0.0, "z": 0.1}
+}
+```
+
+Nano 端会再次限幅：
+
+```text
+vx  <= ±0.8 m/s
+vy  <= ±0.3 m/s
+yaw <= ±0.5 rad/s
+```
+
+### 零速度
+
+```json
+{"type":"zero"}
+```
+
+### 软急停
+
+```json
+{"type":"estop","data":true}
+```
+
+## 6. 安全保护
+
+当前最小版本已经包含：
+
+1. 遥控器误触发阈值：`40`。
+2. 遥控器/Web/Nav 互斥控制模式。
+3. `cmd_mux` 二次限幅。
+4. `cmd_mux` 加速度限制。
+5. Web UDP 超时自动发布零速度。
+6. 急停优先级最高。
+7. Web 页面切换到 `WEB` 模式需要确认。
+8. Web 松开虚拟摇杆会自动发送零速度。
+
+建议实机调试流程：
+
+1. 先点击 `DISABLED`。
+2. 确认 `/cmd_vel` 为零。
+3. 如果使用遥控器，点击 `REMOTE`。
+4. 如果使用 Web，点击 `WEB` 并确认周围安全。
+5. 一旦异常，立即点击 `软急停`。
+
+## 7. 验证命令
+
+查看最终输出速度：
+
+```bash
+ros2 topic echo /cmd_vel
+```
+
+查看遥控器输入：
+
+```bash
+ros2 topic echo /cmd_vel_remote
+```
+
+查看 Web 输入：
+
+```bash
+ros2 topic echo /cmd_vel_web
+```
+
+查看当前仲裁模式：
+
+```bash
+ros2 topic echo /control/mode_state
+```
+
+查看策略状态：
+
+```bash
+ros2 topic echo /runtime/target --field target_source
+```
@@ -0,0 +1,164 @@
+# Pure Pursuit Yaw检查优化 - 最终配置
+
+## 🎯 核心问题
+
+**现象**: 机器人没转到位就开始前进 → 斜着撞杆子
+
+**根本原因**: Pure Pursuit的yaw检查不够严格
+
+---
+
+## ✅ 已修改的参数
+
+### runtime.yaml
+
+```yaml
+# 关键修改1: Script模式yaw门限
+nav_slalom_script_yaw_gate_deg: 8.0  # 从12度 → 8度
+
+# 关键修改2: 全局yaw容差
+nav_goal_yaw_tolerance_deg: 8.0      # 从12度 → 8度
+```
+
+**作用**:
+- yaw偏差 > 8度时：只转向，不前进
+- yaw偏差 ≤ 8度时：才允许前进
+
+---
+
+## 📊 完整配置总结
+
+### 1. 禁用干扰的规划器
+```yaml
+nav_local_planner_enabled: false
+nav_astar_enabled: false
+```
+
+### 2. 速度和精度
+```yaml
+nav_slalom_max_vx: 0.50
+nav_slalom_lookahead: 0.15
+nav_slalom_tolerance: 0.05
+```
+
+### 3. Yaw控制（新增）
+```yaml
+nav_goal_yaw_tolerance_deg: 8.0
+nav_slalom_script_yaw_gate_deg: 8.0
+```
+
+---
+
+## 🔍 工作原理
+
+### Pure Pursuit算法流程
+```
+1. 看前方lookahead距离的目标点
+2. 计算到目标的方向和距离
+3. 检查yaw偏差
+   - 如果 yaw偏差 > yaw_gate (8度):
+       只发wz转向，vx=0
+   - 如果 yaw偏差 ≤ yaw_gate (8度):
+       发vx前进 + wz微调
+4. 到达目标点，推进下一个
+```
+
+### 之前的问题
+```
+yaw_gate = 12度（太宽松）
+↓
+yaw偏差11度时就开始前进
+↓
+还没对准就冲出去
+↓
+斜着撞杆子
+```
+
+### 修改后
+```
+yaw_gate = 8度（更严格）
+↓
+yaw偏差必须≤8度才前进
+↓
+基本对准后才移动
+↓
+不会斜着撞
+```
+
+---
+
+## 📁 使用的文件
+
+**路线**: `points_nav1007_optimized.json`
+- 总航点: 47
+- 绕杆航点: 21
+- 参数: speed=0.50, lookahead=0.15, tolerance=0.05
+
+**地形**: `tools/nav_tools/xml/A.xml`
+
+**配置**: `runtime.yaml` (已修改)
+
+---
+
+## 🚀 下一步
+
+### 实机测试
+```bash
+# 1. 重启系统加载新配置
+ros2 launch sim2real_bringup sim2real_system.launch.py
+
+# 2. 验证参数
+ros2 param get /sim2real_simple_nav_node nav_slalom_script_yaw_gate_deg
+# 应该显示: 8.0
+
+ros2 param get /sim2real_simple_nav_node nav_goal_yaw_tolerance_deg
+# 应该显示: 8.0
+
+# 3. 加载路线
+# 当前归档默认使用: sim2real_ros2_v3/map/routes/1hao_reall.json
+
+# 4. 监控
+ros2 topic echo /cmd_vel_nav
+# 观察: 转向时vx应该接近0，对准后才有vx速度
+```
+
+---
+
+## 💡 预期效果
+
+**修改前**:
+- 机器人边转边进
+- yaw偏差大时仍有前进速度
+- 导致斜着撞杆子
+
+**修改后**:
+- 转向时几乎不前进（vx≈0）
+- 对准后才快速前进（vx=0.5）
+- 动作分离：先转向，后前进
+
+---
+
+## ⚠️ 如果还有问题
+
+### 场景A: 还是斜着撞
+可能需要进一步收紧:
+```yaml
+nav_slalom_script_yaw_gate_deg: 5.0  # 改为5度
+```
+
+### 场景B: 太慢，一直在转
+说明yaw_gate太严格:
+```yaml
+nav_slalom_script_yaw_gate_deg: 10.0  # 放宽到10度
+```
+
+### 场景C: 卡顿
+检查是否在等待yaw对准:
+```bash
+ros2 topic echo /simple_nav/status
+# 看是否一直在"aligning"状态
+```
+
+---
+
+**配置已优化完成，ready for testing!** 🎯