craic/docs/localization.md

# CRAIC 定位系统文档

激光 SLAM 建图 + AMCL 精准定位 + 示教记录点位。移植自 `lzu_robot` / `move_try`
（同一赛事的参考实现），适配 CRAIC 麦轮底盘 + ESP32 + 机械臂平台。

---

## 1. 概述

- **目标**：在已知（预先测量）的竞赛场地上对底盘做绝对、无累积漂移的精准定位，并提供示教记录关键点位的工具。
- **方案**：YDLiDAR → `rf2o` 激光里程计提供连续 `odom`；`slam_gmapping` 建一次场地地图；`nav2 AMCL` 用激光匹配已知地图做全局校正。
- **运行环境**：ROS 2 Humble（conda/robostack `ros2_humble`）。机器人端工作区在 `~/Desktop/ros2`（下文命令默认在此目录、且已 `source install/setup.bash`）。

> 设计上原打算用**轮式里程计**做运动来源，但实测 CRAIC 底盘板上电后只短时上报里程计（见 §8），
> 故改用 `rf2o`（激光里程计）提供 `odom→base`，与 `lzu_robot`/`move_try` 一致。轮式里程计节点保留，
> 待底盘板问题解决后可用 EKF 融合提精度。

---

## 2. 系统架构

### TF 树
```
map ──(amcl，激光匹配已知地图)──> odom
odom ──(rf2o，来自连续 /scan)──────> base_footprint
base_footprint ──(静态外参)────────> laser_frame      # 激光安装位，lidar_yaw 等参数
base_footprint ──(静态恒等)────────> base_link        # 兼容 gmapping/Nav2 默认基准帧
```

### 数据流
```
YDLiDAR ──/dev/ttylzulaser──> ydlidar 驱动 ──> /scan(10Hz)
/scan ──> rf2o ──> /odom + TF(odom→base_footprint)
/scan + odom ──> slam_gmapping ──> /map + TF(map→odom)        # 建图阶段
/scan + 已知地图 ──> amcl ──> TF(map→odom) = 绝对位姿            # 定位阶段
底盘速度命令: keyboard_control ──UDP──> ESP32 ──UART──> 底盘板   # 与定位互不干扰
轮式里程计(可选): 底盘板 ──/dev/ttylzucar──> chassis_odometry ──> /odom(独立测试用)
```

---

## 3. 软件清单

### 自建包 `ros2/src/craic_localization`
| 文件 | 作用 |
|---|---|
| `craic_localization/chassis_odometry.py` | 轮式里程计节点（读 `/dev/ttylzucar`，发 `/odom`+TF）。**默认不参与建图/定位**，独立测试用 |
| `craic_localization/teach_points.py` | 示教节点：记录 map 系下位姿存 YAML |
| `craic_localization/navigate_to_point.py` | 导航节点：自动驱动到示教点（读 taught_points.yaml + AMCL 闭环 + UDP 发 XYW） |
| `craic_localization/show_points.py` | 在 rviz 显示所有示教点位（MarkerArray 箭头+文字标签） |
| `launch/lidar.launch.py` | YDLiDAR 驱动 + 静态外参(base_footprint→laser_frame、→base_link) |
| `launch/bringup.launch.py` | 轮式里程计 + 激光 + rviz（传感器自检） |
| `launch/mapping.launch.py` | 激光 + rf2o + slam_gmapping（建图） |
| `launch/localization.launch.py` | 激光 + rf2o + map_server + amcl + lifecycle_manager（AMCL 定位） |
| `config/lidar.yaml` | YDLiDAR TminiPro 参数（端口 `/dev/ttylzulaser`） |
| `config/gmapping.yaml` | gmapping 建图参数（`/**` 通配键，base_footprint/odom/map） |
| `config/amcl.yaml` | map_server + amcl 参数（omni 运动模型） |
| `rviz/localization.rviz` | rviz 配置（TF/Map/LaserScan/Odometry） |
| `maps/` | 建图存盘输出（`craic.pgm` + `craic.yaml`） |
| `config/taught_points.yaml` | 示教输出（运行时生成） |

### 移植包
- `ros2/src/ydlidar_ros2_driver` —— 来自 `move_try`，依赖系统 `ydlidar_sdk`（已装在 conda env）。
- `ros2/src/rf2o_laser_odometry` —— 来自 `lzu_robot`，激光里程计（ICRA'16）。

### 外部依赖（Nav2，已装入 conda env）
`nav2_amcl`(amcl)、`nav2_map_server`(map_server, map_saver_cli)、`nav2_lifecycle_manager`(lifecycle_manager)、`slam_gmapping`/`openslam_gmapping`。

