# yiliao_ws 项目说明

本文件是 `/home/sunrise/yiliao_ws` 工作区的项目级说明，主要记录当前比赛会用到的模块、接口、启动方式和调试注意事项。后续如果某个子目录内还有自己的 `AGENTS.md`，进入该子目录工作时以更近的说明为准。

## 项目定位

- 这是 RDKx5 机器人上的 ROS 2 Humble 工作区，当前主要服务于医疗赛道/竞速赛道任务。
- 当前比赛主链路是调度型架构：底盘、雷达、相机、二维码、Nav2、VLM、TTS 等功能分别由独立模块完成，`src/racing_control` 只负责总调度。
- 当前默认只走 Nav2 路径规划与跟随，`trajectory_guard` 只作为可选备用链路，不作为默认路径。
- 比赛点位、路线和行为开关应优先放在 YAML/launch 参数中，不要把新采集的场地点位直接写死进 C++。
- 修改比赛逻辑时优先保证流程稳定、有序、可调试；除非明确需要，不要在总调度节点里另起感知、规划或控制功能。

## 环境与访问

- 机器人 SSH：
  ```bash
  ssh sunrise@192.168.10.210
  ```
- 工作区根目录：
  ```bash
  /home/sunrise/yiliao_ws
  ```
- 运行 ROS 2 命令前通常需要：
  ```bash
  cd /home/sunrise/yiliao_ws
  source /opt/ros/humble/setup.bash
  source install/setup.bash
  ```
- 部分相机/Hobot 相关流程可能还需要：
  ```bash
  source /opt/tros/humble/setup.bash
  ```
- 远端仓库可能已有无关改动或运行日志。不要随意 `git reset`、`git checkout --` 或删除未确认文件。

## 编译与测试

- 编译单个包：
  ```bash
  cd /home/sunrise/yiliao_ws
  source /opt/ros/humble/setup.bash
  source install/setup.bash
  colcon build --packages-select <package_name> --cmake-args -DBUILD_TESTING=ON
  ```
- 测试 `racing_control`：
  ```bash
  cd /home/sunrise/yiliao_ws
  source /opt/ros/humble/setup.bash
  source install/setup.bash
  colcon test --packages-select racing_control
  colcon test-result --verbose --test-result-base build/racing_control
  ```
- 不要在同一个工作区里同时启动多个 `colcon build`。
- `ament_xmllint` 可能依赖远端 ROS schema。如果只有 xmllint 因网络/schema 临时失败，先重跑验证，再考虑改 XML。

## 比赛模块总览

### 总调度

- 包路径：`src/racing_control`
- 主节点：`src/racing_control/src/racing_control.cpp`
- 配置：`src/racing_control/config/racing_control.yaml`
- 启动：`src/racing_control/launch/racing_control.launch.py`
- 点位转换说明：`src/racing_control/点位格式转换.md`
- 设计原则：
  - 只做比赛流程调度，不重复实现二维码、VLM、Nav2、底盘控制等子功能。
  - 通过 `/sign4return` 控制二维码、VLM 和 Nav2 参数档位。
  - 通过 Nav2 action 完成到点、路径规划和路径跟随。
- 当前比赛流程：
  - 导航到 `qr_pose` 附近。
  - 一旦收到可解析二维码结果，立即选择顺/逆时针路线。
  - 识别二维码后不再等待 QR 目标，按配置前往 `post_qr_pose` 和 `entry_pose`。
  - 进入对应方向路线，按 `clockwise_waypoints` 或 `counterclockwise_waypoints` 执行。
  - 到第 `vlm_waypoint_number` 个路线点时触发 VLM。
  - home 前一个点到达后发布 `/sign4return=10` 调参，再发布 home goal。
  - 最后返回方向对应的 `clockwise_home_pose` 或 `counterclockwise_home_pose`。
- 使用的 Nav2 actions：
  - `/navigate_to_pose`
  - `/compute_path_through_poses`
  - `/follow_path`
- 常用启动参数：
  ```bash
  ros2 launch racing_control racing_control.launch.py auto_start:=false
  ros2 launch racing_control racing_control.launch.py auto_start:=true
  ros2 launch racing_control racing_control.launch.py enable_vlm_image_relay:=true
  ```

### 点位与路线参数

- 点位数组使用 `[x, y, yaw_radians]`，坐标系由 `frame_id` 指定。
- `racing_control.yaml` 中当前关键字段：
  - `qr_pose`：二维码区域导航点。
  - `post_qr_pose`：二维码识别后的后置过渡点，用于后续调参。
  - `entry_pose`：进入正式路线前的入口点。
  - `clockwise_waypoints`：顺时针路线点，不包含 home。
  - `counterclockwise_waypoints`：逆时针路线点，不包含 home。
  - `clockwise_home_pose`、`counterclockwise_home_pose`：最终 home 点。
