# 自定义 Hybrid A* 导航操作手册

本手册适用于仓库中的实车启动入口 `real_hybrid_astar.launch.py`。系统链路为：

`STM32 底盘 -> EKF 里程计 -> AMCL/地图 -> 激光雷达 -> Hybrid A* -> Pure Pursuit -> 阿克曼命令转换`

## 1. 安全和前置条件

开始前确认：

1. 机器人架起或周围有足够空旷区域，首次测试使用低速参数。
2. 底盘串口、波特率和雷达设备路径正确。
3. 真实地图 YAML 与对应 PGM/PNG 文件存在，地图坐标系为 `map`。
4. 已测得机器人最小转弯半径，并在 `real_hybrid_astar.yaml` 中配置。
5. 机器人初始位置在地图的可通行区域内。
6. 只启动本手册中的导航链路。不要同时启动 Nav2 `controller_server`、
   `origincar_bringup`、`path_follower_demo.py` 或 `udp_to_cmdvel.py`，否则会有多个
   节点同时发布 `/cmd_vel`。

仓库中的 `maps/real_5x5.yaml` 是空房间合成地图，只用于软件链路测试，不能作为真实场地地图。

## 2. 编译工作空间

在机器人电脑上执行：

```bash
cd ~/liao_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install \
  --packages-select \
  origincar_msg origincar_description origincar_base \
  lslidar_msgs lslidar_driver planner
source install/setup.bash
```

如果只修改了 `planner`，也可以只编译该包及其依赖。每个新终端都要重新执行：

```bash
source /opt/ros/humble/setup.bash
source ~/liao_ws/install/setup.bash
```

## 3. 检查地图

地图 YAML 和图像必须成对存在。先检查尺寸、分辨率和原点：

```bash
cd ~/liao_ws
python3 src/planner/scripts/map_info.py /实际路径/map.yaml
```

仓库测试地图的检查命令：

```bash
python3 src/planner/scripts/map_info.py \
  src/planner/maps/real_5x5.yaml
```

5×5 米地图应显示 `size: 5.000 x 5.000 m` 和 `PASS`。真实地图尺寸不是 5×5 米时，启动前记录实际宽高，后面传给 `preflight_check.py`。

## 4. 启动导航系统

启动前替换地图、雷达参数和底盘串口：

```bash
ros2 launch planner real_hybrid_astar.launch.py \
  map:=/实际地图/map.yaml \
  lidar_params:=/实际雷达参数.yaml \
  lidar_serial_port:=/dev/ttyCH343USB0 \
  lidar_model:=N10 \
  start_lidar:=true \
  serial_port:=/dev/实际底盘串口 \
  serial_baud:=921600 \
  enable_motion:=false
```

使用仓库合成地图进行软件测试：

```bash
ros2 launch planner real_hybrid_astar.launch.py \
  map:=$HOME/liao_ws/src/planner/maps/real_5x5.yaml \
  lidar_params:=/实际雷达参数.yaml \
  lidar_serial_port:=/dev/ttyCH343USB0 \
  lidar_model:=N10 \
  start_lidar:=true \
  serial_port:=/dev/实际底盘串口 \
  serial_baud:=921600 \
  enable_motion:=false
```

保持该终端运行并观察日志。启动入口已经包含底盘、EKF、雷达、地图服务器、AMCL、规划器、Pure Pursuit 和 `/cmd_vel` 到 `/ackermann_cmd` 的转换节点。首次启动保持 `enable_motion:=false`，此时只验证规划，不执行路径。

如果已经通过 `ros2 launch lslidar_driver ...` 单独启动雷达，必须把 `start_lidar:=true` 改为 `start_lidar:=false`，避免两个进程争抢同一串口。

## 5. 检查 ROS 接口

打开第二个终端并加载环境：

```bash
source /opt/ros/humble/setup.bash
source ~/liao_ws/install/setup.bash
```

依次检查：

```bash
ros2 topic echo /scan --once
ros2 topic echo /map --once
ros2 topic echo /odom_combined --once
ros2 run tf2_ros tf2_echo map base_footprint
```

确认以下条件：

- `/scan` 持续有激光数据；
- `/map` 的 `header.frame_id` 为 `map`；
- `/odom_combined` 持续更新；
- `map -> base_footprint` 和 `base_footprint -> laser` TF 可用。

## 6. 初始化 AMCL 位姿

在 RViz 中使用 **2D Pose Estimate** 设置机器人当前位置，或命令行发布（将坐标替换为实际值）：

