docs: add localization system documentation

Comprehensive reference for the LiDAR + AMCL + teach-in localization stack. Covers TF tree, architecture, phased implementation plan, configuration, and teach-in/navigation workflow for the CRAIC desktop competition.

Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-openagent)

Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>

docs/localization.md

@@ -0,0 +1,253 @@
+# 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：示教
+q
+```
+交互命令：**回车/`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 变了需重新示教**，建议重建图 |
+
+通用排查：`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`（同赛事场地/规划参考）