# Hybrid A* 与 Pure Pursuit 实车测试方案

## 1. 测试目标

分别验证两个对象：

1. Hybrid A* 是否能在规定时间内生成无碰撞、满足最小转弯半径的路径。
2. Pure Pursuit 是否能稳定跟踪该路径，在到点、取消、定位失效时可靠停车。

测试必须按阶段执行。前一阶段未通过，不进入下一阶段。

## 2. 安全规则

- 至少两人参与：一人操作电脑，一人负责硬件急停。
- 第一次运动测试必须架起驱动轮。
- 落地测试速度保持 `linear_speed <= 0.20 m/s`、`reverse_speed <= 0.10 m/s`。
- 禁止同时启动 Nav2 controller、`origincar_bringup`、巡线、UDP 控制或其他 `/cmd_vel` 发布节点。
- 合成 `real_5x5.yaml` 只允许做不运动的规划测试。除非真实场地就是对应的 5×5 米空房间，否则禁止用它进行自动驾驶。
- ROS 取消不是硬件急停。任何异常首先使用硬件急停。

## 3. 测试输出

启动记录器：

```bash
ros2 run planner navigation_test_recorder.py --ros-args \
  -p test_name:=P1_straight \
  -p output_dir:=$HOME/yiliao_ws/test_results
```

测试结束后按 `Ctrl+C`，得到：

```text
test_results/P1_straight_时间.csv
test_results/P1_straight_时间.json
```

JSON 中的关键指标：

| 字段 | 含义 |
|---|---|
| `planning_latency_sec` | 从目标到收到路径的延迟 |
| `path_occupied_points_max` | 路径落入原始占用/未知栅格的点数 |
| `minimum_planned_turning_radius_m` | 路径估计最小转弯半径 |
| `cross_track_error_m` | 实车到当前路径的横向误差 |
| `final_goal_error_m` | 记录结束时的目标距离 |
| `cancel_to_zero_command_sec` | 取消到收到零 `/cmd_vel` 的延迟 |
| `transform_failures` | 记录期间 TF 查询失败次数 |

每个落地用例至少重复 5 次，保留全部 CSV/JSON，不只保留最好结果。

自动评估单个或多个结果：

```bash
ros2 run planner evaluate_navigation_test.py planning \
  $HOME/yiliao_ws/test_results/P[1-4]*.json

ros2 run planner evaluate_navigation_test.py tracking \
  $HOME/yiliao_ws/test_results/T*.json
```

评估器按本手册的默认阈值返回 `PASS` 或 `FAIL`，退出码分别为 0 或 1。需要调整验收指标时使用脚本的命令行参数，不要修改原始测试结果。

## 4. 阶段 0：硬件与定位门槛

### 4.1 雷达

当前 N10 参数文件默认使用 `/dev/ttyCH343USB0`。先确认：

```bash
grep -E "lidar_name|interface_selection|serial_port_" \
  $HOME/yiliao_ws/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml

ls -l /dev/ttyACM* /dev/ttyUSB* /dev/ttyCH343USB* 2>/dev/null
id -nG
```

任何时刻只允许一个 `lslidar_driver_node`。如果雷达已单独启动，启动 planner 实车入口时使用 `start_lidar:=false`。

启动后检查：

```bash
ros2 param get /lslidar_driver_node lidar_name
ros2 param get /lslidar_driver_node interface_selection
ros2 param get /lslidar_driver_node serial_port_
ros2 topic hz /scan
ros2 run planner lidar_health_check.py
```

通过标准：连续 60 秒有 `/scan`，频率不低于雷达标称频率的 80%，无持续设备断连。

旧版雷达驱动可能反复打印笼统的 `error`。是否阻塞测试以 `/scan` 健康检查为准：健康检查通过时可继续只规划测试；没有 `/scan` 或健康检查失败时必须先处理设备路径、权限、占用或供电问题。

### 4.2 底盘、地图与 TF

```bash
ros2 topic hz /odom
ros2 topic hz /odom_combined
ros2 run tf2_ros tf2_echo odom base_footprint
ros2 run tf2_ros tf2_echo map base_footprint
ros2 run planner preflight_check.py
```

通过标准：

- `/odom_combined` 稳定接近 20 Hz；
- `odom -> base_footprint` 连续；
- 设置 AMCL 初始位姿后 `map -> base_footprint` 连续；
- `preflight_check.py` 返回成功；
- `/cmd_vel`、`/ackermann_cmd` 的发布/订阅数量均符合预检要求。

## 5. 阶段 1：只规划测试

启动时必须使用：

```text
enable_motion:=false
```

启动记录器和三个监视终端：

```bash
ros2 topic echo /hybrid_astar_status
ros2 topic echo /pure_pursuit_status
ros2 topic echo /plan
```

以测试地图中心附近 `(0, 0, 0)` 为起点，执行以下用例：