- `use_post_qr_pose: true` 时启用 QR 后置点；设为 `false` 时二维码识别后直接去 `entry_pose`。
- `vlm_waypoint_number: 4` 表示正式路线第 4 个点是 VLM 拍摄点。不要依赖 `goal_004` 之类的点名。
- 二维码方向解析：
  - 文本包含 `顺` 或奇数数字时选择顺时针。
  - 文本包含 `逆` 或偶数数字时选择逆时针。

### Nav2 与轨迹保护

- 包路径：`src/navigation/obstacle_nav2`
- 主启动：`launch/obstacle_nav2.launch.py`
- 轨迹保护启动：`launch/trajectory_guard.launch.py`
- 运行时参数档位切换：`launch/nav2_profile_tuner.launch.py`
- 关键配置：
  - `config/nav2_profile_10.yaml`：普通/默认导航参数。
  - `config/nav2_profile_11.yaml`：任务二路线参数。
  - `config/trajectory_guard.yaml`：轨迹保护参数。
- `trajectory_guard_node` 订阅 `/trajectory_guard/input_path`，属于备用链路；默认比赛流程不依赖它。
- `nav2_profile_tuner` 监听 `/sign4return`，收到 10 或 11 后调用 Nav2 参数服务切换参数档位。
- `obstacle_nav2.launch.py` 当前以里程计坐标为主，默认 global frame 是 `odom`；静态地图和 AMCL 相关配置留作备用。

### 相机与二维码

- 相机包：`src/car_usb_cam`
- 二维码包：`src/qr_detection`
- 常见相机话题：`/image`
- 当前 VLM/调度链路期望的图像类型：`sensor_msgs/msg/CompressedImage`
- 二维码结果话题：`/qr_results`，类型 `std_msgs/msg/String`
- 二维码检测受 `/sign4return` 控制：
  - `0`：启用二维码检测。
  - `5`：关闭二维码检测。
- 常用检查命令：
  ```bash
  ros2 topic echo /qr_results
  ros2 topic info /image
  ros2 topic hz /image
  ```
- 如果 `racing_control` 到二维码区域后不继续走，优先检查：
  - 日志里是否出现 `QR result received`。
  - `/qr_results` 是否真的有输出。
  - 输出文本是否包含 `顺`、`逆` 或可解析数字。

### VLM 与语音

- 包路径：`src/vlm_detect`
- 启动文件：
  - `launch/vlm_detect.launch.py`：OpenAI 兼容 VLM 服务流程。
  - `launch/local_vlm_adapter.launch.py`：本地 `hobot_llamacpp` 适配流程。
- VLM 触发信号：`/sign4return=9`
- VLM 结果话题：`/vlm_result`
- TTS 服务：`/tts/speak`，类型 `origincar_msg/srv/Speak`
- `vlm_detect` 的图像输入可通过 launch 参数 `image_topic` 配置。
- `racing_control` 可选单帧 VLM 图像转发：
  - `enable_vlm_image_relay: false` 默认关闭。
  - 输入话题：`vlm_image_input_topic`，默认 `/image`。
  - 输出话题：`vlm_image_output_topic`，默认 `/vlm_image`。
  - 启用后，`racing_control` 在发布 `/sign4return=9` 前，把缓存到的一帧压缩图像发布到 `/vlm_image`。
  - 若要让 VLM 使用该帧，VLM 需这样启动：
    ```bash
    ros2 launch vlm_detect vlm_detect.launch.py image_topic:=/vlm_image
    ```
- 当前 VLM 拍摄模式：
  - `vlm_capture_mode: stop`：到 VLM 点停住，发布图像/触发信号，等待 `vlm_capture_wait_sec` 后继续。
  - `vlm_capture_mode: pass_through`：保留的经过拍照模式，按 `pass_through_vlm_trigger_radius` 在接近 VLM 点时触发。

### 底盘、雷达、里程计与消息

- 底盘包：`src/origincar_base`
  - 常见启动：`origincar_bringup.launch.py`、`base_serial.launch.py`、`ekf.launch.py`
  - 负责底盘串口、IMU、里程计、EKF 和 TF 等基础能力。
- 雷达驱动路径：`src/LSLIDAR_X_ROS2-20240228/src`
- 障碍物检测包：`src/obstacle_scanner`
  - 消费雷达数据，发布导航需要的障碍物信息。
- 消息/服务包：`src/origincar_msg`
  - 包含 `origincar_msg/srv/Speak`。
- 当前 `racing_control` 期望里程计话题是 `/odom_combined`。