---

## 4. 环境准备与编译

一次性安装 Nav2 定位组件（若环境无）：
```bash
conda install -n ros2_humble -c robostack-staging -c conda-forge \
    ros-humble-nav2-amcl ros-humble-nav2-map-server ros-humble-nav2-lifecycle-manager
```

编译（在工作区根目录）：
```bash
conda activate ros2_humble
cd ~/Desktop/ros2
colcon build --symlink-install --packages-select \
    ydlidar_ros2_driver rf2o_laser_odometry craic_localization
source install/setup.bash
```

> **开发机 ↔ 机器人同步**：代码在开发机维护，需同步到机器人 `~/Desktop/ros2` 后**重新 colcon build**。
> 新增/改动包时务必确认源码已同步（尤其新包 `ydlidar_ros2_driver`、`rf2o_laser_odometry`）。

---

## 5. 硬件接口

| 设备 | 接口 | 说明 |
|---|---|---|
| 底盘 MCU | `/dev/ttylzucar` @115200 | 上报 32 字节里程计包；命令通过 ESP32 转发，非直连 |
| 激光雷达 | `/dev/ttylzulaser` @230400 | YDLiDAR TminiPro，发 `/scan` 10Hz |
| 底盘命令 | UDP `192.168.4.1:8888` → ESP32 → UART | `XYW:<x>:<y>:<w>:XZHY`（麦轮速度） |

**底盘里程计协议**（移植自 lzu seriallzucar）：32 字节，帧尾 `'L''Z''U'`；`packet[0]`=校验和=`sum(packet[1:30])%256`；
`packet[1:29]`=`<fffffff>`=`x,y,yaw,4轮位置`(x/y 单位 mm)；`packet[29]`=模式字节，`'D'`=有效。
打开串口后下发 `UP0LZU` 使能上报（参数 `enable_cmd`）。

---

## 6. 使用流程

> 前置：每个终端都先 `cd ~/Desktop/ros2 && source install/setup.bash`。

### 6.1 传感器自检（可选）
```bash
ros2 launch craic_localization bringup.launch.py
```
rviz 看 `/scan`、TF。用于核对激光朝向（调 `lidar_yaw` 等，见 §7）。

### 6.2 建图
```bash
# 终端1：建图栈（激光 + rf2o + gmapping + rviz）
ros2 launch craic_localization mapping.launch.py
# 终端2：遥控慢速走遍场地（见 6.6）
# 终端3：地图满意后存盘（gmapping 保持运行）
mkdir -p src/craic_localization/maps
ros2 run nav2_map_server map_saver_cli \
    -f src/craic_localization/maps/craic \
    --ros-args -p save_map_timeout:=10000.0
```
**走图要点**：慢、贴墙走遍每条边界、最后绕回起点；rviz Fixed Frame 设 `map`，墙应是单条清晰线、无重影。
**起点即原点**：建图启动时机器人所在位置=map(0,0)、正前方=map +X。建议从固定起点开始。

存盘得到 `craic.pgm` + `craic.yaml`。**确认 `craic.yaml` 含 `free_thresh`**（缺则 map_server 加载失败）：
```bash
tail -1 src/craic_localization/maps/craic.yaml   # 应为 free_thresh: 0.25
# 若缺： printf '\nfree_thresh: 0.25\n' >> src/craic_localization/maps/craic.yaml
```
存盘后重编一次让默认地图安装：`colcon build --symlink-install --packages-select craic_localization`。
> 地图四周大片灰色是 gmapping 预留画布（`xmin/xmax ±10` → 20m×20m），属正常，不影响 AMCL。

### 6.3 定位（AMCL）
```bash
ros2 launch craic_localization localization.launch.py \
    map:=$HOME/Desktop/ros2/src/craic_localization/maps/craic.yaml
# 起点不在地图原点时给初值：加 init_x:= init_y:= init_yaw:=，或 rviz 工具栏 "2D Pose Estimate" 点
```
正常表现：rviz(Fixed Frame=`map`) 红色激光点贴合墙体，移动时始终咬住墙、位姿稳定不漂。

### 6.4 定位精度测试
```bash
ros2 run tf2_ros tf2_echo map base_footprint    # 实时读 x/y/yaw
ros2 topic echo /amcl_pose                       # 位姿 + 协方差
```
- **重复定位**：标记一物理点记位姿 → 开一圈回同点再记 → 差值 < 3cm 为佳。
- **定长移动**：精确前进 1.00m，AMCL 报的位移误差应在几 cm 内。
- (可选) rviz Add → PoseArray → `/particlecloud`，粒子收紧=收敛。