| 用例 | 操作 | 预期 |
|---|---|---|
| P1 直线路径 | `send_goal.py 1.0 0.0` | 非空路径，无倒车段 |
| P2 转弯路径 | `send_goal.py 1.0 1.0` | 曲率连续且无占用点 |
| P3 后方目标 | `send_goal.py -0.8 0.0` | 允许出现倒车段，不频繁换挡 |
| P4 锥桶重规划 | 路径发布后在路径中部发布锥桶 | 2 Hz 内重规划或安全清空路径 |
| P5 目标占用 | 把目标放在边界墙上 | 拒绝规划并发布空路径 |
| P6 取消 | 路径生成后发布取消 | 规划器和跟踪器都报告取消 |

锥桶命令示例：

```bash
ros2 topic pub --once /random_obstacles \
  std_msgs/msg/Float32MultiArray "{data: [0.5, 0.0, 0.15]}"

ros2 topic pub --once /clear_random_obstacles \
  std_msgs/msg/Bool "{data: true}"
```

规划器通过标准：

- 自由起终点成功率 >= 95%（至少 20 次）；
- 规划延迟 p95 <= 0.50 秒，最大值 <= 1.0 秒；
- `path_occupied_points_max == 0`；
- 规划最小转弯半径 >= 配置值的 90%（当前为 0.36 米）；
- 目标占用或无路时必须清空危险旧路径；
- 锥桶更新到新路径/空路径的时间 <= 0.75 秒；
- `enable_motion:=false` 时底盘不得运动。

## 6. 阶段 2：架空轮控制测试

架起驱动轮并准备硬件急停，然后用 `enable_motion:=true` 重启。重新初始化 AMCL 并运行预检。

依次测试短距离前进、左转、右转、倒车，观察：

```bash
ros2 topic echo /cmd_vel
ros2 topic echo /ackermann_cmd
```

通过标准：

- 正速度对应车轮前进，负速度对应车轮倒转；
- 正转角和负转角分别对应预期左右方向；
- `/ackermann_cmd.drive.steering_angle` 不超过 `0.60 rad`；
- 无命令时不持续转动；
- 停止 `/cmd_vel` 后，planner 内的桥接看门狗在 0.25 秒左右将 `/ackermann_cmd` 归零；
- 发布 `/navigation_cancel` 后 0.25 秒内命令归零。

任何符号相反都必须先修正，禁止通过交换测试目标来掩盖问题。

## 7. 阶段 3：低速落地跟踪

必须换成与真实环境匹配的地图。每次用例流程：

1. 将机器人放在标记起点。
2. 设置 AMCL 初始位姿并等待扫描与地图重合。
3. 运行 `preflight_check.py`。
4. 启动记录器。
5. 发布目标，保持急停人员就位。
6. 到达或失败后立即取消，停止记录器。
7. 重复 5 次。

推荐用例：

| 用例 | 路径特征 | 距离 |
|---|---|---|
| T1 | 直线 | 1.0 米 |
| T2 | 单个 90 度转弯 | 1.0 至 1.5 米 |
| T3 | S 形连续转弯 | 2.0 至 3.0 米 |
| T4 | 包含倒车 | 0.5 至 1.0 米 |
| T5 | 锥桶触发重规划 | 1.5 至 2.0 米 |

跟踪器基线通过标准：

- 到点成功率 >= 90%（每类至少 5 次，总计至少 25 次）；
- 横向误差 RMS <= 0.12 米；
- 横向误差 p95 <= 0.20 米；
- 最大横向误差 <= 0.30 米；
- 最终位置误差 <= 0.15 米；
- 无碰撞、压线、失控或错误换挡；
- 定位/里程计过期时能停车；
- 取消到零命令 p95 <= 0.25 秒；
- TF 查询失败为 0。

这些是当前 0.20 m/s 低速配置的基线。如果场地测量精度不足，应先改善定位，不应直接放宽安全指标。

## 8. 阶段 4：故障与恢复

| 故障注入 | 合格行为 |
|---|---|
| 拔掉雷达或停止 `/scan` | AMCL 不可信，不再发送新目标；人工取消 |
| 暂停 `/odom_combined` | 跟踪器在 `max_odom_age` 后发布零速度 |
| 发布路径上的锥桶 | 安全重规划；无安全路径则停车 |
| 发布 `/navigation_cancel` | 清目标、清路径并停车 |
| 启动第二个 `/cmd_vel` 发布者 | 预检失败，禁止测试 |
| 目标在地图外或障碍物内 | 空路径并报告拒绝/失败 |

关键安全项出现一次失败，即本轮版本不合格，修复后从阶段 1 重新回归。

## 9. 最终判定

只有同时满足以下条件，才能认为规划与跟踪方案适合当前低速实车：

1. 阶段 0 全部通过。
2. 规划成功率、延迟、碰撞和转弯半径全部达标。
3. 25 次以上落地测试满足成功率与误差指标。
4. 取消、定位过期和无路场景均能可靠停车。
5. 没有任何碰撞、失控、错误方向或多控制器竞争。