### 启动编排

- `src/my_robot_bringup/launch/master_launch.py` 是较完整的分阶段总启动，涉及底盘、雷达、TTS、相机、二维码、障碍物、规划和 VLM。
- 当前 `racing_control` 流程会直接依赖 Nav2 actions，并结合 `obstacle_nav2`/轨迹保护相关节点。修改 `master_launch.py` 前，应确认现场实际是通过总 launch 还是多个终端分别启动。
- `planner` 包里还有较旧或备用的 Hybrid A*、Pure Pursuit 等流程。除非当前 launch 明确使用它，否则先把它当作历史/备用路径看待。

## 共享信号与话题

- `/sign4return` 类型是 `std_msgs/msg/Int32`，被多个模块共享：
  - `0`：启用二维码检测。
  - `5`：关闭二维码检测。
  - `9`：触发 VLM 拍摄/推理。
  - `10`：应用普通 Nav2 参数档。
  - `11`：应用任务二 Nav2 参数档。
- 新增 `/sign4return` 数值前，必须检查所有订阅者，包括底盘、感知和导航参数切换节点。
- 其他关键接口：
  - `/qr_results`：二维码文本结果。
  - `/vlm_result`：VLM 文本结果。
  - `/tts/speak`：TTS 服务。
  - `/image`：压缩相机流。
  - `/vlm_image`：可选单帧 VLM 图像转发输出。
  - `/trajectory_guard/input_path`：轨迹保护输入路径。
  - `/cmd_vel`：真实运动控制通道，避免多个节点同时发布。

## 调试注意事项

- 不要让多个节点同时发布 `/cmd_vel`，除非明确 remap 掉其中一个。
- 二维码区域后不继续走时：
  - 看 `racing_control` 日志是否出现 `QR result received`。
  - 直接 echo `/qr_results`。
  - 确认二维码文本能被解析为顺/逆方向。
- VLM 报 `No image` 或没有结果时：
  - 检查 `ros2 topic info /image`。
  - 如果启用转发，确认 `enable_vlm_image_relay:=true`，并且 VLM 使用 `image_topic:=/vlm_image`。
  - 确认相机实际发布的是 `sensor_msgs/msg/CompressedImage`。
- 路线段末端卡住时：
  - 检查 `/odom_combined` 是否连续。
  - 检查 `circle_goal_tolerance` 是否过小。
  - 如果临时启用了 `trajectory_guard`，再检查它是否正常向 `/follow_path` 转发。
- Nav2 在任务二前后行为差异大时：
  - 查看 `nav2_profile_10.yaml` 和 `nav2_profile_11.yaml`。
  - 查看 `nav2_profile_tuner` 日志里是否收到并应用 `/sign4return` 10/11。
- launch 已启动但按空格不能开始比赛时，可能是 stdin 不是 TTY；可使用 `auto_start:=true`。
- 更新现场点位时，优先修改 `src/racing_control/config/racing_control.yaml`，然后重新 build/install `racing_control`，确保 launch 读到安装后的配置。

## 关键参数清单

- `frame_id`：比赛点位所属坐标系。
- `qr_pose`、`post_qr_pose`、`entry_pose`：二维码阶段到正式路线阶段的关键过渡点。
- `clockwise_waypoints`、`counterclockwise_waypoints`：顺/逆时针路线点。
- `clockwise_home_pose`、`counterclockwise_home_pose`：顺/逆方向 home 点。
- `use_post_qr_pose`：是否启用二维码后置点。
- `vlm_waypoint_number`：第几个正式路线点触发 VLM。
- `vlm_capture_mode`：`stop` 或 `pass_through`。
- `vlm_capture_wait_sec`：停住拍照模式下触发 VLM 后等待时间。
- `enable_vlm_image_relay`：是否由 `racing_control` 把 `/image` 的单帧图像转发到 `/vlm_image`。
- `vlm_image_input_topic`、`vlm_image_output_topic`：VLM 图像转发输入/输出话题。

## 待后续补齐

- 比赛日标准启动顺序：到底使用 `my_robot_bringup/master_launch.py`，还是分终端启动 `obstacle_nav2`、`vlm_detect`、`racing_control` 等节点。
- 当前最权威的 Nav2 参数归属：`obstacle_nav2` 的 profile 看起来是比赛主路径，但 `planner` 和 `my_robot_bringup` 仍保留旧流程。
- 不同部署模式下相机话题的最终类型。近期 `vlm_detect` 期望 `CompressedImage`，换相机驱动前要重新确认。
- `/sign4return` 除 0、5、9、10、11 外，在底盘板和感知节点里的完整含义。
- 现场最新点位来源、采集时间和对应 JSON/YAML 转换记录。