### 6.5 示教记录点位
```bash
# 终端1：定位栈(6.3) 终端2：遥控(6.6) 终端3：示教
ros2 run craic_localization teach_points --ros-args -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml
```
交互命令：**回车/`r`** 记录当前预设点并前进；`p` 查看当前位姿；`name <X>` 记自定义点；
`del <X>` 删除；`list` 列出；`skip`/`back` 跳过/回退；`save` 存盘；`q` 退出。
预设顺序 `B_1..B_6, C_1, D_1, E_1, F_1`（每记一点自动存盘）。记 A 区用 `name A_1`…，或改 `points` 参数。
输出（map 系，米/弧度）：
```yaml
points:
  B_1: {x: 1.234, y: 0.456, yaw: 1.571}
  ...
```
**在 rviz 看示教点**：`localization.launch.py` 已自动启动 `show_points`（青色箭头=位置+朝向，白字=点名，话题 `/taught_points`，rviz 内置 MarkerArray 显示）。
想在示教**过程中实时**看到新点，给定位 launch 传 `points_file:=<你的 taught_points.yaml 绝对路径>`，或单独运行：
```bash
ros2 run craic_localization show_points --ros-args \
    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml
```

### 6.6 自动导航到示教点
读取 `taught_points.yaml`，用 AMCL 闭环把底盘开到指定点（麦轮 holonomic P 控制，经 UDP 发 XYW）。
**首次务必先 dry_run 验证方向，再低速实测，手放 Ctrl+C（退出自动停车）。** 前置：定位栈(6.3) 在运行。
```bash
# ① 干跑：只打印命令不发，确认逻辑与方向符号
ros2 run craic_localization navigate_to_point --ros-args \
    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml \
    -p dry_run:=true -p goal:=B_1
# ② 交互选点（低速实测）
ros2 run craic_localization navigate_to_point --ros-args \
    -p points_file:=$PWD/src/craic_localization/config/taught_points.yaml \
    -p max_linear:=40.0 -p max_angular:=20.0
# ③ 一次性去某点 / 供上层任务程序用话题指定
ros2 run craic_localization navigate_to_point --ros-args -p points_file:=... -p goal:=C_1
ros2 topic pub -1 /goto std_msgs/String "{data: 'C_1'}"
```
交互命令：输入点名→导航；`s` 停车；`q` 退出。到达 `pos_tolerance`(5cm)/`yaw_tolerance`(~3°) 容差内停车。
**若实车方向相反**：翻转 `sign_x`/`sign_y`/`sign_w`（默认 -1，按 keyboard_control 推导）。如左右反了 `-p sign_x:=1`。

### 6.7 遥控
```bash
ros2 run udp_teleop keyboard_control --ros-args \
    --params-file src/udp_teleop/config/params.yaml \
    -p chassis_linear_speed:=40 -p chassis_angular_speed:=20    # 建图建议降速
```
按键：`W/S` 前后、`A/D` 左右平移、`Q/E` 左右转；`Ctrl+C` 退出。须 `ros2 run`、交互终端。

---

## 7. 配置 / 关键参数

**`lidar.launch.py`**：`lidar_x/y/z`、`lidar_yaw`(默认 -3.14159=-180°，使机体 X 轴对准底盘实际前进方向；若前后颠倒改 0.0)、
`intensity`(true；基础 Tmini 无强度时改 false)、`sample_rate`(4)、`baudrate`(230400)、`base_frame`、`laser_frame`。

**`localization.launch.py`**：`map`(地图 yaml)、`init_x/init_y/init_yaw`(初始位姿)、`use_rviz`。

**`mapping.launch.py`**：`use_rviz`。范围 `xmin/xmax/ymin/ymax`(±10) 在 `config/gmapping.yaml`，可收小到 ±3 得更紧凑地图（需重新建图）。

**`amcl.yaml`**：`odom_model_type: omni`(麦轮)、`base_frame_id: base_footprint`、`laser_max_range: 12.0`、`alpha1..5`(里程计噪声)。

**`chassis_odometry`**（独立测试）：`port`、`enable_cmd`(UP0LZU)、`enable_cmd_period`(0=仅打开时发一次)、`require_mode_d`(true)。

**`navigate_to_point`**：`points_file`、`goal`(一次性目标)、`max_linear`(60)/`max_angular`(30)、`kp_linear`(150)/`kp_angular`(40)、`pos_tolerance`(0.05)/`yaw_tolerance`(0.05)、`sign_x/sign_y/sign_w`(默认 -1，方向修正)、`goal_timeout`(30s)、`dry_run`、`udp_ip/udp_port`。