```bash
ros2 topic pub --once /initialpose \
  geometry_msgs/msg/PoseWithCovarianceStamped \
  "{header: {frame_id: map}, pose: {pose: {position: {x: 0.54, y: 0.20}, orientation: {w: 1.0}}, covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.10]}}"
```

等待 AMCL 稳定，确认 `map -> base_footprint` TF 连续可查。

## 7. 运行启动前检查

默认按 5×5 米地图检查：

```bash
ros2 run planner preflight_check.py
```

检查通过后才允许发送目标。预检还会确认 `/cmd_vel` 只有一个发布者，并检查 `/ackermann_cmd` 转换节点与底盘订阅者。真实地图尺寸不同，例如 5.45×5.75 米：

```bash
ros2 run planner preflight_check.py --ros-args \
  -p expected_map_width:=5.45 \
  -p expected_map_height:=5.75
```

检查失败时，不要发送目标，先根据错误补齐地图、雷达、里程计或 TF。

## 8. 发布目标点

目标点使用 `map` 坐标系，单位为米：

```bash
ros2 run planner send_goal.py 1.00 1.00
```

第三个参数可指定终点航向角（弧度）：

```bash
ros2 run planner send_goal.py 1.00 1.00 1.57
```

默认只要求到达目标位置，位置误差小于约 0.15 米即停止。状态和路径不是持久话题，应先在其他终端启动监视，再发送目标：

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

只规划模式下，正常结果是规划状态 `plan_ok`、`/plan` 出现路径，且机器人不运动。`enable_motion:=true` 时，Pure Pursuit 才会发布实际速度，经过阿克曼转换后由底盘执行，机器人到达目标后停车。

仓库地图的坐标范围是 `[-2.5, 2.5)`，目标必须严格位于范围内部并避开膨胀后的边界墙，不能使用 `(2.5, 2.5)`。

首次只规划测试完成后，先取消目标并停止启动文件。确认转向方向、轴距和最大转角正确，再重新启动并显式允许低速运动：

```bash
ros2 launch planner real_hybrid_astar.launch.py \
  map:=/实际地图/map.yaml \
  lidar_params:=/实际雷达参数.yaml \
  lidar_serial_port:=/dev/ttyCH343USB0 \
  lidar_model:=N10 \
  start_lidar:=true \
  serial_port:=/dev/实际底盘串口 \
  serial_baud:=921600 \
  wheelbase:=0.143 \
  max_steering_angle:=0.60 \
  enable_motion:=true
```

重新启动后必须再次初始化 AMCL、运行 `preflight_check.py`，然后发布一个距离较近且无障碍的目标。第一次闭环测试应架起驱动轮或准备硬件急停，确认前进、倒车和左右转向符号正确后再落地运行。

## 9. 取消导航和停车

取消当前目标必须使用导航取消话题：

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

该命令会清除规划目标和跟踪路径，并持续保持停车。只发布一次零速度 `/cmd_vel` 不可靠，因为跟踪器可能在 20 Hz 下重新覆盖它。

## 10. 常见问题

### `preflight_check.py` 报无 `/map`

检查 `map:=` 路径、YAML 引用的图像路径，以及 `map_server` 和生命周期管理器日志。

### 地图尺寸检查失败

用 `map_info.py` 查看实际尺寸，然后通过 `expected_map_width` 和 `expected_map_height` 传入真实尺寸。不要为了绕过检查而修改真实地图参数。

### 无 `/scan` 或雷达 TF

检查 `lidar_params` 是否匹配雷达型号、串口权限和 `base_footprint -> laser` 静态 TF。

### 无 `map -> base_footprint`

先在 RViz 设置 AMCL 初始位姿，确认 `/scan` 与地图大致重合；没有有效定位时禁止自动驾驶。

### `/cmd_vel` 发布者数量不是 1

执行：

```bash
ros2 topic info /cmd_vel -v
```

关闭重复启动的控制器，只保留 `topology_pure_pursuit` 发布速度指令。

### 规划失败或机器人原地不动

检查起点和目标是否在空闲区域，地图障碍物膨胀是否过大，机器人最小转弯半径是否填写正确，并查看 `/hybrid_astar_status` 的错误信息。

## 11. 结束运行

先发送 `/navigation_cancel`，确认机器人停止，再在启动终端按 `Ctrl+C` 关闭节点。实车测试结束后断开驱动电源或使底盘进入硬件急停状态。