---

## 8. 故障排查

| 现象 | 原因 | 处理 |
|---|---|---|
| 激光狂刷 `Check Sum X != Y` | 有第二个进程在读 `/dev/ttylzulaser`，字节流被撕裂 | `pgrep -af ydlidar`；`pkill -f ydlidar_ros2_driver_node`；保证单实例。若仍报：试 `intensity:=false`（Tmini 无强度） |
| gmapping `"base_link" ... does not exist` | gmapping 硬编码基准帧 `base_link`，而 TF 树只有 base_footprint | 已修：`lidar.launch.py` 发 base_footprint→base_link 恒等 TF |
| gmapping `Message Filter ... queue is full`、地图/位置不动 | `odom→base` TF 冻结：轮式里程计板上电后只短时上报 | 已修：改用 `rf2o` 从 /scan 连续提供 odom→base（见 §1） |
| map_server 加载地图失败 | `craic.yaml` 缺 `free_thresh` | 补 `free_thresh: 0.25`（见 6.2） |
| rviz 地图上一串绿箭头 | Odometry 显示保留历史(Keep) | 已改默认 `Keep:1`；或 rviz 里取消勾选 Odometry（不影响地图） |
| 轮式里程计只在上电后几帧有数据 | 底盘板上电后停止上报（固有行为，UP0LZU 未能维持） | 建图/定位已不依赖它；如需启用试 `enable_cmd_period:=1.0` 保活，或确认主机串口 TX 已接、或经 ESP32 通道使能 |
| 时间戳显示 ~2000 年 | 机器人系统时钟未对时 | 单机不影响；多机协同前用 NTP/RTC 对时 |
| 导航时往错误方向开 / 原地打转 | XYW 速度符号与实车不一致 | 翻转 `sign_x/sign_y/sign_w`(默认 -1)；先 `dry_run:=true` 核对，再低速实测 |
| rviz 里机体朝向/前进方向与遥控差 90° | 激光外参 `lidar_yaw` 未校准 | 设 `lidar_yaw=-180°`(前后颠倒则 0.0)；改后 **yaw 变了需重新示教**，建议重建图 |
| rviz 里示教点/地图整体乱飘跳动(机器人/激光却平滑) | Fixed Frame 设成了 `odom`，map 帧内容随 AMCL 每次校正跳变 | rviz Fixed Frame 改 `map`(已设为默认) |
| 平移一段再返回，回位坐标误差大(静止时稳定) | 激光里程计 rf2o 运动跟踪偏差 + 麦轮物理漂移；**非地图大小问题** | 慢速驱动；根治用轮式里程计+EKF(见 §9)；先做物理标记/对比 rf2o 与 AMCL 定位区分原因 |
| 激光点云形状对、但整体偏离墙线，AMCL 不往墙上贴 | `sigma_hit` 太小(0.05)→偏差超几 cm 就无梯度，AMCL 无法纠正 | 调大 `sigma_hit`(0.2)、`z_hit`(0.9)、`laser_max_range`(≥场地对角线)、`laser_likelihood_max_dist`(2.0)；并给准初始位姿 |

通用排查：`ros2 run tf2_ros tf2_echo map base_footprint`、`ros2 topic hz /scan /odom /map`、
`ros2 run tf2_tools view_frames`（看 TF 树连通）。

---

## 9. 已知限制与后续

- **激光外参标定**：`lidar_yaw` 已校准为 -180°，使机体 X 轴对准底盘实际前进方向（rviz 显示与遥控一致，导航默认符号 `sign_x/y/w=-1` 也随之正确）。平移 `lidar_x/y` 仍为 0 占位，需要更高位置精度可再标。
  **改动 `lidar_yaw` 后**：机体朝向变了 —— 旧地图仍可定位（AMCL 自动补偿），但**示教点的 yaw 需重新记录**，建议重新建图让地图朝向也一致。
- **轮式里程计**：底盘板“持续上报”问题待解；解决后可用 `robot_localization` EKF 融合 轮速 + rf2o 再喂 AMCL 提精度。
- **地图坐标系**：`map(0,0)` = 建图起点。比赛复现时让机器人从同一物理起点开机，或用 “2D Pose Estimate” 给初值。

---

## 10. 参考来源
- `lzu_robot/src/cgbot/cgbot/seriallzucar.py`（里程计协议）、`maps/amcl_config.yaml`、`src/slam_gmapping`、`src/rf2o_laser_odometry`、`src/ydlidar_ros2_driver-humble`
- `move_try/src/ydlidar_ros2_driver/params/TminiPro.yaml`、`src/move_try`（同赛事场地/规划参考）