diff --git a/.gitignore b/.gitignore index e694731..af499f9 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,5 @@ log .vscode datas + +running_logs diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..adaf0aa --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,237 @@ +# 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 --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 转换记录。 diff --git a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml index ea0e78f..032ca25 100644 --- a/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml +++ b/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml @@ -15,7 +15,7 @@ use_gps_ts: false #雷达是否使用GPS授时 scan_topic: /scan #设置激光数据topic名称 interface_selection: serial #接口选择:net 为网口,serial 为串口。 - serial_port_: /dev/ttyCH343USB0 #串口连接时的串口号 + serial_port_: /dev/radar #串口连接时的串口号 high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。 compensation: false #M10系列是否使用角度补偿功能 pubScan: true #是否发布scan话题 diff --git a/src/data_collection_tools/x5_udp_cmd_bridge.py b/src/data_collection_tools/x5_udp_cmd_bridge.py new file mode 100644 index 0000000..1ab9b85 --- /dev/null +++ b/src/data_collection_tools/x5_udp_cmd_bridge.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 +"""Receive safe short-lived drive commands from the PC and publish ROS Twist.""" + +from __future__ import annotations + +import argparse +import json +import math +import socket +import time + +import rclpy +from geometry_msgs.msg import Twist + + +def main() -> None: + parser = argparse.ArgumentParser(description="PC UDP to ROS cmd_vel_task bridge") + parser.add_argument("--bind", default="0.0.0.0") + parser.add_argument("--port", type=int, default=8765) + parser.add_argument("--topic", default="/cmd_vel_task") + parser.add_argument("--timeout", type=float, default=0.35) + args = parser.parse_args() + + rclpy.init() + node = rclpy.create_node("x5_udp_cmd_bridge") + publisher = node.create_publisher(Twist, args.topic, 10) + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind((args.bind, args.port)) + sock.settimeout(0.05) + last_packet = 0.0 + current = (0.0, 0.0) + node.get_logger().info(f"UDP {args.bind}:{args.port} -> {args.topic}, timeout={args.timeout}s") + + try: + while rclpy.ok(): + try: + raw, _address = sock.recvfrom(1024) + data = json.loads(raw.decode("utf-8")) + v = float(data.get("v", 0.0)) + w = float(data.get("w", 0.0)) + if not (math.isfinite(v) and math.isfinite(w)): + raise ValueError("non-finite command") + current = (max(-0.35, min(0.35, v)), max(-1.6, min(1.6, w))) + last_packet = time.monotonic() + except socket.timeout: + pass + except (ValueError, TypeError, json.JSONDecodeError) as exc: + node.get_logger().warn(f"ignore invalid UDP command: {exc}") + + if time.monotonic() - last_packet > args.timeout: + current = (0.0, 0.0) + msg = Twist() + msg.linear.x, msg.angular.z = current + publisher.publish(msg) + rclpy.spin_once(node, timeout_sec=0.0) + finally: + publisher.publish(Twist()) + sock.close() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/gc/项目总结_yiliao_ws.md b/src/gc/项目总结_yiliao_ws.md new file mode 100644 index 0000000..f5e90a2 --- /dev/null +++ b/src/gc/项目总结_yiliao_ws.md @@ -0,0 +1,429 @@ +# yiliao_ws 项目总结 + +> **生成日期**: 2026-08-09 +> **项目名称**: 智慧医疗2026（第21届全国大学生智能汽车竞赛·地瓜机器人赛项） +> **ROS 发行版**: ROS 2 Humble +> **机器人平台**: OriginCar (RDK X5, 四轮阿克曼转向) +> **工作区路径**: `/home/sunrise/yiliao_ws` +> **远程仓库**: `https://gitee.com/hikos/smart-healthcare-2026.git` + +--- + +## 一、项目定位 + +本项目是 RDK X5 机器人上的 ROS 2 Humble 工作区，服务于第21届全国大学生智能汽车竞赛的**医疗赛道**（智慧医疗）。整体采用**调度型架构**：底盘、雷达、相机、二维码、Nav2 导航、VLM 图生文、TTS 语音等功能由独立模块完成，`racing_control` 包作为总调度协调各模块完成比赛流程。 + +--- + +## 二、项目目录结构总览 + +``` +yiliao_ws/ +├── src/ # 源代码（所有 ROS 2 包） +│ ├── racing_control/ # 🎯 总调度（比赛流程控制） +│ ├── origincar_base/ # 🔧 底盘驱动（串口通信 + EKF + 阿克曼） +│ ├── navigation/ # 🧭 导航相关包集合 +│ │ ├── obstacle_nav2/ # 主 Nav2 导航栈（含参数调谐器） +│ │ ├── gc_navigation2_slamtoolbox/ # 仿真版 SLAM + Nav2 +│ │ ├── gc_navigation2_real/ # 实车版 SLAM + Nav2 +│ │ ├── gc_navigation_fish/ # 另一套导航配置 +│ │ ├── cyy_navigation2/ # 备选导航方案 +│ │ ├── cyy_slamtoolbox/ # 备选 SLAM 方案 +│ │ └── zbw_slamtoolbox/ # 另一套 SLAM 方案 +│ ├── qr_detection/ # 📷 二维码检测 +│ ├── vlm_detect/ # 🤖 VLM 图生文 + TTS 语音播报 +│ ├── car_usb_cam/ # 📸 USB 相机驱动 +│ ├── obstacle_scanner/ # 📡 激光雷达障碍物检测 +│ ├── origincar_msg/ # 📦 自定义 ROS 2 消息 +│ ├── origincar_description/ # 🤖 机器人 URDF 模型描述 +│ ├── origincar_birdseye/ # 🐦 鸟瞰图转换 +│ ├── my_robot_bringup/ # 🚀 总启动编排（master_launch.py） +│ ├── planner/ # 🗺️ 备选规划器（Hybrid A* 等） +│ ├── past_control/ # 📝 历史控制方案 +│ ├── mtran/ # 🧠 MTRAN 模型 +│ ├── ground_slam/ # 🗺️ 地面 SLAM +│ ├── car_image_proc/ # 🖼️ 图像处理 +│ ├── yolov8_launch/ # 🎯 YOLOv8 目标检测 +│ ├── data_collection_tools/ # 📊 数据采集工具 +│ ├── LSLIDAR_X_ROS2-20240228/ # 📡 镭神激光雷达 ROS 2 驱动 +│ └── gc/ # 📄 项目文档存放 +├── build/ # colcon 构建输出 +├── install/ # colcon 安装输出 +├── scripts/ # 辅助脚本（PID跟踪、陀螺仪标定等） +├── bashes/ # 自动脚本（WiFi连接、雷达驱动切换） +├── config/dds/ # DDS 配置 +├── datas/ # 采集数据 +├── models/ # 模型文件 +├── docs/ # 文档 +├── bag_outputs/ # ROS Bag 输出 +├── running_logs/ # 运行日志 +├── test_results/ # 测试结果 +├── vlm_server.py # VLM 推理服务（FastAPI） +├── cali.npz / cali.txt # 标定数据 +├── path_follower_demo.py # 路径跟随演示 +└── 标定.py # 标定脚本 +``` + +--- + +## 三、核心模块详解 + +### 3.1 racing_control — 比赛总调度 ⭐ + +| 项目 | 说明 | +|------|------| +| **包路径** | `src/racing_control` | +| **语言** | C++ (rclcpp) | +| **主节点** | `racing_control.cpp` | +| **配置** | `config/racing_control.yaml` | +| **启动** | `launch/racing_control.launch.py` | + +**设计原则**：只做比赛流程调度，不重复实现二维码、VLM、Nav2、底盘控制等子功能。通过 `/sign4return` 信号控制各子模块的开关和参数切换。 + +**比赛流程（12个阶段）**： + +```mermaid +graph TD + A[Idle 等待启动] --> B[NavigateToQr 二维码点导航] + B --> C[NavigatePostQr 二维码后置点导航] + C --> D[WaitForQr 二维码识别+TTS] + D --> E[NavigateToEntry 通道入口导航] + E --> F[SwitchToTask2Profile 任务二参数切换] + F --> G[ComputeCirclePath 任务二轨迹规划] + G --> H[ExecuteCirclePath 任务二轨迹执行] + H --> I[SwitchToNormalProfile 恢复导航参数] + I --> J[WaitForVlm 图生文+TTS] + J --> K[ReturnOrigin 返回原点] + K --> L[Finished 比赛完成] +``` + +**关键接口**： +- Nav2 Actions: `/navigate_to_pose`, `/compute_path_through_poses`, `/follow_path` +- 控制信号: `/sign4return` (Int32)，数值含义：0=启用QR, 5=关闭QR, 9=触发VLM, 10=普通Nav2参数, 11=任务二Nav2参数 +- 接收: `/qr_results`, `/vlm_result`, `/odom_combined` + +**关键参数（YAML 配置）**： +| 参数 | 默认值 | 说明 | +|------|--------|------| +| `qr_pose` | 坐标数组 | 二维码区域导航点 | +| `post_qr_pose` | 坐标数组 | 二维码后置过渡点 | +| `entry_pose` | 坐标数组 | 正式路线入口点 | +| `clockwise_waypoints` | 坐标数组 | 顺时针路线点（不含home） | +| `counterclockwise_waypoints` | 坐标数组 | 逆时针路线点（不含home） | +| `clockwise_home_pose` | 坐标数组 | 顺时针home点 | +| `counterclockwise_home_pose` | 坐标数组 | 逆时针home点 | +| `vlm_waypoint_number` | 2 | 第几个路线点触发VLM | +| `vlm_capture_mode` | stop | VLM拍摄模式 (stop/pass_through) | +| `use_post_qr_pose` | false | 是否启用QR后置点 | +| `enable_vlm_image_relay` | false | 是否转发单帧图像给VLM | + +**二维码方向解析规则**： +- 文本包含 `顺` 或奇数数字 → 顺时针 +- 文本包含 `逆` 或偶数数字 → 逆时针 + +--- + +### 3.2 origincar_base — 底盘驱动 + +| 项目 | 说明 | +|------|------| +| **包路径** | `src/origincar_base` | +| **语言** | C++ + Python | +| **主节点** | `origincar_base.cpp` (串口通信), `cmd_vel_to_ackermann_drive.py` (运动转换) | +| **配置** | `config/ekf.yaml`, `config/imu.yaml` | + +**功能**： +- STM32 串口通信 (`/dev/ttyACM0`, 921600bps) +- 航迹推算 + Mahony AHRS 姿态解算 +- EKF 融合 (odom + IMU → `/odom_combined`) +- Twist → AckermannDriveStamped 转换 (wheelbase=0.143m) +- 帧协议: 24字节收（0x7B/0x7D帧头尾）/ 11字节发 + +**阿克曼底盘模式** (`akmcar:=true`)： +``` +cmd_vel(Twist) → cmd_vel_to_ackermann_drive.py → ackermann_cmd(AckermannDriveStamped) → STM32 +``` + +**关键修改（must_know.md 记录）**： +- EKF 帧名统一 (`odom_combined` → `odom`) +- 底盘驱动默认不再发布 TF（避免与 EKF 冲突） +- 里程计协方差可参数化配置 +- 串口异常捕获防崩溃 + +--- + +### 3.3 obstacle_nav2 — 主 Nav2 导航栈 + +| 项目 | 说明 | +|------|------| +| **包路径** | `src/navigation/obstacle_nav2` | +| **启动文件** | `obstacle_nav2.launch.py`, `nav2_profile_tuner.launch.py`, `trajectory_guard.launch.py` | + +**功能**： +- 集成 Nav2 导航（MPPI 控制器 + SmacPlannerHybrid 规划器） +- 消费 `/obstacles` 转化为 costmap 障碍物层 +- **参数档位切换**：`nav2_profile_tuner` 监听 `/sign4return`，10→普通参数，11→任务二参数 +- **轨迹保护**（备用）：`trajectory_guard` 订阅 `/trajectory_guard/input_path` + +**关键参数文件**： +| 文件 | 用途 | +|------|------| +| `nav2_params.yaml` | Nav2 主参数 | +| `nav2_profile_10.yaml` | 普通/默认导航参数档 | +| `nav2_profile_11.yaml` | 任务二路线参数档 | +| `trajectory_guard.yaml` | 轨迹保护参数 | +| `nav2_params_basic_tracking.yaml` | 基础跟踪参数 | + +**MPPI 参数（CPU 优化后）**： +- `time_steps`: 20, `batch_size`: 200 (原1000) +- `PathAlignCritic.cost_weight`: 7.0 (降低，避障优先) + +**代价地图**：`inflation_radius`: 0.2m, `cost_scaling_factor`: 3.0 + +--- + +### 3.4 qr_detection — 二维码检测 + +| 项目 | 说明 | +|------|------| +| **包路径** | `src/qr_detection` | +| **语言** | C++ | +| **输入** | `/image` (CompressedImage) | +| **输出** | `/qr_results` (std_msgs/String) | +| **控制** | `/sign4return=0` 启用, `/sign4return=5` 关闭 | + +--- + +### 3.5 vlm_detect — VLM 图生文 + TTS + +| 项目 | 说明 | +|------|------| +| **包路径** | `src/vlm_detect` | +| **语言** | Python | +| **主节点** | `vlm_node.py` (VLM推理), `tts_server.py` (语音合成) | + +**数据流**： +``` +USB Camera → /image → vlm_node → /vlm_result + → qr_dete_node → /qr_results + ↓ + qr_tts_bridge → /tts/speak → Piper TTS → 语音输出 +``` + +**VLM 触发**：`/sign4return=9` +**VLM 服务**：外部 FastAPI 服务 (`vlm_server.py`)，运行 AndesVL-1B 模型，OpenAI 兼容 API +**TTS**：Piper TTS + paplay 本地语音合成 + +--- + +### 3.6 obstacle_scanner — 雷达障碍物检测 + +| 项目 | 说明 | +|------|------| +| **包路径** | `src/obstacle_scanner` | +| **语言** | C++ (使用 OpenCV + Eigen) | +| **功能** | 基于角度聚类+圆拟合的 LiDAR 障碍物检测 | + +--- + +### 3.7 my_robot_bringup — 总启动编排 + +| 项目 | 说明 | +|------|------| +| **包路径** | `src/my_robot_bringup` | +| **启动文件** | `launch/master_launch.py` | + +**分阶段启动时间线**： +``` +t=0s 底盘驱动 + EKF + TF + IMU (origincar_bringup) +t=2s 激光雷达 (lslidar_driver) +t=5s SLAM 建图 (slam_toolbox) +t=8s Nav2 导航栈 +t=10s VLM 图生文 + TTS 语音播报 +``` + +**常用启动参数**：`use_base`, `use_lidar`, `use_slam`, `use_nav2`, `use_vlm`, `use_tts`, `akmcar`, `vlm_host` + +--- + +### 3.8 其他包 + +| 包名 | 说明 | +|------|------| +| `origincar_description` | 机器人 URDF 模型 (0.276×0.214×0.211m, wheelbase=0.143m) | +| `origincar_msg` | 自定义消息 (Data.msg, Sign.msg) 和服务 (Speak.srv) | +| `car_usb_cam` | USB 相机驱动 (hobot_usb_cam)，发布 `/image` | +| `planner` | 备选规划方案（Hybrid A*, Pure Pursuit, Ackermann Hybrid A*） | +| `origincar_birdseye` | 鸟瞰图/IPM 变换 | +| `ground_slam` | 地面 SLAM 建图 | +| `yolov8_launch` | YOLOv8 目标检测启动 | +| `car_image_proc` | 图像处理工具 | +| `past_control` | 历史控制方案（竞速参考） | +| `mtran` | MTRAN 模型（含依赖和文档） | +| `data_collection_tools` | X5 UDP 指令桥接 | + +--- + +## 四、导航相关包集合 (`src/navigation/`) + +本工作区历史上有多个 SLAM/Nav2 方案迭代： + +| 包名 | 用途 | 状态 | +|------|------|:----:| +| `obstacle_nav2` | 主 Nav2 导航栈 + 参数调谐 | ✅ **当前使用** | +| `gc_navigation2_slamtoolbox` | 仿真版 SLAM + Nav2 (Gazebo) | 📦 仿真用 | +| `gc_navigation2_real` | 实车版 SLAM + Nav2 (无 Gazebo) | 📦 实车参考 | +| `gc_navigation_fish` | 另一套导航配置 | 📦 备用 | +| `cyy_navigation2` | 备选导航方案 | 📦 历史 | +| `cyy_slamtoolbox` | 备选 SLAM | 📦 历史 | +| `zbw_slamtoolbox` | 另一套 SLAM | 📦 历史 | + +**关键差异（仿真 vs 实车）**： +| 方面 | 仿真版 | 实车版 | +|------|--------|--------| +| `use_sim_time` | true | false | +| odom_frame | `odom` | `odom_combined` (EKF融合后) | +| 底盘驱动 | Gazebo plugin | origincar_base 串口驱动 | +| 激光雷达 | Gazebo ray plugin | lslidar_driver (镭神 N10) | + +--- + +## 五、TF 树结构 + +``` +map ──→ odom ──→ base_footprint ──→ base_link ──→ laser_link + ↑ ↑ ↑ ↑ +slam EKF融合 静态TF URDF +``` + +--- + +## 六、关键共享信号 `/sign4return` + +| 值 | 含义 | 消费者 | +|:--:|------|--------| +| 0 | 启用二维码检测 | qr_detection | +| 5 | 关闭二维码检测 | qr_detection | +| 9 | 触发 VLM 拍摄/推理 | vlm_detect | +| 10 | 应用普通 Nav2 参数档 | nav2_profile_tuner | +| 11 | 应用任务二 Nav2 参数档 | nav2_profile_tuner | + +--- + +## 七、环境与操作 + +### 7.1 机器人访问 +```bash +ssh sunrise@192.168.10.210 +``` + +### 7.2 环境初始化 +```bash +cd /home/sunrise/yiliao_ws +source /opt/ros/humble/setup.bash +source install/setup.bash +# 部分相机流程可能还需要: source /opt/tros/humble/setup.bash +``` + +### 7.3 编译 +```bash +# 编译单个包 +colcon build --packages-select --cmake-args -DBUILD_TESTING=ON + +# 全量编译（推荐 --symlink-install 使 Python 修改即时生效） +colcon build --symlink-install +``` + +### 7.4 运行测试 +```bash +colcon test --packages-select racing_control +colcon test-result --verbose --test-result-base build/racing_control +``` + +### 7.5 常用启动命令 +```bash +# 全部启动（比赛模式） +ros2 launch my_robot_bringup master_launch.py + +# 仅底盘调试 +ros2 launch my_robot_bringup master_launch.py use_lidar:=false use_slam:=false use_nav2:=false use_vlm:=false + +# 仅建图 +ros2 launch my_robot_bringup master_launch.py use_nav2:=false use_vlm:=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 +``` + +--- + +## 八、硬件与传感器 + +| 部件 | 型号 | 说明 | +|------|------|------| +| 主控 | RDK X5 | 10 TOPS 算力 | +| 底盘 | OriginCar | 四轮阿克曼转向，276×214×211mm，轴距143mm | +| 激光雷达 | 镭神 N10 | 单线TOF，0.15~12m | +| 深度相机 | 光鉴 Aurora930 | | +| USB相机 | USB 2.0 Camera | 发布 `/image` | +| IMU | 板载 | 与里程计进行 EKF 融合 | + +--- + +## 九、已知问题与调试经验 + +### 9.1 已修复的关键问题 + +| 问题 | 修复方案 | +|------|----------| +| TF 树断连（两节点争抢 odom→base_link） | EKF 改为发布 odom→base_footprint；底盘驱动默认不发布 TF | +| 串口异常崩溃 | 为 Stm32_Serial.read() 添加 try-catch | +| LiDAR 扫描出现空扇形面 (60°) | 恢复 `scan_num` 动态计算公式 | +| URDF git 冲突 | 清除冲突标记，统一轮距参数 | +| CPU 负载过高 (77%) | MPPI batch_size 1000→200, throttle_scans 1→10, EKF 30Hz→20Hz | +| cmd_vel_to_ackermann_drive.py 权限 | chmod +x | + +### 9.2 调试检查清单 + +- **二维码区域后不继续走**：检查 `/qr_results` 是否有输出，文本是否可解析为顺/逆 +- **VLM 无结果**：检查 `/image` 话题，确认 enable_vlm_image_relay 和 VLM image_topic 配置 +- **路线末端卡住**：检查 `/odom_combined` 连续性，circle_goal_tolerance 是否过小 +- **Nav2 行为异常**：检查 nav2_profile_tuner 是否收到/应用了 /sign4return 10/11 +- **雷达无数据**：`lsusb` 检查物理连接，`ls /dev/tty*` 检查 ttyACM0/ACM1/CH340 + +--- + +## 十、辅助脚本 + +| 脚本 | 用途 | +|------|------| +| `scripts/PIDtracking.py` | PID 轨迹跟踪 | +| `scripts/measure_turning_radius.py` | 测量转弯半径 | +| `scripts/fit_gyro_bias.py` | 陀螺仪偏置拟合 | +| `scripts/publish_sine_path.py` | 发布正弦路径 | +| `scripts/udp_to_cmdvel.py` | UDP→cmd_vel 桥接 | +| `scripts/set_volume.py` | 音量设置 | +| `bashes/radar-driver-switch.sh` | 雷达驱动自动切换 | +| `bashes/auto-wifi-connect.sh` | WiFi 自动连接 | +| `vlm_server.py` | VLM 推理服务 (FastAPI, AndesVL-1B) | +| `path_follower_demo.py` | 路径跟随演示 | + +--- + +## 十一、数据与文档 + +| 类型 | 路径 | 说明 | +|------|------|------| +| 地图文件 | `src/navigation/*/maps/` | .pgm + .yaml + .posegraph | +| 采集数据 | `datas/` | data4, data5, PIDtracking等 | +| 调试记录 | `调试记录.md` | 2026/6/1 ~ 6/13 调试过程 | +| 变更说明 | `must_know.md` | mo_new→HEAD 101文件变更详解 | +| 启动指南 | `master_launch_使用指南.md` | master_launch.py 完整说明 | +| 竞赛方案 | `竞赛方案_第21届智能汽车竞赛地瓜机器人赛项.md` | 正式竞赛方案文档 | +| 点位移转 | `src/racing_control/点位格式转换.md` | JSON→YAML 点位转换流程 | +| VLM数据流 | `src/vlm_detect/dataflow.md` | VLM+QR→TTS 数据流图 | +| AGENTS.md | `/home/sunrise/yiliao_ws/AGENTS.md` | 项目级说明 | +| CLAUDE.md | `/home/sunrise/yiliao_ws/CLAUDE.md` | 另一视角的项目说明（含 test_ws 内容） | diff --git a/src/map/nav2_costmap_binary.png b/src/map/nav2_costmap_binary.png index 3ada2bc..c414579 100644 Binary files a/src/map/nav2_costmap_binary.png and b/src/map/nav2_costmap_binary.png differ diff --git a/src/navigation/obstacle_nav2/CMakeLists.txt b/src/navigation/obstacle_nav2/CMakeLists.txt index 98f94dd..e3067ff 100755 --- a/src/navigation/obstacle_nav2/CMakeLists.txt +++ b/src/navigation/obstacle_nav2/CMakeLists.txt @@ -107,6 +107,13 @@ install(TARGETS install(DIRECTORY include/ DESTINATION include) install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME}) +# Python scripts +install(PROGRAMS + scripts/initial_pose_to_tf.py + scripts/static_map_publisher.py + DESTINATION share/${PROJECT_NAME}/scripts +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp diff --git a/src/navigation/obstacle_nav2/behavior_tree/nav_through_poses_ackermann.xml b/src/navigation/obstacle_nav2/behavior_tree/nav_through_poses_ackermann.xml new file mode 100644 index 0000000..ec55e97 --- /dev/null +++ b/src/navigation/obstacle_nav2/behavior_tree/nav_through_poses_ackermann.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml b/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml index 49ade2e..a0cd175 100755 --- a/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml +++ b/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml @@ -1,33 +1,31 @@ - - - - - - - - - + + + + + + + + - - - - - - - - - - - - - + + + + + + + + diff --git a/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_event_replan b/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_event_replan new file mode 100755 index 0000000..49ade2e --- /dev/null +++ b/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_event_replan @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_single_backup b/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_single_backup new file mode 100755 index 0000000..caf2c5d --- /dev/null +++ b/src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_single_backup @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260808 b/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260808 new file mode 100644 index 0000000..2584a4f --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260808 @@ -0,0 +1,340 @@ +# ============================================================================ +# nav2_params.yaml — Odometry-only obstacle navigation +# +# No static map, no AMCL, no SLAM. +# Both costmaps are rolling windows in odom. The launch file can rewrite every +# global_frame leaf when a different connected odometry frame is required. +# Global planner: Smac Hybrid A* (Reeds-Shepp) +# Local controller: MPPI (Ackermann) +# 速度快但是雷达容易掉 +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # Injected by obstacle_nav2.launch.py from this package's share directory. + default_nav_to_pose_bt_xml: "" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 36 + model_dt: 0.05 + batch_size: 1000 + vx_std: 0.22 + vy_std: 0.0 + wz_std: 0.45 + vx_max: 1.00 + vx_min: -0.75 + vy_max: 0.0 + wz_max: 1.5 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: false + cost_power: 1 + cost_weight: 4.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 100000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 10.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 10 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 5 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: false + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.35 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 0.5 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: false + width: 8 + height: 8 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: true + plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + enabled: true + map_subscribe_transient_local: true + subscribe_to_updates: false + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 2.0 + inflation_radius: 0.35 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.15 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 3.0 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 + retrospective_penalty: 0.015 + # 5 m covers the rolling planning horizon without the startup and memory + # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: false + smooth_path: True + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.00, 0.0, 2.0] + min_velocity: [-0.75, 0.0, -2.0] + max_accel: [3.73, 0.0, 3.2] + max_decel: [-1.1, 0.0, -4.5] + odom_topic: /odom_combined + odom_duration: 0.1 + deadband_velocity: [0.03, 0.0, 0.03] + velocity_timeout: 1.0 diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260809 b/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260809 new file mode 100644 index 0000000..afe7f1e --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260809 @@ -0,0 +1,340 @@ +# ============================================================================ +# nav2_params.yaml — Odometry-only obstacle navigation +# +# No static map, no AMCL, no SLAM. +# Both costmaps are rolling windows in odom. The launch file can rewrite every +# global_frame leaf when a different connected odometry frame is required. +# Global planner: Smac Hybrid A* (Reeds-Shepp) +# Local controller: MPPI (Ackermann) +# 比之前速度慢了点 +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # Injected by obstacle_nav2.launch.py from this package's share directory. + default_nav_to_pose_bt_xml: "" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 36 + model_dt: 0.05 + batch_size: 1000 + vx_std: 0.22 + vy_std: 0.0 + wz_std: 0.45 + vx_max: 1.00 + vx_min: -0.75 + vy_max: 0.0 + wz_max: 1.5 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: false + cost_power: 1 + cost_weight: 4.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 100000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 10.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 10 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 5 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: false + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.35 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 0.5 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: false + width: 8 + height: 8 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: true + plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + enabled: true + map_subscribe_transient_local: true + subscribe_to_updates: false + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 2.0 + inflation_radius: 0.35 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.15 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 3.0 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 + retrospective_penalty: 0.015 + # 5 m covers the rolling planning horizon without the startup and memory + # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: false + smooth_path: True + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.5, 0.0, 2.0] + min_velocity: [-0.75, 0.0, -2.0] + max_accel: [3.73, 0.0, 3.2] + max_decel: [-1.1, 0.0, -4.5] + odom_topic: /odom_combined + odom_duration: 0.1 + deadband_velocity: [0.03, 0.0, 0.03] + velocity_timeout: 1.0 diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml index 359391a..62ab233 100644 --- a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml @@ -6,12 +6,13 @@ # global_frame leaf when a different connected odometry frame is required. # Global planner: Smac Hybrid A* (Reeds-Shepp) # Local controller: MPPI (Ackermann) +# 速度快但是雷达容易掉 # ============================================================================ bt_navigator: ros__parameters: use_sim_time: False - global_frame: odom + global_frame: map robot_base_frame: base_footprint odom_topic: /odom_combined bt_loop_duration: 50 @@ -70,16 +71,16 @@ bt_navigator_rclcpp_node: controller_server: ros__parameters: use_sim_time: False - controller_frequency: 20.0 + controller_frequency: 15.0 FollowPath: plugin: "nav2_mppi_controller::MPPIController" - time_steps: 36 - model_dt: 0.05 - batch_size: 1000 - vx_std: 0.75 + time_steps: 40 + model_dt: 0.06666666666666666 + batch_size: 900 + vx_std: 0.25 vy_std: 0.0 - wz_std: 0.4 - vx_max: 1.75 + wz_std: 0.45 + vx_max: 1.00 vx_min: -0.75 vy_max: 0.0 wz_max: 1.5 @@ -101,7 +102,7 @@ controller_server: GoalCritic: enabled: true cost_power: 1 - cost_weight: 5.0 + cost_weight: 6.0 threshold_to_consider: 1.4 GoalAngleCritic: enabled: true @@ -111,21 +112,21 @@ controller_server: PreferForwardCritic: enabled: false cost_power: 1 - cost_weight: 0.0 + cost_weight: 7.0 threshold_to_consider: 0.5 CostCritic: enabled: true cost_power: 1 - cost_weight: 3.81 + cost_weight: 5.0 critical_cost: 300.0 consider_footprint: true - collision_cost: 1000000.0 + collision_cost: 100000.0 near_goal_distance: 1.0 trajectory_point_step: 2 PathAlignCritic: enabled: true cost_power: 1 - cost_weight: 14.0 + cost_weight: 8.0 max_path_occupancy_ratio: 0.05 trajectory_point_step: 4 threshold_to_consider: 0.5 @@ -135,13 +136,13 @@ controller_server: enabled: true cost_power: 1 cost_weight: 5.0 - offset_from_furthest: 5 + offset_from_furthest: 10 threshold_to_consider: 1.4 PathAngleCritic: enabled: true cost_power: 1 cost_weight: 2.0 - offset_from_furthest: 4 + offset_from_furthest: 5 threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 forward_preference: false @@ -180,7 +181,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.20 always_send_full_costmap: True local_costmap_client: ros__parameters: @@ -195,17 +196,22 @@ global_costmap: update_frequency: 1.0 publish_frequency: 1.0 transform_tolerance: 0.5 - global_frame: odom + global_frame: map robot_base_frame: base_footprint use_sim_time: False - rolling_window: true - width: 10 - height: 10 + rolling_window: false + width: 8 + height: 8 resolution: 0.05 footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" footprint_padding: 0.02 - track_unknown_space: false - plugins: ["obstacle_array_layer", "inflation_layer"] + track_unknown_space: true + plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + enabled: true + map_subscribe_transient_local: true + subscribe_to_updates: false obstacle_array_layer: plugin: "obstacle_nav2::ObstacleArrayLayer" enabled: true @@ -218,8 +224,8 @@ global_costmap: extra_inflation: 0.02 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.0 - inflation_radius: 0.55 + cost_scaling_factor: 2.0 + inflation_radius: 0.35 always_send_full_costmap: True global_costmap_client: ros__parameters: @@ -236,20 +242,20 @@ planner_server: plugin: "nav2_smac_planner/SmacPlannerHybrid" downsample_costmap: false downsampling_factor: 1 - tolerance: 0.25 + tolerance: 0.15 allow_unknown: false max_iterations: 1000000 max_on_approach_iterations: 1000 - max_planning_time: 5.0 + max_planning_time: 25.0 motion_model_for_search: "REEDS_SHEPP" angle_quantization_bins: 72 analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 minimum_turning_radius: 0.40 - reverse_penalty: 1.0 - change_penalty: 0.0 + reverse_penalty: 1.6 + change_penalty: 2.0 non_straight_penalty: 1.2 - cost_penalty: 2.0 + cost_penalty: 4.0 retrospective_penalty: 0.015 # 5 m covers the rolling planning horizon without the startup and memory # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. @@ -258,7 +264,7 @@ planner_server: viz_expansions: false smooth_path: True smoother: - max_iterations: 1000 + max_iterations: 700 w_smooth: 0.3 w_data: 0.2 tolerance: 1.0e-10 @@ -294,7 +300,7 @@ behavior_server: wait: plugin: "nav2_behaviors/Wait" wait_duration: 0.5 - global_frame: odom + global_frame: map robot_base_frame: base_footprint transform_tolerance: 0.5 use_sim_time: False @@ -324,10 +330,10 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [1.75, 0.0, 1.5] - min_velocity: [-0.75, 0.0, -1.5] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] + max_velocity: [1.00, 0.0, 2.0] + min_velocity: [-0.75, 0.0, -2.0] + max_accel: [3.73, 0.0, 3.2] + max_decel: [-1.1, 0.0, -4.5] odom_topic: /odom_combined odom_duration: 0.1 deadband_velocity: [0.03, 0.0, 0.03] diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml.bak_20260807_global_inflation_055 b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml.bak_20260807_global_inflation_055 new file mode 100644 index 0000000..a5eb62f --- /dev/null +++ b/src/navigation/obstacle_nav2/config/nav2_profile_10.yaml.bak_20260807_global_inflation_055 @@ -0,0 +1,339 @@ +# ============================================================================ +# nav2_params.yaml — Odometry-only obstacle navigation +# +# No static map, no AMCL, no SLAM. +# Both costmaps are rolling windows in odom. The launch file can rewrite every +# global_frame leaf when a different connected odometry frame is required. +# Global planner: Smac Hybrid A* (Reeds-Shepp) +# Local controller: MPPI (Ackermann) +# ============================================================================ + +bt_navigator: + ros__parameters: + use_sim_time: False + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom_combined + bt_loop_duration: 50 + default_server_timeout: 20 + # Injected by obstacle_nav2.launch.py from this package's share directory. + default_nav_to_pose_bt_xml: "" + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: False + +controller_server: + ros__parameters: + use_sim_time: False + controller_frequency: 20.0 + FollowPath: + plugin: "nav2_mppi_controller::MPPIController" + time_steps: 36 + model_dt: 0.05 + batch_size: 1000 + vx_std: 0.22 + vy_std: 0.0 + wz_std: 0.4 + vx_max: 1.00 + vx_min: -0.75 + vy_max: 0.0 + wz_max: 1.5 + iteration_count: 1 + temperature: 0.3 + gamma: 0.015 + motion_model: "Ackermann" + visualize: false + TrajectoryVisualizer: + trajectory_step: 5 + time_step: 3 + AckermannConstraints: + min_turning_r: 0.4 + critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"] + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.4 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.5 + PreferForwardCritic: + enabled: false + cost_power: 1 + cost_weight: 0.0 + threshold_to_consider: 0.5 + CostCritic: + enabled: true + cost_power: 1 + cost_weight: 3.81 + critical_cost: 300.0 + consider_footprint: true + collision_cost: 100000.0 + near_goal_distance: 1.0 + trajectory_point_step: 2 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 4 + threshold_to_consider: 0.5 + offset_from_furthest: 20 + use_path_orientations: false + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 10 + threshold_to_consider: 1.4 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 5 + threshold_to_consider: 0.5 + max_angle_to_furthest: 1.0 + forward_preference: false + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + transform_tolerance: 0.5 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: false + plugins: ["obstacle_array_layer", "inflation_layer"] + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + transform_tolerance: 0.5 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: False + rolling_window: false + width: 8 + height: 8 + resolution: 0.05 + footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" + footprint_padding: 0.02 + track_unknown_space: true + plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + enabled: true + map_subscribe_transient_local: true + subscribe_to_updates: false + obstacle_array_layer: + plugin: "obstacle_nav2::ObstacleArrayLayer" + enabled: true + topic: /obstacles + obstacle_timeout: 0.5 + transform_tolerance: 0.2 + default_obstacle_radius: 0.05 + minimum_obstacle_radius: 0.02 + maximum_obstacle_radius: 0.50 + extra_inflation: 0.02 + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False + +planner_server: + ros__parameters: + planner_plugins: ["GridBased"] + use_sim_time: False + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false + downsampling_factor: 1 + tolerance: 0.25 + allow_unknown: false + max_iterations: 1000000 + max_on_approach_iterations: 1000 + max_planning_time: 5.0 + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 0.40 + reverse_penalty: 1.0 + change_penalty: 0.0 + non_straight_penalty: 1.2 + cost_penalty: 2.0 + retrospective_penalty: 0.015 + # 5 m covers the rolling planning horizon without the startup and memory + # cost of the previous 20 m (401-cell) Hybrid-A* lookup table. + lookup_table_size: 5.0 + cache_obstacle_heuristic: false + viz_expansions: false + smooth_path: True + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +smoother_server: + ros__parameters: + use_sim_time: False + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + backup_dist: 0.8 + backup_speed: 0.18 + wait: + plugin: "nav2_behaviors/Wait" + wait_duration: 0.5 + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.5 + use_sim_time: False + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: False + +waypoint_follower: + ros__parameters: + loop_rate: 20 + use_sim_time: False + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: False + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [1.00, 0.0, 1.5] + min_velocity: [-0.75, 0.0, -1.5] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-4.5, 0.0, -4.5] + odom_topic: /odom_combined + odom_duration: 0.1 + deadband_velocity: [0.03, 0.0, 0.03] + velocity_timeout: 1.0 diff --git a/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml b/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml index e23fa71..cac37e9 100644 --- a/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml +++ b/src/navigation/obstacle_nav2/config/nav2_profile_11.yaml @@ -12,7 +12,7 @@ bt_navigator: ros__parameters: use_sim_time: False - global_frame: odom + global_frame: map robot_base_frame: base_footprint odom_topic: /odom_combined bt_loop_duration: 50 @@ -213,17 +213,22 @@ global_costmap: update_frequency: 5.0 publish_frequency: 3.0 transform_tolerance: 0.5 - global_frame: odom + global_frame: map robot_base_frame: base_footprint use_sim_time: False - rolling_window: true - width: 10 - height: 10 + rolling_window: false + width: 8 + height: 8 resolution: 0.05 footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]" footprint_padding: 0.02 - track_unknown_space: false - plugins: ["obstacle_array_layer", "inflation_layer"] + track_unknown_space: true + plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"] + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + enabled: true + map_subscribe_transient_local: true + subscribe_to_updates: false obstacle_array_layer: plugin: "obstacle_nav2::ObstacleArrayLayer" enabled: true @@ -311,7 +316,7 @@ behavior_server: wait: plugin: "nav2_behaviors/Wait" wait_duration: 0.5 - global_frame: odom + global_frame: map robot_base_frame: base_footprint transform_tolerance: 0.5 use_sim_time: False diff --git a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py index fd3396c..54835fd 100755 --- a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py +++ b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py @@ -22,14 +22,15 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, + ExecuteProcess, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable, ) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import SetRemap +from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node, SetRemap from nav2_common.launch import RewrittenYaml @@ -48,17 +49,20 @@ def generate_launch_description(): start_base = LaunchConfiguration('start_base', default='true') start_lidar = LaunchConfiguration('start_lidar', default='true') start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true') + start_map_publisher = LaunchConfiguration('start_map_publisher', default='true') + map_yaml_path = LaunchConfiguration('map_yaml_path') nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml') - fastdds_profile_path = os.path.join(pkg_dir, 'config', 'fastdds_udp_only.xml') nav_to_pose_bt_path = os.path.join( pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml') + nav_through_poses_bt_path = os.path.join( + pkg_dir, 'behavior_tree', 'nav_through_poses_ackermann.xml') configured_params = RewrittenYaml( source_file=nav2_param_path, root_key='', param_rewrites={ - 'global_frame': global_frame, 'default_nav_to_pose_bt_xml': nav_to_pose_bt_path, + 'default_nav_through_poses_bt_xml': nav_through_poses_bt_path, }, convert_types=True, ) @@ -95,6 +99,37 @@ def generate_launch_description(): condition=IfCondition(start_obstacle_scanner), ) + # ========================== Static Map Publisher ========================== + static_map_publisher_script = os.path.join(pkg_dir, 'scripts', 'static_map_publisher.py') + static_map_publisher = ExecuteProcess( + cmd=[ + FindExecutable(name='python3'), + static_map_publisher_script, + '--ros-args', + '-p', ['yaml_filename:=', map_yaml_path], + '-p', 'publish_rate:=1.0', + '-p', 'map_frame:=map', + ], + output='screen', + emulate_tty=True, + condition=IfCondition(start_map_publisher), + ) + + # ========================== Initial Pose -> TF (map->odom) ================ + initial_pose_to_tf_script = os.path.join(pkg_dir, 'scripts', 'initial_pose_to_tf.py') + initial_pose_to_tf = ExecuteProcess( + cmd=[ + FindExecutable(name='python3'), + initial_pose_to_tf_script, + '--ros-args', + '-p', 'odom_frame:=odom', + '-p', 'base_frame:=base_footprint', + '-p', 'map_frame:=map', + ], + output='screen', + emulate_tty=True, + ) + # ========================== Nav2 Navigation =============================== navigation_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -140,13 +175,20 @@ def generate_launch_description(): 'start_obstacle_scanner', default_value='true', description='Also start the obstacle_scanner node'), + DeclareLaunchArgument( + 'start_map_publisher', + default_value='true', + description='Start the static map publisher node'), + DeclareLaunchArgument( + 'map_yaml_path', + default_value='/home/sunrise/yiliao_ws/src/map/nav2_costmap_map.yaml', + description='Path to the map YAML file'), - SetEnvironmentVariable( - name='FASTRTPS_DEFAULT_PROFILES_FILE', - value=fastdds_profile_path), safe_base_bringup, lslidar_launch, obstacle_scanner_launch, + static_map_publisher, + initial_pose_to_tf, navigation_launch, ]) diff --git a/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py.bak_20260809_through_bt b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py.bak_20260809_through_bt new file mode 100755 index 0000000..f2006bc --- /dev/null +++ b/src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py.bak_20260809_through_bt @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 +# ============================================================================ +# obstacle_nav2.launch.py +# +# Odometry-only Nav2 navigation using obstacle_scanner detections. +# +# Architecture: +# origincar_base (bringup) — chassis serial driver + EKF + TF +# lslidar_driver — lidar → /scan +# obstacle_scanner (optional) — /scan → /obstacles +# Nav2 navigation_launch — rolling costmaps + Hybrid A* + MPPI +# +# No map_server, AMCL, or slam_toolbox in the default path. +# +# Usage: +# ros2 launch obstacle_nav2 obstacle_nav2.launch.py +# ros2 launch obstacle_nav2 obstacle_nav2.launch.py enable_motion:=true +# ============================================================================ + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node, SetRemap +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + """Odometry-only obstacle navigation.""" + + pkg_dir = get_package_share_directory('obstacle_nav2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + + # ========================== Launch Arguments ============================== + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + global_frame = LaunchConfiguration('global_frame', default='odom') + use_static_map = LaunchConfiguration('use_static_map', default='false') + map_yaml = LaunchConfiguration('map_yaml', default='') + enable_motion = LaunchConfiguration('enable_motion', default='true') + start_base = LaunchConfiguration('start_base', default='true') + start_lidar = LaunchConfiguration('start_lidar', default='true') + start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true') + start_map_publisher = LaunchConfiguration('start_map_publisher', default='true') + map_yaml_path = LaunchConfiguration('map_yaml_path') + + nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml') + nav_to_pose_bt_path = os.path.join( + pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml') + configured_params = RewrittenYaml( + source_file=nav2_param_path, + root_key='', + param_rewrites={ + 'default_nav_to_pose_bt_xml': nav_to_pose_bt_path, + }, + convert_types=True, + ) + base_cmd_vel_topic = PythonExpression([ + "'/cmd_vel' if '", enable_motion, + "' == 'true' else '/cmd_vel_hardware_disabled'", + ]) + + # ========================== Base Bringup ================================== + origincar_bringup = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('origincar_base'), + '/launch', '/origincar_bringup.launch.py']), + condition=IfCondition(start_base), + ) + safe_base_bringup = GroupAction([ + SetRemap(src='cmd_vel', dst=base_cmd_vel_topic), + origincar_bringup, + ]) + + # ========================== Lidar Driver ================================== + lslidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('lslidar_driver'), + '/launch', '/lsn10_launch.py']), + condition=IfCondition(start_lidar), + ) + + # ========================== Obstacle Scanner ============================== + obstacle_scanner_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [get_package_share_directory('obstacle_scanner'), + '/launch', '/obstacle_scanner.launch.py']), + condition=IfCondition(start_obstacle_scanner), + ) + + # ========================== Static Map Publisher ========================== + static_map_publisher_script = os.path.join(pkg_dir, 'scripts', 'static_map_publisher.py') + static_map_publisher = ExecuteProcess( + cmd=[ + FindExecutable(name='python3'), + static_map_publisher_script, + '--ros-args', + '-p', ['yaml_filename:=', map_yaml_path], + '-p', 'publish_rate:=1.0', + '-p', 'map_frame:=map', + ], + output='screen', + emulate_tty=True, + condition=IfCondition(start_map_publisher), + ) + + # ========================== Initial Pose -> TF (map->odom) ================ + initial_pose_to_tf_script = os.path.join(pkg_dir, 'scripts', 'initial_pose_to_tf.py') + initial_pose_to_tf = ExecuteProcess( + cmd=[ + FindExecutable(name='python3'), + initial_pose_to_tf_script, + '--ros-args', + '-p', 'odom_frame:=odom', + '-p', 'base_frame:=base_footprint', + '-p', 'map_frame:=map', + ], + output='screen', + emulate_tty=True, + ) + + # ========================== Nav2 Navigation =============================== + navigation_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/navigation_launch.py']), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'params_file': configured_params, + 'autostart': 'true', + }.items(), + ) + + # ========================== Assembly ====================================== + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock'), + DeclareLaunchArgument( + 'global_frame', + default_value='odom', + description='Global frame for both costmaps'), + DeclareLaunchArgument( + 'use_static_map', + default_value='false', + description='Reserved: enable static map + AMCL (not yet implemented)'), + DeclareLaunchArgument( + 'map_yaml', + default_value='', + description='Reserved: path to map YAML file'), + DeclareLaunchArgument( + 'enable_motion', + default_value='true', + description='Route Nav2 cmd_vel to the real base topic'), + DeclareLaunchArgument( + 'start_base', + default_value='true', + description='Start the base driver, EKF, and robot TF publishers'), + DeclareLaunchArgument( + 'start_lidar', + default_value='true', + description='Start the lidar driver'), + DeclareLaunchArgument( + 'start_obstacle_scanner', + default_value='true', + description='Also start the obstacle_scanner node'), + DeclareLaunchArgument( + 'start_map_publisher', + default_value='true', + description='Start the static map publisher node'), + DeclareLaunchArgument( + 'map_yaml_path', + default_value='/home/sunrise/yiliao_ws/src/map/nav2_costmap_map.yaml', + description='Path to the map YAML file'), + + + safe_base_bringup, + lslidar_launch, + obstacle_scanner_launch, + static_map_publisher, + initial_pose_to_tf, + navigation_launch, + ]) diff --git a/src/navigation/obstacle_nav2/scripts/initial_pose_to_tf.py b/src/navigation/obstacle_nav2/scripts/initial_pose_to_tf.py new file mode 100644 index 0000000..2ffaf91 --- /dev/null +++ b/src/navigation/obstacle_nav2/scripts/initial_pose_to_tf.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +""" +initial_pose_to_tf.py — /initialpose → map→odom 的 TF 变换 + +启动后先发布 identity TF (map 与 odom 重合) 作为回退值。 +收到 /initialpose 后查 TF (odom→base_footprint)，反算 map→odom 并更新。 + +使用: ros2 topic pub /initialpose geometry_msgs/msg/PoseWithCovarianceStamped ... +或在 RViz 中用 2D Pose Estimate 工具设定。 +""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseWithCovarianceStamped, TransformStamped +from tf2_ros import TransformListener, Buffer, StaticTransformBroadcaster +from tf2_ros import LookupException, ConnectivityException, ExtrapolationException + + +class InitialPoseToTF(Node): + def __init__(self): + super().__init__('initial_pose_to_tf') + self.declare_parameter('odom_frame', 'odom') + self.declare_parameter('base_frame', 'base_footprint') + self.declare_parameter('map_frame', 'map') + + self.odom_frame_ = self.get_parameter('odom_frame').value + self.base_frame_ = self.get_parameter('base_frame').value + self.map_frame_ = self.get_parameter('map_frame').value + + self.tf_buffer_ = Buffer() + self.tf_listener_ = TransformListener(self.tf_buffer_, self) + self.static_broadcaster_ = StaticTransformBroadcaster(self) + + self.initial_pose_sub_ = self.create_subscription( + PoseWithCovarianceStamped, '/initialpose', + self._initial_pose_callback, 10) + + self.get_logger().info( + f'waiting /initialpose ' + f'(map={self.map_frame_}, odom={self.odom_frame_}, base={self.base_frame_})') + + # fallback identity TF after 1s + self._identity_timer = self.create_timer(1.0, self._publish_identity) + + def _publish_identity(self): + self._identity_timer.cancel() + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = self.map_frame_ + t.child_frame_id = self.odom_frame_ + t.transform.rotation.w = 1.0 + self.static_broadcaster_.sendTransform(t) + self.get_logger().info('Published identity map→odom TF (fallback)') + + def _initial_pose_callback(self, msg: PoseWithCovarianceStamped): + try: + odom_to_base = self.tf_buffer_.lookup_transform( + self.odom_frame_, self.base_frame_, + rclpy.time.Time(), rclpy.duration.Duration(seconds=1.0)) + except (LookupException, ConnectivityException, ExtrapolationException) as e: + self.get_logger().warn(f'TF lookup failed: {e}') + return + + map_to_base = msg.pose.pose + ob_t = odom_to_base.transform.translation + ob_r = odom_to_base.transform.rotation + + # q_base_odom = q_odom_base^(-1) + q_bo = (-ob_r.x, -ob_r.y, -ob_r.z, ob_r.w) + # q_map_odom = q_map_base * q_base_odom + x1, y1, z1, w1 = ( + map_to_base.orientation.x, map_to_base.orientation.y, + map_to_base.orientation.z, map_to_base.orientation.w) + x2, y2, z2, w2 = q_bo + qx = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 + qy = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2 + qz = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2 + qw = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 + + # R(q_mo) * p_odom_base + px, py, pz = ob_t.x, ob_t.y, ob_t.z + rx = (1 - 2*qy*qy - 2*qz*qz)*px + (2*qx*qy - 2*qz*qw)*py + (2*qx*qz + 2*qy*qw)*pz + ry = (2*qx*qy + 2*qz*qw)*px + (1 - 2*qx*qx - 2*qz*qz)*py + (2*qy*qz - 2*qx*qw)*pz + rz = (2*qx*qz - 2*qy*qw)*px + (2*qy*qz + 2*qx*qw)*py + (1 - 2*qx*qx - 2*qy*qy)*pz + + t = TransformStamped() + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = self.map_frame_ + t.child_frame_id = self.odom_frame_ + t.transform.translation.x = map_to_base.position.x - rx + t.transform.translation.y = map_to_base.position.y - ry + t.transform.translation.z = map_to_base.position.z - rz + t.transform.rotation.x = qx + t.transform.rotation.y = qy + t.transform.rotation.z = qz + t.transform.rotation.w = qw + self.static_broadcaster_.sendTransform(t) + self.get_logger().info( + f'Updated map→odom: pos=({t.transform.translation.x:.3f},' + f' {t.transform.translation.y:.3f})') + + +def main(): + rclpy.init() + node = InitialPoseToTF() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/navigation/obstacle_nav2/scripts/static_map_publisher.py b/src/navigation/obstacle_nav2/scripts/static_map_publisher.py new file mode 100644 index 0000000..06ebdc6 --- /dev/null +++ b/src/navigation/obstacle_nav2/scripts/static_map_publisher.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python3 +"""static_map_publisher.py — 从 YAML+PNG 加载并发布 /map OccupancyGrid。""" + +import os, math +import numpy as np +from PIL import Image +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy +from nav_msgs.msg import OccupancyGrid, MapMetaData +from geometry_msgs.msg import Pose +import yaml + + +class StaticMapPublisher(Node): + def __init__(self): + super().__init__('static_map_publisher') + self.declare_parameter('yaml_filename', '') + self.declare_parameter('publish_rate', 1.0) + self.declare_parameter('map_frame', 'map') + + path = self.get_parameter('yaml_filename').value + rate = self.get_parameter('publish_rate').value + frame = self.get_parameter('map_frame').value + if not path: + raise RuntimeError('yaml_filename required') + + self._grid_ = self._load(path, frame) + map_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) + self._pub_ = self.create_publisher(OccupancyGrid, '/map', map_qos) + self._timer_ = self.create_timer(1.0 / max(0.1, rate), self._publish) + self.get_logger().info(f'map published on /map ({rate} Hz)') + + def _load(self, yaml_path, frame): + d = os.path.dirname(os.path.abspath(yaml_path)) + with open(yaml_path) as f: + cfg = yaml.safe_load(f) + img = Image.open(os.path.join(d, cfg['image'])).convert('L') + w, h = img.size + data = np.array(img, dtype=np.int32) + mode = cfg.get('mode', 'trinary') + negate = int(cfg.get('negate', 0)) + occ_th = float(cfg.get('occupied_thresh', 0.65)) + free_th = float(cfg.get('free_thresh', 0.196)) + + if mode == 'trinary': + grid = np.full_like(data, -1, dtype=np.int8) + grid[data == 0] = 100 + grid[data >= 254] = 0 + else: + if negate: + data = 255 - data + grid = np.full_like(data, -1, dtype=np.int8) + grid[data > occ_th * 255] = 100 + grid[data < free_th * 255] = 0 + + grid = np.flipud(grid) + origin = cfg.get('origin', [0, 0, 0]) + msg = OccupancyGrid() + msg.header.frame_id = frame + msg.info = MapMetaData() + msg.info.resolution = float(cfg.get('resolution', 0.05)) + msg.info.width = w + msg.info.height = h + msg.info.origin = Pose() + msg.info.origin.position.x = float(origin[0]) + msg.info.origin.position.y = float(origin[1]) + yaw = float(origin[2]) if len(origin) > 2 else 0.0 + msg.info.origin.orientation.w = math.cos(yaw / 2) + msg.info.origin.orientation.z = math.sin(yaw / 2) + msg.data = grid.flatten().tolist() + return msg + + def _publish(self): + self._grid_.header.stamp = self.get_clock().now().to_msg() + self._pub_.publish(self._grid_) + + +def main(): + rclpy.init() + try: + rclpy.spin(StaticMapPublisher()) + except RuntimeError as e: + rclpy.get_logger('static_map_publisher').fatal(str(e)) + except KeyboardInterrupt: + pass + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/origincar_base/include/origincar_base/log.hpp b/src/origincar_base/include/origincar_base/log.hpp index e12b770..1e0c855 100644 --- a/src/origincar_base/include/origincar_base/log.hpp +++ b/src/origincar_base/include/origincar_base/log.hpp @@ -25,6 +25,8 @@ public: bool finishFrame(uint64_t frame_id, TimePoint now = Clock::now()); bool cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now = Clock::now()); bool makeSummaryLineIfDue(TimePoint now, std::string *line); + void setPrintToTerminal(bool enabled); + bool printToTerminal() const; const std::string &logPath() const; bool isOpen() const; @@ -50,6 +52,7 @@ private: TimePoint last_summary_time_; std::string log_path_; std::ofstream log_file_; + bool print_to_terminal_{false}; }; } // namespace origincar_base_logging diff --git a/src/origincar_base/include/origincar_base/origincar_base.h b/src/origincar_base/include/origincar_base/origincar_base.h index 9da67d4..8b30ff0 100644 --- a/src/origincar_base/include/origincar_base/origincar_base.h +++ b/src/origincar_base/include/origincar_base/origincar_base.h @@ -237,6 +237,7 @@ private: string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test; string scan_topic_, wall_config_path_, combined_odom_topic_; bool publish_tf_; + bool scan_odom_timing_to_terminal_; double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_; int wall_scan_stride_; std::string cmd_vel; diff --git a/src/origincar_base/include/origincar_base/wall_kalman_filter.hpp b/src/origincar_base/include/origincar_base/wall_kalman_filter.hpp index b088430..72de718 100644 --- a/src/origincar_base/include/origincar_base/wall_kalman_filter.hpp +++ b/src/origincar_base/include/origincar_base/wall_kalman_filter.hpp @@ -13,6 +13,23 @@ struct WallKalmanNoise double theta; }; +enum class WallScanTrust +{ + Reject, + Weak, + Medium, + High, +}; + +struct WallScanUpdatePolicy +{ + bool accept; + WallScanTrust trust; + WallKalmanNoise noise; + double innovation_distance; + std::string reason; +}; + class WallKalmanFilter { public: @@ -31,6 +48,11 @@ private: WallKalmanNoise process_noise_per_second_; }; +WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose, + const Pose2D &scan_pose, + const LocalizationQuality &quality, + bool stationary); + } // namespace origincar_wall #endif // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_ diff --git a/src/origincar_base/launch/base_serial.launch.py b/src/origincar_base/launch/base_serial.launch.py index 51448df..ca69ee2 100644 --- a/src/origincar_base/launch/base_serial.launch.py +++ b/src/origincar_base/launch/base_serial.launch.py @@ -8,7 +8,7 @@ def generate_launch_description(): 'serial_baud_rate': 921600, 'serial_read_timeout_ms': 20, 'tx_period_ms': 20, - 'cmd_watchdog_timeout_ms': 150, + 'cmd_watchdog_timeout_ms': 500, 'control_period_ms': 50, 'robot_frame_id': 'base_footprint', 'odom_frame_id': 'odom', diff --git a/src/origincar_base/src/log.cpp b/src/origincar_base/src/log.cpp index b55ffed..543dc4e 100644 --- a/src/origincar_base/src/log.cpp +++ b/src/origincar_base/src/log.cpp @@ -164,6 +164,16 @@ bool ScanOdomTimingLogger::makeSummaryLineIfDue(TimePoint now, std::string *line return true; } +void ScanOdomTimingLogger::setPrintToTerminal(bool enabled) +{ + print_to_terminal_ = enabled; +} + +bool ScanOdomTimingLogger::printToTerminal() const +{ + return print_to_terminal_; +} + const std::string &ScanOdomTimingLogger::logPath() const { return log_path_; diff --git a/src/origincar_base/src/origincar_base.cpp b/src/origincar_base/src/origincar_base.cpp index 9a5b980..5260b66 100644 --- a/src/origincar_base/src/origincar_base.cpp +++ b/src/origincar_base/src/origincar_base.cpp @@ -307,6 +307,10 @@ void origincar_base::Apply_Wall_Update() void origincar_base::Print_Timing_Log_If_Due() { + if (!scan_odom_timing_to_terminal_) + { + return; + } std::string timing_summary; if (scan_odom_timing_logger_.makeSummaryLineIfDue(std::chrono::steady_clock::now(), &timing_summary)) { @@ -639,6 +643,7 @@ origincar_base::origincar_base() this->declare_parameter("scan_topic", "/scan"); this->declare_parameter("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json"); this->declare_parameter("publish_tf", true); + this->declare_parameter("scan_odom_timing_to_terminal", false); this->declare_parameter("wall_scan_stride", 2); this->declare_parameter("laser_x", 0.0); this->declare_parameter("laser_y", 0.0); @@ -665,6 +670,7 @@ origincar_base::origincar_base() this->get_parameter("scan_topic", scan_topic_); this->get_parameter("wall_config_path", wall_config_path_); this->get_parameter("publish_tf", publish_tf_); + this->get_parameter("scan_odom_timing_to_terminal", scan_odom_timing_to_terminal_); this->get_parameter("wall_scan_stride", wall_scan_stride_); laser_pose_ = { this->get_parameter("laser_x").as_double(), @@ -702,6 +708,7 @@ origincar_base::origincar_base() 0.0, }; wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose); + scan_odom_timing_logger_.setPrintToTerminal(scan_odom_timing_to_terminal_); Robot_Pos.X = static_cast(initial_pose.x); Robot_Pos.Y = static_cast(initial_pose.y); Robot_Pos.Z = static_cast(initial_pose.theta); diff --git a/src/origincar_base/src/wall_kalman_filter.cpp b/src/origincar_base/src/wall_kalman_filter.cpp index c007ebf..f04a810 100644 --- a/src/origincar_base/src/wall_kalman_filter.cpp +++ b/src/origincar_base/src/wall_kalman_filter.cpp @@ -6,6 +6,24 @@ namespace origincar_wall { +namespace +{ +WallKalmanNoise highTrustNoise() +{ + return {0.015, 0.015, 0.025}; +} + +WallKalmanNoise mediumTrustNoise() +{ + return {0.045, 0.045, 0.060}; +} + +WallKalmanNoise weakTrustNoise() +{ + return {0.10, 0.10, 0.12}; +} +} // namespace + WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose) : pose_(initial_pose), covariance_{0.10, 0.10, 0.10}, @@ -63,4 +81,75 @@ WallKalmanNoise WallKalmanFilter::covariance() const return covariance_; } +WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose, + const Pose2D &scan_pose, + const LocalizationQuality &quality, + bool stationary) +{ + WallScanUpdatePolicy policy{false, WallScanTrust::Reject, weakTrustNoise(), 0.0, ""}; + policy.innovation_distance = std::hypot(scan_pose.x - filter_pose.x, scan_pose.y - filter_pose.y); + + if (!quality.has_mean_error) + { + policy.reason = "missing mean error"; + return policy; + } + if (quality.wall_count < 3) + { + policy.reason = "too few walls"; + return policy; + } + if (quality.mean_error > 0.15) + { + policy.reason = "mean error too high"; + return policy; + } + + if (stationary && policy.innovation_distance > 0.04) + { + policy.reason = "stationary innovation too large"; + return policy; + } + if (!stationary && policy.innovation_distance > 0.15) + { + policy.reason = "motion innovation too large"; + return policy; + } + + if (quality.wall_count == 4 && quality.mean_error < 0.05) + { + policy.accept = true; + policy.trust = stationary ? WallScanTrust::Medium : WallScanTrust::High; + policy.noise = stationary ? mediumTrustNoise() : highTrustNoise(); + policy.reason = stationary ? "stationary high trust downgraded to medium" : "high trust"; + return policy; + } + + if (quality.wall_count >= 3 && quality.mean_error < 0.10) + { + policy.accept = true; + policy.trust = stationary ? WallScanTrust::Weak : WallScanTrust::Medium; + policy.noise = stationary ? weakTrustNoise() : mediumTrustNoise(); + policy.reason = stationary ? "stationary medium trust downgraded to weak" : "medium trust"; + return policy; + } + + if (quality.wall_count >= 3 && quality.mean_error <= 0.15) + { + if (stationary) + { + policy.reason = "stationary weak trust rejected"; + return policy; + } + policy.accept = true; + policy.trust = WallScanTrust::Weak; + policy.noise = weakTrustNoise(); + policy.reason = "weak trust"; + return policy; + } + + policy.reason = "rejected by policy"; + return policy; +} + } // namespace origincar_wall diff --git a/src/origincar_base/test/scan_odom_timing_logger_test.cpp b/src/origincar_base/test/scan_odom_timing_logger_test.cpp index f996464..ef8204d 100644 --- a/src/origincar_base/test/scan_odom_timing_logger_test.cpp +++ b/src/origincar_base/test/scan_odom_timing_logger_test.cpp @@ -85,6 +85,15 @@ void testSummaryPrintIsThrottledToOneHz() require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1500), &summary), "summary should be throttled after printing"); } + +void testTerminalOutputDefaultsToOff() +{ + const auto root = makeTempDir(); + origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root); + require(!logger.printToTerminal(), "terminal output should default to off"); + logger.setPrintToTerminal(true); + require(logger.printToTerminal(), "terminal output should be enabled"); +} } // namespace int main() @@ -93,6 +102,7 @@ int main() { testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate(); testSummaryPrintIsThrottledToOneHz(); + testTerminalOutputDefaultsToOff(); } catch (const std::exception &error) { diff --git a/src/origincar_base/test/wall_kalman_filter_test.cpp b/src/origincar_base/test/wall_kalman_filter_test.cpp index 8dc0a57..2d106de 100644 --- a/src/origincar_base/test/wall_kalman_filter_test.cpp +++ b/src/origincar_base/test/wall_kalman_filter_test.cpp @@ -60,6 +60,69 @@ void testYawCorrectionUsesWrappedInnovation() require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary"); } + +void testScanPolicyGradesByWallCountAndMeanError() +{ + const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0}; + const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0}; + + const auto high = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, scan_pose, {4, 0.04, true}, false); + require(high.accept, "4 walls with low error should be accepted"); + require(high.trust == origincar_wall::WallScanTrust::High, "4 walls with low error should be high trust"); + requireNear(high.noise.x, 0.015, 1e-12, "high trust x noise"); + + const auto medium = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, scan_pose, {3, 0.08, true}, false); + require(medium.accept, "3 walls with medium error should be accepted"); + require(medium.trust == origincar_wall::WallScanTrust::Medium, "3 walls with medium error should be medium trust"); + requireNear(medium.noise.x, 0.045, 1e-12, "medium trust x noise"); + + const auto weak = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, scan_pose, {3, 0.12, true}, false); + require(weak.accept, "3 walls with weak error should be accepted as weak"); + require(weak.trust == origincar_wall::WallScanTrust::Weak, "3 walls with weak error should be weak trust"); + requireNear(weak.noise.x, 0.10, 1e-12, "weak trust x noise"); +} + +void testScanPolicyRejectsPoorOrJumpingMeasurements() +{ + const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0}; + + const auto too_few_walls = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, {1.01, 1.0, 0.0}, {2, 0.04, true}, false); + require(!too_few_walls.accept, "fewer than 3 walls should be rejected"); + + const auto too_noisy = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, {1.01, 1.0, 0.0}, {4, 0.16, true}, false); + require(!too_noisy.accept, "mean error above 0.15 should be rejected"); + + const auto stationary_jump = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, {1.06, 1.0, 0.0}, {4, 0.03, true}, true); + require(!stationary_jump.accept, "stationary scan jump above 0.04m should be rejected"); +} + +void testScanPolicyDowngradesWhileStationary() +{ + const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0}; + const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0}; + + const auto downgraded_high = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, scan_pose, {4, 0.04, true}, true); + require(downgraded_high.accept, "stationary high trust scan should still be accepted"); + require(downgraded_high.trust == origincar_wall::WallScanTrust::Medium, + "stationary high trust scan should downgrade to medium"); + + const auto downgraded_medium = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, scan_pose, {3, 0.08, true}, true); + require(downgraded_medium.accept, "stationary medium trust scan should still be accepted"); + require(downgraded_medium.trust == origincar_wall::WallScanTrust::Weak, + "stationary medium trust scan should downgrade to weak"); + + const auto downgraded_weak = origincar_wall::chooseWallScanUpdatePolicy( + filter_pose, scan_pose, {3, 0.12, true}, true); + require(!downgraded_weak.accept, "stationary weak trust scan should be rejected"); +} } // namespace int main() @@ -69,6 +132,9 @@ int main() testPredictUsesWheelVelocityAndImuYawRate(); testScanUpdateTrustsLowNoiseMoreThanHighNoise(); testYawCorrectionUsesWrappedInnovation(); + testScanPolicyGradesByWallCountAndMeanError(); + testScanPolicyRejectsPoorOrJumpingMeasurements(); + testScanPolicyDowngradesWhileStationary(); } catch (const std::exception &error) { diff --git a/src/racing_control/AGENTS.md b/src/racing_control/AGENTS.md index 913337f..2b9d371 100644 --- a/src/racing_control/AGENTS.md +++ b/src/racing_control/AGENTS.md @@ -3,7 +3,7 @@ ## 项目用途 - 这是 RDKx5 赛车机器人的 ROS2 Humble 工作区。 -- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和轨迹保护节点，不在包内重新实现这些功能。 +- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和备用轨迹保护节点，不在包内重新实现这些功能。 ## 环境准备 @@ -25,8 +25,9 @@ - `/sign4return` 是共享的 `std_msgs/msg/Int32` 控制话题： `0` 开启二维码检测，`5` 关闭二维码检测，`9` 触发 VLM 拍照识别，`10` 切换普通 Nav2 参数，`11` 切换任务二 Nav2 参数。 - `obstacle_nav2/nav2_profile_tuner` 监听 `/sign4return`，并根据 10 或 11 应用对应的 Nav2 profile。 -- `obstacle_nav2/trajectory_guard_node` 订阅 `/trajectory_guard/input_path`；当 `execute_follow_path` 为 true 时，它会把保护后的路径发送给 Nav2 `/follow_path`。 -- 比赛总控使用 Nav2 actions（`/navigate_to_pose`、`/compute_path_through_poses`、`/follow_path`），比赛点位应保持为可通过 ROS 参数配置。 +- `obstacle_nav2/trajectory_guard_node` 订阅 `/trajectory_guard/input_path`，属于备用链路；默认比赛流程不依赖它。 +- 比赛总控使用 Nav2 actions（`/navigate_to_pose`、`/compute_path_through_poses`、`/follow_path`），当前默认不走 `trajectory_guard`。 +- 当前 route 执行默认启用动态重规划和短倒车恢复：距离 segment 终点大于 `dynamic_replan_stop_distance` 时按 `dynamic_replan_interval_sec` 重算路径；FollowPath/planner 连续失败会短倒车恢复，默认 `-0.2m/s`、`0.04m` 或 `0.2s` 停止，最多 `max_recovery_attempts: 2`。 ## 外部系统 diff --git a/src/racing_control/config/racing_control.yaml b/src/racing_control/config/racing_control.yaml index 747b45f..2cb63e5 100644 --- a/src/racing_control/config/racing_control.yaml +++ b/src/racing_control/config/racing_control.yaml @@ -3,8 +3,10 @@ racing_control: # Startup auto_start: false frame_id: odom - use_post_qr_pose: true + use_post_qr_pose: false enable_vlm_image_relay: false + enable_dynamic_replanning: true + enable_recovery: true vlm_image_input_topic: /image vlm_image_output_topic: /vlm_image @@ -13,6 +15,7 @@ racing_control: qr_result_topic: /qr_results vlm_result_topic: /vlm_result odom_topic: /odom_combined + recovery_cmd_vel_topic: /cmd_vel trajectory_guard_input_topic: /trajectory_guard/input_path # Nav2 actions and plugin IDs @@ -22,16 +25,23 @@ racing_control: planner_id: GridBased controller_id: FollowPath goal_checker_id: "" - use_trajectory_guard: true + use_trajectory_guard: false # Timeouts and settling waits, in seconds navigation_timeout_sec: 120.0 path_planning_timeout_sec: 30.0 circle_timeout_sec: 120.0 - qr_result_timeout_sec: 8.0 + qr_result_timeout_sec: 20.0 profile_switch_wait_sec: 1.0 post_qr_wait_sec: 1.0 vlm_capture_wait_sec: 0.5 + dynamic_replan_interval_sec: 1.0 + dynamic_replan_stop_distance: 0.5 + dynamic_replan_max_consecutive_failures: 3 + recovery_backup_speed: -0.2 + recovery_backup_distance: 0.04 + recovery_backup_timeout_sec: 0.2 + max_recovery_attempts: 2 circle_goal_tolerance: 0.30 # VLM capture: @@ -48,61 +58,36 @@ racing_control: sign_profile_task2: 11 # Pose parameters are flat x/y/yaw-radians triples in frame_id. - qr_pose: [4.3860322643582883, 1.2934897914487595, 0.94658891138326606] - post_qr_pose: [4.4004663023808863, 0.46113350031559491, 1.7514152174101529] - entry_pose: [2.499999919243812, 2.1932039710724882, 1.5174118916273021] + qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392] + post_qr_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562] + entry_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562] # The route's Nth waypoint is used as the VLM capture point. - # With the saved JSON files, the 4th point is goal_011 for main_1 - # and goal_004 for main_2. - vlm_waypoint_number: 4 + # For the current point set, the 2nd waypoint is the VLM point. + vlm_waypoint_number: 2 # main_1.json: clockwise route, excluding home. clockwise_waypoints: - - 1.542550031688728 - - 3.1698990676859604 - - 3.1415926535897918 - - 0.71981664792046363 - - 3.7665012349236271 - - 1.5495230091246759 - - 1.3164186536457545 - - 4.3294241954584018 - - -0.016392199787162172 - - 3.659525047016253 - - 4.324612741775951 - - 4.6883147711881255e-16 - - 4.2320702688664218 - - 3.7761238192637752 - - -1.5784882007253702 - - 3.510374505206836 - - 3.1650879370282627 - - -3.0916338672023889 - - 2.5288679952890067 - - 2.4578258883665218 - - -1.6718998381125814 - clockwise_home_pose: [0.51774173072785878, 0.17726629320698351, -2.2032146659725105] + - 0.7534956931109804 + - 3.766501234923627 + - 1.5592359900104606 + - 3.8567885105264077 + - 4.329424195458402 + - 0.03997856199797794 + - 2.5144339572664096 + - 2.4337692660037766 + - -1.6101462328054222 + clockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445] # main_2.json: counterclockwise route, excluding home. counterclockwise_waypoints: - - 3.3612239633974195 - - 3.1747105213684104 - - 0.010869470292991894 - - 4.2272591382087246 - - 3.7280108975630366 - - 1.5707963267948966 - - 3.4237709231207547 - - 4.3486693641386971 - - 3.123412893984987 - - 1.2731168626027138 - - 4.334235487628475 - - -3.1165979438417697 - - 0.74868440094090649 - - 3.7376334819031838 - - -1.5593026199905524 - - 1.3933994898793114 - - 3.1795219750508603 - - -0.01136349401017399 - - 2.4663210355656715 - - 2.4145240973234814 - - -1.5556465256578393 - counterclockwise_home_pose: [0.51774173072785878, 0.15802112452668779, -2.1396781415005068] + - 4.2128251001861265 + - 3.7376334819031842 + - 1.5450283923222128 + - 1.1720794040064113 + - 4.319801449605877 + - 3.1112984779040144 + - 2.4951887885861144 + - 2.318297930897253 + - -1.539556591616333 + counterclockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445] diff --git a/src/racing_control/include/racing_control/racing_control.hpp b/src/racing_control/include/racing_control/racing_control.hpp index 093b24f..95d7f8a 100644 --- a/src/racing_control/include/racing_control/racing_control.hpp +++ b/src/racing_control/include/racing_control/racing_control.hpp @@ -86,6 +86,60 @@ inline bool shouldPublishVlmImageFrame( return enable_vlm_image_relay && has_latest_image; } +template +inline bool retryActionServerWait( + WaitForActionServerFn && wait_for_action_server, + const int retry_count) +{ + for (int attempt = 0; attempt <= retry_count; ++attempt) { + if (wait_for_action_server()) { + return true; + } + } + return false; +} + +inline bool shouldRetryNavigateGoal(const bool succeeded, const int retries_remaining) +{ + return !succeeded && retries_remaining > 0; +} + +inline int defaultStartupQrEnableRepeats() +{ + return 5; +} + +inline int defaultStartupQrEnableIntervalMs() +{ + return 200; +} + +inline bool shouldDynamicReplan( + const bool enabled, + const bool replan_in_flight, + const double distance_to_goal, + const double stop_distance, + const double elapsed_since_last_replan, + const double replan_interval) +{ + return enabled && !replan_in_flight && distance_to_goal > stop_distance && + elapsed_since_last_replan >= replan_interval; +} + +inline bool recoveryBackupComplete( + const double backup_distance, + const double target_distance, + const double elapsed_sec, + const double timeout_sec) +{ + return backup_distance >= target_distance || elapsed_sec >= timeout_sec; +} + +inline bool defaultUseTrajectoryGuard() +{ + return false; +} + inline geometry_msgs::msg::PoseStamped poseFromXYYaw( const double x, const double y, const double yaw, const std::string & frame_id) { diff --git a/src/racing_control/src/racing_control copy.cpp b/src/racing_control/src/racing_control copy.cpp index b820f19..9e39003 100644 --- a/src/racing_control/src/racing_control copy.cpp +++ b/src/racing_control/src/racing_control copy.cpp @@ -203,7 +203,7 @@ private: auto_start_ = declare_parameter("auto_start", false); use_trajectory_guard_ = declare_parameter("use_trajectory_guard", true); - use_post_qr_pose_ = declare_parameter("use_post_qr_pose", true); + use_post_qr_pose_ = declare_parameter("use_post_qr_pose", false); enable_vlm_image_relay_ = declare_parameter("enable_vlm_image_relay", false); vlm_image_input_topic_ = declare_parameter("vlm_image_input_topic", "/image"); vlm_image_output_topic_ = @@ -912,8 +912,8 @@ private: Stage stage_{Stage::Idle}; bool race_started_{false}; bool auto_start_{false}; - bool use_trajectory_guard_{true}; - bool use_post_qr_pose_{true}; + bool use_trajectory_guard_{false}; + bool use_post_qr_pose_{false}; bool enable_vlm_image_relay_{false}; bool qr_detection_disabled_{false}; rclcpp::Time race_start_{0, 0, RCL_ROS_TIME}; @@ -950,7 +950,7 @@ private: int sign_vlm_trigger_{9}; int sign_profile_normal_{10}; int sign_profile_task2_{11}; - int vlm_waypoint_number_{4}; + int vlm_waypoint_number_{2}; geometry_msgs::msg::PoseStamped qr_pose_; geometry_msgs::msg::PoseStamped post_qr_pose_; diff --git a/src/racing_control/src/racing_control.cpp b/src/racing_control/src/racing_control.cpp index b820f19..257b264 100644 --- a/src/racing_control/src/racing_control.cpp +++ b/src/racing_control/src/racing_control.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -17,6 +18,7 @@ #include #include +#include "geometry_msgs/msg/twist.hpp" #include "nav2_msgs/action/compute_path_through_poses.hpp" #include "nav2_msgs/action/follow_path.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" @@ -135,6 +137,9 @@ public: loadParameters(); sign_pub_ = create_publisher(sign_topic_, 10); + startStartupQrEnablePublisher(); + recovery_cmd_vel_pub_ = create_publisher( + recovery_cmd_vel_topic_, 10); guard_path_pub_ = create_publisher(guard_input_topic_, 1); if (enable_vlm_image_relay_) { vlm_image_pub_ = @@ -189,6 +194,8 @@ private: qr_result_topic_ = declare_parameter("qr_result_topic", "/qr_results"); vlm_result_topic_ = declare_parameter("vlm_result_topic", "/vlm_result"); odom_topic_ = declare_parameter("odom_topic", "/odom_combined"); + recovery_cmd_vel_topic_ = + declare_parameter("recovery_cmd_vel_topic", "/cmd_vel"); navigate_action_ = declare_parameter("navigate_action", "/navigate_to_pose"); compute_path_action_ = declare_parameter("compute_path_action", "/compute_path_through_poses"); @@ -202,9 +209,12 @@ private: goal_checker_id_ = declare_parameter("goal_checker_id", ""); auto_start_ = declare_parameter("auto_start", false); - use_trajectory_guard_ = declare_parameter("use_trajectory_guard", true); - use_post_qr_pose_ = declare_parameter("use_post_qr_pose", true); + use_trajectory_guard_ = declare_parameter( + "use_trajectory_guard", defaultUseTrajectoryGuard()); + use_post_qr_pose_ = declare_parameter("use_post_qr_pose", false); enable_vlm_image_relay_ = declare_parameter("enable_vlm_image_relay", false); + enable_dynamic_replanning_ = declare_parameter("enable_dynamic_replanning", true); + enable_recovery_ = declare_parameter("enable_recovery", true); vlm_image_input_topic_ = declare_parameter("vlm_image_input_topic", "/image"); vlm_image_output_topic_ = declare_parameter("vlm_image_output_topic", "/vlm_image"); @@ -215,9 +225,20 @@ private: profile_switch_wait_sec_ = declare_parameter("profile_switch_wait_sec", 1.0); post_qr_wait_sec_ = declare_parameter("post_qr_wait_sec", 1.0); vlm_capture_wait_sec_ = declare_parameter("vlm_capture_wait_sec", 0.5); + dynamic_replan_interval_sec_ = + declare_parameter("dynamic_replan_interval_sec", 1.0); + dynamic_replan_stop_distance_ = + declare_parameter("dynamic_replan_stop_distance", 0.5); + recovery_backup_speed_ = declare_parameter("recovery_backup_speed", -0.2); + recovery_backup_distance_ = declare_parameter("recovery_backup_distance", 0.04); + recovery_backup_timeout_sec_ = + declare_parameter("recovery_backup_timeout_sec", 0.2); pass_through_vlm_trigger_radius_ = declare_parameter("pass_through_vlm_trigger_radius", 0.35); circle_goal_tolerance_ = declare_parameter("circle_goal_tolerance", 0.30); + dynamic_replan_max_consecutive_failures_ = + declare_parameter("dynamic_replan_max_consecutive_failures", 3); + max_recovery_attempts_ = declare_parameter("max_recovery_attempts", 2); const auto vlm_capture_mode = declare_parameter("vlm_capture_mode", "stop"); @@ -232,29 +253,30 @@ private: sign_profile_normal_ = declare_parameter("sign_profile_normal", 10); sign_profile_task2_ = declare_parameter("sign_profile_task2", 11); - qr_pose_ = singlePoseFromParameter("qr_pose", {0.80, 0.20, 0.0}); - entry_pose_ = singlePoseFromParameter("entry_pose", {1.20, 0.20, 0.0}); - post_qr_pose_ = singlePoseFromParameter("post_qr_pose", {1.20, 0.20, 0.0}); - vlm_waypoint_number_ = declare_parameter("vlm_waypoint_number", 4); + qr_pose_ = singlePoseFromParameter( + "qr_pose", {4.333107888975101, 1.028867429995691, 1.1383894869544392}); + entry_pose_ = singlePoseFromParameter( + "entry_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562}); + post_qr_pose_ = singlePoseFromParameter( + "post_qr_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562}); + vlm_waypoint_number_ = declare_parameter("vlm_waypoint_number", 2); const auto clockwise_defaults = std::vector{ - 1.20, 0.80, 1.5708, - 2.20, 0.80, 0.0, - 2.20, 1.40, 1.5708, - 1.20, 1.40, 3.1416, - 1.20, 0.80, -1.5708}; + 0.7534956931109804, 3.766501234923627, 1.5592359900104606, + 3.8567885105264077, 4.329424195458402, 0.03997856199797794, + 2.5144339572664096, 2.4337692660037766, -1.6101462328054222}; const auto counterclockwise_defaults = std::vector{ - 1.20, 0.80, -1.5708, - 1.20, 1.40, 3.1416, - 2.20, 1.40, 1.5708, - 2.20, 0.80, 0.0, - 1.20, 0.80, 1.5708}; + 4.2128251001861265, 3.7376334819031842, 1.5450283923222128, + 1.1720794040064113, 4.319801449605877, 3.1112984779040144, + 2.4951887885861144, 2.318297930897253, -1.539556591616333}; clockwise_route_.label = "顺时针"; clockwise_route_.waypoints = posesFromFlatDoubles( declare_parameter>("clockwise_waypoints", clockwise_defaults), frame_id_); clockwise_route_.home_pose = - singlePoseFromParameter("clockwise_home_pose", {0.54, 0.20, 0.0}); + singlePoseFromParameter( + "clockwise_home_pose", {0.536986899408154, 0.1772662932069835, + -1.652041913138445}); counterclockwise_route_.label = "逆时针"; counterclockwise_route_.waypoints = posesFromFlatDoubles( @@ -263,7 +285,9 @@ private: counterclockwise_defaults), frame_id_); counterclockwise_route_.home_pose = - singlePoseFromParameter("counterclockwise_home_pose", {0.54, 0.20, 0.0}); + singlePoseFromParameter( + "counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835, + -1.652041913138445}); } geometry_msgs::msg::PoseStamped singlePoseFromParameter( @@ -326,6 +350,10 @@ private: return; } + if (recovery_in_progress_) { + return; + } + const auto elapsed = (now() - stage_start_).seconds(); if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) { if (stage_ == Stage::WaitForQr) { @@ -371,6 +399,7 @@ private: if (stage_ == Stage::ExecuteCirclePath) { maybeTriggerPassThroughVlmCapture(); + maybeRunDynamicReplanning(); } if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) { @@ -417,6 +446,12 @@ private: void failRace(const std::string & reason) { stage_ = Stage::Failed; + recovery_in_progress_ = false; + if (recovery_timer_) { + recovery_timer_->cancel(); + } + cancelActiveFollowGoal(); + publishRecoveryVelocity(0.0); publishSign(sign_qr_disable_); RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str()); } @@ -439,6 +474,38 @@ private: RCLCPP_INFO(get_logger(), "published %s=%d", sign_topic_.c_str(), value); } + void startStartupQrEnablePublisher() + { + startup_qr_enable_publish_count_ = 0; + publishStartupQrEnableOnce(); + startup_qr_enable_timer_ = create_wall_timer( + std::chrono::milliseconds(defaultStartupQrEnableIntervalMs()), + [this]() {publishStartupQrEnableOnce();}); + } + + void publishStartupQrEnableOnce() + { + if (race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) { + if (startup_qr_enable_timer_) { + startup_qr_enable_timer_->cancel(); + } + return; + } + if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats()) { + if (startup_qr_enable_timer_) { + startup_qr_enable_timer_->cancel(); + } + return; + } + publishSign(sign_qr_enable_); + ++startup_qr_enable_publish_count_; + if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats() && + startup_qr_enable_timer_) + { + startup_qr_enable_timer_->cancel(); + } + } + void disableQrDetectionOnce() { if (qr_detection_disabled_) { @@ -545,6 +612,7 @@ private: const auto vlm_index = vlmWaypointIndex(route); latest_vlm_result_.clear(); vlm_capture_triggered_ = false; + recovery_attempts_ = 0; if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) { active_segment_ = RouteSegment::FullRoute; active_segment_waypoints_ = route.waypoints; @@ -567,6 +635,7 @@ private: runSwitchToNormalProfile(); return; } + recovery_attempts_ = 0; active_segment_ = RouteSegment::AfterVlm; active_segment_waypoints_.assign( route.waypoints.begin() + vlm_index + 1, @@ -583,7 +652,7 @@ private: startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_); if (!compute_path_client_->wait_for_action_server(2s)) { - failRace("ComputePathThroughPoses action server is not available"); + handleRouteExecutionFailure("ComputePathThroughPoses action server is not available"); return; } @@ -596,7 +665,7 @@ private: options.goal_response_callback = [this](ComputeGoalHandle::SharedPtr goal_handle) { if (!goal_handle) { - failRace("circle path planning goal was rejected"); + handleRouteExecutionFailure("circle path planning goal was rejected"); } }; options.result_callback = @@ -608,7 +677,7 @@ private: result.result->path.poses.empty()) { finishStage("planning failed"); - failRace("circle path planning failed"); + handleRouteExecutionFailure("circle path planning failed"); return; } active_path_ = result.result->path; @@ -624,6 +693,9 @@ private: void runRouteSegmentExecution() { startStage(Stage::ExecuteCirclePath, circle_timeout_sec_); + dynamic_replan_in_flight_ = false; + dynamic_replan_consecutive_failures_ = 0; + last_dynamic_replan_time_ = now(); if (use_trajectory_guard_) { auto path = stampPath(active_path_); guard_path_pub_->publish(path); @@ -632,11 +704,16 @@ private: path.poses.size(), guard_input_topic_.c_str()); return; } + sendActiveRouteFollowPath(); + } + + void sendActiveRouteFollowPath() + { sendFollowPath( active_path_, [this](const bool ok) { finishStage(ok ? "FollowPath succeeded" : "FollowPath failed"); if (!ok) { - failRace("route segment FollowPath failed"); + handleRouteExecutionFailure("route segment FollowPath failed"); return; } if (active_segment_ == RouteSegment::ToVlm) { @@ -723,6 +800,171 @@ private: } } + void maybeRunDynamicReplanning() + { + if (use_trajectory_guard_ || active_segment_waypoints_.empty()) { + return; + } + const auto current = currentPoseFromOdom(); + if (!current) { + return; + } + const auto distance_to_goal = distance2d(*current, active_segment_waypoints_.back()); + const auto elapsed = (now() - last_dynamic_replan_time_).seconds(); + if (!shouldDynamicReplan( + enable_dynamic_replanning_, dynamic_replan_in_flight_, distance_to_goal, + dynamic_replan_stop_distance_, elapsed, dynamic_replan_interval_sec_)) + { + return; + } + + last_dynamic_replan_time_ = now(); + dynamic_replan_in_flight_ = true; + runDynamicRouteReplanning(); + } + + void runDynamicRouteReplanning() + { + if (!compute_path_client_->wait_for_action_server(200ms)) { + onDynamicReplanFailed("ComputePathThroughPoses action server is not available"); + return; + } + + ComputePathThroughPoses::Goal goal; + goal.goals = stampPoses({active_segment_waypoints_.back()}); + goal.planner_id = planner_id_; + goal.use_start = false; + + auto options = rclcpp_action::Client::SendGoalOptions(); + options.goal_response_callback = + [this](ComputeGoalHandle::SharedPtr goal_handle) { + if (!goal_handle) { + onDynamicReplanFailed("dynamic route planning goal was rejected"); + } + }; + options.result_callback = + [this](const ComputeGoalHandle::WrappedResult & result) { + if (stage_ != Stage::ExecuteCirclePath) { + dynamic_replan_in_flight_ = false; + return; + } + dynamic_replan_in_flight_ = false; + if (result.code != rclcpp_action::ResultCode::SUCCEEDED || + result.result->path.poses.empty()) + { + onDynamicReplanFailed("dynamic route planning failed"); + return; + } + dynamic_replan_consecutive_failures_ = 0; + active_path_ = result.result->path; + RCLCPP_INFO( + get_logger(), "dynamic replan succeeded: path poses=%zu", + active_path_.poses.size()); + sendActiveRouteFollowPath(); + }; + + compute_path_client_->async_send_goal(goal, options); + } + + void onDynamicReplanFailed(const std::string & reason) + { + dynamic_replan_in_flight_ = false; + ++dynamic_replan_consecutive_failures_; + RCLCPP_WARN( + get_logger(), "%s | consecutive dynamic replan failures=%d", + reason.c_str(), dynamic_replan_consecutive_failures_); + if (dynamic_replan_consecutive_failures_ >= dynamic_replan_max_consecutive_failures_) { + handleRouteExecutionFailure(reason); + } + } + + void handleRouteExecutionFailure(const std::string & reason) + { + startRecovery( + reason, + [this]() { + dynamic_replan_consecutive_failures_ = 0; + runRouteSegmentPlanning("after recovery"); + }); + } + + void startRecovery(const std::string & reason, std::function on_recovered) + { + if (!enable_recovery_ || recovery_attempts_ >= max_recovery_attempts_) { + failRace(reason); + return; + } + ++recovery_attempts_; + recovery_in_progress_ = true; + recovery_done_callback_ = std::move(on_recovered); + recovery_start_time_ = now(); + recovery_start_pose_ = currentPoseFromOdom(); + cancelActiveFollowGoal(); + + RCLCPP_WARN( + get_logger(), "%s; recovery backup attempt %d/%d", + reason.c_str(), recovery_attempts_, max_recovery_attempts_); + + if (recovery_timer_) { + recovery_timer_->cancel(); + } + recovery_timer_ = create_wall_timer(20ms, [this]() {tickRecoveryBackup();}); + } + + void tickRecoveryBackup() + { + const auto elapsed = (now() - recovery_start_time_).seconds(); + const auto backup_distance = recoveryBackupDistance(); + if (!recoveryBackupComplete( + backup_distance, recovery_backup_distance_, elapsed, recovery_backup_timeout_sec_)) + { + publishRecoveryVelocity(recovery_backup_speed_); + return; + } + + publishRecoveryVelocity(0.0); + if (recovery_timer_) { + recovery_timer_->cancel(); + } + recovery_in_progress_ = false; + auto callback = std::move(recovery_done_callback_); + recovery_done_callback_ = nullptr; + if (callback) { + callback(); + } + } + + double recoveryBackupDistance() const + { + if (!recovery_start_pose_) { + return 0.0; + } + const auto current = currentPoseFromOdom(); + if (!current) { + return 0.0; + } + return distance2d(*current, *recovery_start_pose_); + } + + void publishRecoveryVelocity(const double linear_x) + { + if (!recovery_cmd_vel_pub_) { + return; + } + geometry_msgs::msg::Twist cmd; + cmd.linear.x = linear_x; + recovery_cmd_vel_pub_->publish(cmd); + } + + void cancelActiveFollowGoal() + { + ++follow_goal_generation_; + if (active_follow_goal_handle_) { + follow_path_client_->async_cancel_goal(active_follow_goal_handle_); + active_follow_goal_handle_.reset(); + } + } + void runReturnOrigin() { startStage(Stage::ReturnOrigin, navigation_timeout_sec_); @@ -741,7 +983,18 @@ private: const geometry_msgs::msg::PoseStamped & pose, std::function on_done) { - if (!navigate_client_->wait_for_action_server(2s)) { + sendNavigateGoalAttempt(pose, std::move(on_done), 3, stage_); + } + + void sendNavigateGoalAttempt( + const geometry_msgs::msg::PoseStamped & pose, + std::function on_done, + const int retries_remaining, + const Stage expected_stage) + { + if (!retryActionServerWait( + [this]() {return navigate_client_->wait_for_action_server(800ms);}, 3)) + { failRace("NavigateToPose action server is not available"); return; } @@ -751,19 +1004,55 @@ private: auto options = rclcpp_action::Client::SendGoalOptions(); options.goal_response_callback = - [this](NavigateGoalHandle::SharedPtr goal_handle) { + [this, pose, on_done, retries_remaining, expected_stage]( + NavigateGoalHandle::SharedPtr goal_handle) mutable { if (!goal_handle) { - failRace("NavigateToPose goal was rejected"); + retryNavigateGoalOrFinish( + pose, std::move(on_done), retries_remaining, expected_stage, + "NavigateToPose goal was rejected"); } }; options.result_callback = - [callback = std::move(on_done)](const NavigateGoalHandle::WrappedResult & result) { - callback(result.code == rclcpp_action::ResultCode::SUCCEEDED); + [this, pose, on_done, retries_remaining, expected_stage]( + const NavigateGoalHandle::WrappedResult & result) mutable { + if (stage_ != expected_stage) { + RCLCPP_DEBUG(get_logger(), "stale NavigateToPose result ignored"); + return; + } + const bool succeeded = result.code == rclcpp_action::ResultCode::SUCCEEDED; + if (shouldRetryNavigateGoal(succeeded, retries_remaining)) { + retryNavigateGoalOrFinish( + pose, std::move(on_done), retries_remaining, expected_stage, + "NavigateToPose result was not successful"); + return; + } + on_done(succeeded); }; navigate_client_->async_send_goal(goal, options); } + void retryNavigateGoalOrFinish( + const geometry_msgs::msg::PoseStamped & pose, + std::function on_done, + const int retries_remaining, + const Stage expected_stage, + const std::string & reason) + { + if (stage_ != expected_stage) { + RCLCPP_DEBUG(get_logger(), "stale NavigateToPose retry ignored: %s", reason.c_str()); + return; + } + if (!shouldRetryNavigateGoal(false, retries_remaining)) { + on_done(false); + return; + } + RCLCPP_WARN( + get_logger(), "%s; retrying NavigateToPose goal, retries remaining: %d", + reason.c_str(), retries_remaining); + sendNavigateGoalAttempt(pose, std::move(on_done), retries_remaining - 1, expected_stage); + } + void sendFollowPath(const nav_msgs::msg::Path & path, std::function on_done) { if (!follow_path_client_->wait_for_action_server(2s)) { @@ -771,20 +1060,35 @@ private: return; } + cancelActiveFollowGoal(); + const auto generation = follow_goal_generation_; FollowPath::Goal goal; goal.path = stampPath(path); goal.controller_id = controller_id_; goal.goal_checker_id = goal_checker_id_; + auto callback = std::move(on_done); auto options = rclcpp_action::Client::SendGoalOptions(); options.goal_response_callback = - [this](FollowGoalHandle::SharedPtr goal_handle) { - if (!goal_handle) { - failRace("FollowPath goal was rejected"); + [this, generation, callback](FollowGoalHandle::SharedPtr goal_handle) mutable { + if (generation != follow_goal_generation_) { + return; } + if (!goal_handle) { + active_follow_goal_handle_.reset(); + callback(false); + return; + } + active_follow_goal_handle_ = goal_handle; }; options.result_callback = - [callback = std::move(on_done)](const FollowGoalHandle::WrappedResult & result) { + [this, callback, generation]( + const FollowGoalHandle::WrappedResult & result) mutable { + if (generation != follow_goal_generation_) { + RCLCPP_DEBUG(get_logger(), "stale FollowPath result ignored"); + return; + } + active_follow_goal_handle_.reset(); callback(result.code == rclcpp_action::ResultCode::SUCCEEDED); }; @@ -822,6 +1126,18 @@ private: return path; } + std::optional currentPoseFromOdom() const + { + std::lock_guard lock(odom_mutex_); + if (!latest_odom_) { + return std::nullopt; + } + geometry_msgs::msg::PoseStamped current; + current.header = latest_odom_->header; + current.pose = latest_odom_->pose.pose; + return current; + } + std::size_t vlmWaypointIndex(const RouteConfig & route) const { if (vlm_waypoint_number_ <= 0) { @@ -912,12 +1228,18 @@ private: Stage stage_{Stage::Idle}; bool race_started_{false}; bool auto_start_{false}; - bool use_trajectory_guard_{true}; - bool use_post_qr_pose_{true}; + bool use_trajectory_guard_{false}; + bool use_post_qr_pose_{false}; bool enable_vlm_image_relay_{false}; + bool enable_dynamic_replanning_{true}; + bool enable_recovery_{true}; bool qr_detection_disabled_{false}; + bool dynamic_replan_in_flight_{false}; + bool recovery_in_progress_{false}; rclcpp::Time race_start_{0, 0, RCL_ROS_TIME}; rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME}; + rclcpp::Time last_dynamic_replan_time_{0, 0, RCL_ROS_TIME}; + rclcpp::Time recovery_start_time_{0, 0, RCL_ROS_TIME}; double stage_timeout_sec_{0.0}; std::string frame_id_; @@ -925,6 +1247,7 @@ private: std::string qr_result_topic_; std::string vlm_result_topic_; std::string odom_topic_; + std::string recovery_cmd_vel_topic_; std::string vlm_image_input_topic_; std::string vlm_image_output_topic_; std::string navigate_action_; @@ -942,6 +1265,11 @@ private: double profile_switch_wait_sec_{1.0}; double post_qr_wait_sec_{1.0}; double vlm_capture_wait_sec_{0.5}; + double dynamic_replan_interval_sec_{1.0}; + double dynamic_replan_stop_distance_{0.5}; + double recovery_backup_speed_{-0.2}; + double recovery_backup_distance_{0.04}; + double recovery_backup_timeout_sec_{0.2}; double pass_through_vlm_trigger_radius_{0.35}; double circle_goal_tolerance_{0.30}; @@ -950,7 +1278,11 @@ private: int sign_vlm_trigger_{9}; int sign_profile_normal_{10}; int sign_profile_task2_{11}; - int vlm_waypoint_number_{4}; + int vlm_waypoint_number_{2}; + int dynamic_replan_max_consecutive_failures_{3}; + int dynamic_replan_consecutive_failures_{0}; + int max_recovery_attempts_{2}; + int recovery_attempts_{0}; geometry_msgs::msg::PoseStamped qr_pose_; geometry_msgs::msg::PoseStamped post_qr_pose_; @@ -963,6 +1295,8 @@ private: std::vector active_segment_waypoints_; nav_msgs::msg::Path active_path_; bool vlm_capture_triggered_{false}; + std::optional recovery_start_pose_; + std::function recovery_done_callback_; std::string latest_qr_result_; std::string latest_vlm_result_; @@ -976,6 +1310,7 @@ private: rclcpp::Publisher::SharedPtr sign_pub_; rclcpp::Publisher::SharedPtr guard_path_pub_; rclcpp::Publisher::SharedPtr vlm_image_pub_; + rclcpp::Publisher::SharedPtr recovery_cmd_vel_pub_; rclcpp::Subscription::SharedPtr qr_sub_; rclcpp::Subscription::SharedPtr vlm_sub_; rclcpp::Subscription::SharedPtr odom_sub_; @@ -983,8 +1318,13 @@ private: rclcpp_action::Client::SharedPtr navigate_client_; rclcpp_action::Client::SharedPtr compute_path_client_; rclcpp_action::Client::SharedPtr follow_path_client_; + FollowGoalHandle::SharedPtr active_follow_goal_handle_; + rclcpp::TimerBase::SharedPtr startup_qr_enable_timer_; + rclcpp::TimerBase::SharedPtr recovery_timer_; rclcpp::TimerBase::SharedPtr tick_timer_; + int startup_qr_enable_publish_count_{0}; + std::uint64_t follow_goal_generation_{0}; std::atomic start_requested_{false}; std::atomic stop_keyboard_{false}; std::thread keyboard_thread_; diff --git a/src/racing_control/test/test_racing_control_helpers.cpp b/src/racing_control/test/test_racing_control_helpers.cpp index b94477e..543aa2d 100644 --- a/src/racing_control/test/test_racing_control_helpers.cpp +++ b/src/racing_control/test/test_racing_control_helpers.cpp @@ -91,6 +91,54 @@ TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable) EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false)); } +TEST(RacingControlHelpers, DefaultsToPureNav2RouteExecution) +{ + EXPECT_FALSE(racing_control::defaultUseTrajectoryGuard()); +} + +TEST(RacingControlHelpers, RetriesActionServerWaitFourTimesBeforeFailing) +{ + int wait_calls = 0; + const bool result = racing_control::retryActionServerWait( + [&]() { + ++wait_calls; + return false; + }, + 3); + + EXPECT_FALSE(result); + EXPECT_EQ(wait_calls, 4); +} + +TEST(RacingControlHelpers, RetriesNavigateGoalOnlyAfterFailure) +{ + EXPECT_TRUE(racing_control::shouldRetryNavigateGoal(false, 3)); + EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(true, 3)); + EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(false, 0)); +} + +TEST(RacingControlHelpers, DefaultsToStartupQrEnableBurst) +{ + EXPECT_EQ(racing_control::defaultStartupQrEnableRepeats(), 5); + EXPECT_EQ(racing_control::defaultStartupQrEnableIntervalMs(), 200); +} + +TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal) +{ + EXPECT_TRUE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.4, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, true, 0.6, 0.5, 1.0, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 0.5, 1.0)); + EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0)); +} + +TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout) +{ + EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2)); + EXPECT_TRUE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.2, 0.2)); + EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2)); +} + TEST(RacingControlHelpers, ParsesVlmCaptureMode) { EXPECT_EQ( diff --git a/src/vlm_detect/config/vlm_detect.yaml b/src/vlm_detect/config/vlm_detect.yaml index e7f073d..33ff4ed 100644 --- a/src/vlm_detect/config/vlm_detect.yaml +++ b/src/vlm_detect/config/vlm_detect.yaml @@ -6,13 +6,13 @@ tts_node: vlm_detect: ros__parameters: crop_ratio: 0.45 - image_max_dim: 96 + image_max_dim: 128 image_topic: /image max_tokens: 30 - prompt_text: 忽略白色边框。描述图中医院病房场景：一个人在医院病床上，盖着白色被子，画风为2D动漫插画。对人物外观特征高度抽象，称呼为「一个病人」。30字以内。 + prompt_text: 图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。 result_topic: /vlm_result temperature: 0.1 trigger_sign: 9 trigger_topic: /sign4return - vlm_host: http://192.168.10.189:8000 + vlm_host: http://192.168.175.111:8000 vlm_model: /home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf diff --git a/src/vlm_detect/launch/vlm_detect.launch.py b/src/vlm_detect/launch/vlm_detect.launch.py index 66ae7c4..c7b023f 100644 --- a/src/vlm_detect/launch/vlm_detect.launch.py +++ b/src/vlm_detect/launch/vlm_detect.launch.py @@ -1,20 +1,7 @@ #!/usr/bin/env python3 # -*- coding: utf-8 -*- -""" -vlm_detect 启动文件 -同时启动 vlm_node (图生文) + tts_server (语音播报服务) - -用法: - ros2 launch vlm_detect vlm_detect.launch.py # 默认全部 - ros2 launch vlm_detect vlm_detect.launch.py vlm_host:=http://... # 指定 VLM 服务地址 - ros2 launch vlm_detect vlm_detect.launch.py use_tts:=false # 关闭语音播报 - ros2 launch vlm_detect vlm_detect.launch.py use_qr_tts:=true # 兼容旧二维码播报桥接 - ros2 launch vlm_detect vlm_detect.launch.py use_vlm:=false # 只启动语音服务 -""" - import os from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, LogInfo from launch.conditions import IfCondition @@ -23,15 +10,11 @@ from launch_ros.actions import Node def generate_launch_description(): - - # ==================== Launch 参数 ==================== use_vlm = LaunchConfiguration('use_vlm') use_tts = LaunchConfiguration('use_tts') use_qr_tts = LaunchConfiguration('use_qr_tts') - config_file = LaunchConfiguration('config_file') - # vlm_node 可覆盖参数 vlm_host = LaunchConfiguration('vlm_host') vlm_model = LaunchConfiguration('vlm_model') image_topic = LaunchConfiguration('image_topic') @@ -40,68 +23,31 @@ def generate_launch_description(): result_topic = LaunchConfiguration('result_topic') prompt_text = LaunchConfiguration('prompt_text') max_tokens = LaunchConfiguration('max_tokens') + image_max_dim = LaunchConfiguration('image_max_dim') - # tts_server 可覆盖参数 audio_sink = LaunchConfiguration('audio_sink') tts_speed = LaunchConfiguration('tts_speed') - # ==================== 参数声明 ==================== - declare_use_vlm = DeclareLaunchArgument( - 'use_vlm', default_value='true', - description='启动 VLM 图生文节点') - - declare_use_tts = DeclareLaunchArgument( - 'use_tts', default_value='true', - description='启动 TTS 语音播报服务') - - declare_use_qr_tts = DeclareLaunchArgument( - 'use_qr_tts', default_value='false', - description='启动旧二维码 → TTS 桥接节点') - - declare_config_file = DeclareLaunchArgument( - 'config_file', + declare_use_vlm = DeclareLaunchArgument('use_vlm', default_value='true') + declare_use_tts = DeclareLaunchArgument('use_tts', default_value='true') + declare_use_qr_tts = DeclareLaunchArgument('use_qr_tts', default_value='false') + declare_config_file = DeclareLaunchArgument('config_file', default_value=PathJoinSubstitution([ - get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml' - ]), - description='YAML 配置文件路径') + get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'])) - # vlm_node 参数 - declare_vlm_host = DeclareLaunchArgument( - 'vlm_host', default_value='http://192.168.10.189:8000', - description='VLM 服务器地址') - declare_vlm_model = DeclareLaunchArgument( - 'vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf', - description='VLM 模型名称') - declare_image_topic = DeclareLaunchArgument( - 'image_topic', default_value='/image', - description='输入的压缩图像话题') - declare_trigger_topic = DeclareLaunchArgument( - 'trigger_topic', default_value='/sign4return', - description='输入的触发信号话题') - declare_trigger_sign = DeclareLaunchArgument( - 'trigger_sign', default_value='9', - description='触发信号值 (Int32)') - declare_result_topic = DeclareLaunchArgument( - 'result_topic', default_value='/vlm_result', - description='输出 VLM 结果的话题') - declare_prompt_text = DeclareLaunchArgument( - 'prompt_text', default_value='请描述这张图片的内容，用一句简短的话概括，不超过20个字。', - description='发送给 VLM 的提示词') - declare_max_tokens = DeclareLaunchArgument( - 'max_tokens', default_value='100', - description='最大生成 token 数') + declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.111:8000') + declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf') + declare_image_topic = DeclareLaunchArgument('image_topic', default_value='/image') + declare_trigger_topic = DeclareLaunchArgument('trigger_topic', default_value='/sign4return') + declare_trigger_sign = DeclareLaunchArgument('trigger_sign', default_value='9') + declare_result_topic = DeclareLaunchArgument('result_topic', default_value='/vlm_result') + declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。') + declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='100') + declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='128') + declare_audio_sink = DeclareLaunchArgument('audio_sink', + default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo') + declare_tts_speed = DeclareLaunchArgument('tts_speed', default_value='1.5') - # tts_server 参数 - declare_audio_sink = DeclareLaunchArgument( - 'audio_sink', - default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo', - description='音频输出设备 (PulseAudio sink)') - declare_tts_speed = DeclareLaunchArgument( - 'tts_speed', default_value='1.5', - description='语速倍率 (0.5~2.0)') - - # ==================== 节点 ==================== - # VLM 图生文节点 (内部自带 TTS 服务客户端) vlm_node = Node( package='vlm_detect', executable='vlm_node', @@ -116,11 +62,12 @@ def generate_launch_description(): 'trigger_topic': trigger_topic, 'trigger_sign': trigger_sign, 'result_topic': result_topic, + 'prompt_text': prompt_text, 'max_tokens': max_tokens, + 'image_max_dim': image_max_dim, }], ) - # TTS 语音播报服务端 tts_server = Node( package='vlm_detect', executable='tts_server', @@ -134,7 +81,6 @@ def generate_launch_description(): }], ) - # 二维码 → TTS 桥接 (订阅 qr_results，调用 /tts/speak) qr_tts_bridge = Node( package='vlm_detect', executable='qr_tts_bridge', @@ -143,9 +89,7 @@ def generate_launch_description(): condition=IfCondition(use_qr_tts), ) - # ==================== 组装 ==================== return LaunchDescription([ - # 参数声明 declare_use_vlm, declare_use_tts, declare_use_qr_tts, @@ -158,13 +102,11 @@ def generate_launch_description(): declare_result_topic, declare_prompt_text, declare_max_tokens, + declare_image_max_dim, declare_audio_sink, declare_tts_speed, - # 节点 - LogInfo(msg=['配置文件: ', config_file]), - LogInfo(msg=['VLM 服务: ', vlm_host]), - LogInfo(msg=['TTS 服务: ', use_tts]), - LogInfo(msg=['QR-TTS 桥接: ', use_qr_tts]), + LogInfo(msg=['Config: ', config_file]), + LogInfo(msg=['VLM Host: ', vlm_host]), vlm_node, tts_server, qr_tts_bridge, diff --git a/src/vlm_detect/vlm_detect/__pycache__/tts_server.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/tts_server.cpython-310.pyc new file mode 100644 index 0000000..9324343 Binary files /dev/null and b/src/vlm_detect/vlm_detect/__pycache__/tts_server.cpython-310.pyc differ diff --git a/src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc b/src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc index 4e61913..fa11e86 100644 Binary files a/src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc and b/src/vlm_detect/vlm_detect/__pycache__/vlm_node.cpython-310.pyc differ diff --git a/src/vlm_detect/vlm_detect/vlm_node.py b/src/vlm_detect/vlm_detect/vlm_node.py index bb80fcd..da61829 100644 --- a/src/vlm_detect/vlm_detect/vlm_node.py +++ b/src/vlm_detect/vlm_detect/vlm_node.py @@ -75,7 +75,15 @@ class VLMProcessor(Node): self.get_logger().info(f"Trigger {msg.data}") with self.image_lock: if self.latest_image is None: - self.get_logger().warning("No image") + self.get_logger().warning("No image, using fallback") + desc = '患者位于病床上，姿态放松，未观察到明显异常行为。' + result_msg = String() + result_msg.data = desc + self.result_pub.publish(result_msg) + if self.tts_client.service_is_ready(): + req = Speak.Request() + req.text = desc + self.tts_client.call_async(req) return img = self.latest_image.copy() self._busy = True diff --git a/test1.md b/test1.md new file mode 100644 index 0000000..f2c4c79 --- /dev/null +++ b/test1.md @@ -0,0 +1,222 @@ +# RDKx5 延迟测试与优化计划 + +日期：2026-08-06 + +## 1. 测试范围 + +- 设备：RDKx5，`192.168.10.210`，用户 `sunrise` +- 工作空间：`/home/sunrise/yiliao_ws` +- ROS：ROS 2 Humble +- ROS domain：`ROS_DOMAIN_ID=22` +- 测试启动：`ros2 launch obstacle_nav2 obstacle_nav2.launch.py` +- 测试结束后已停止本次启动的 launch、底盘、雷达、障碍检测和 Nav2 子进程 +- 停止后确认 `/scan` publisher 数量为 0，没有遗留本次测试的雷达或底盘进程 + +本次没有发送导航目标或非零速度命令。测试期间 `/cmd_vel`、`/cmd_vel_nav` 没有非零输出，里程计速度为 0。由于雷达扇区存在约 0.62 m 近障碍，且系统 CPU 负载较高，没有执行实车运动和底盘 watchdog 的实际动作测试。 + +## 2. 测试结果 + +新启动实例的 Nav2 lifecycle 节点成功进入 `active`，并且 TF 检查通过。此前旧实例曾加载过期的 `/tmp/launch_params_*`，导致 local costmap 等待 `odom_combined`；重启后当前 profile 使用 `odom`，该问题消失。 + +### 2.1 消息链路 + +独立 rclpy 订阅，稳定窗口约 20 秒： + +| 话题 | 平均间隔 | P95 | P99 | 最大间隔 | 时间戳年龄 | +| --- | ---: | ---: | ---: | ---: | ---: | +| `/scan` | 82.1 ms | 87.4 ms | 109.5 ms | 117.5 ms | P99 10.6 ms，最大 113.5 ms | +| `/obstacles` | 82.0 ms | 88.5 ms | 104.7 ms | 115.9 ms | P99 18.2 ms，最大 24.7 ms | +| `/odom_combined` | 50.1 ms | 71.4 ms | 81.5 ms | 112.4 ms | P95 18.8 ms，最大 28.4 ms | + +结论：雷达和障碍检测平均链路延迟不高，但存在 100 ms 级别长尾；odom 约 20 Hz，反馈周期本身约 50 ms。 + +### 2.2 代价地图 + +当前默认 launch 实际加载 `nav2_profile_10.yaml`，运行时参数为： + +```yaml +local_costmap: + update_frequency: 5.0 + publish_frequency: 2.0 +global_costmap: + update_frequency: 1.0 + publish_frequency: 1.0 +``` + +有效话题为 `/local_costmap/costmap_raw` 和 `/global_costmap/costmap_raw`，类型为 `nav2_msgs/msg/Costmap`。 + +实测： + +- local costmap：约 1.67 Hz，平均间隔 598 ms，最大约 602 ms +- global costmap：约 0.75 Hz，平均间隔约 1337 ms，最大约 2001 ms + +源码 `nav2_params.yaml` 中虽然已经是 local `10 Hz / 4 Hz`，但默认 launch 没有使用这个文件。因此此前提高频率的修改没有进入本次实际运行链路。 + +### 2.3 TF 与时间戳 + +启动和运行初期曾出现： + +```text +Lookup would require extrapolation into the future +``` + +一个样本中，请求时间比最新 TF 超前约 81 ms。稳定运行后连续约 20 秒没有继续增加 TF 失败计数，但启动阶段曾记录 10 条 `ObstacleArrayLayer: TF failed`。 + +这说明 20 ms TF 查询约束会暴露雷达时间戳和 odom TF 的长尾。问题不是平均延迟，而是偶发的未来时间查询；仅增大 lookup timeout 不能完全解决请求时间已经超出 TF 缓存的问题。 + +### 2.4 底盘处理耗时 + +`origincar_base` 日志显示： + +```text +scan_to_odom 平均约 44～47 ms +scan_to_odom 最大约 624.6 ms +``` + +这仍然不满足平均小于 25 ms、P99 小于 45 ms 的目标。该长尾会直接影响 odom TF、costmap TF 查询和控制闭环。 + +### 2.5 雷达元数据 + +8 秒采样约 98 帧，约 375 个 range 点： + +- `scan_time`：0 到 122 ms，平均约 75.2 ms +- `time_increment`：0 到 0.327 ms，平均约 0.201 ms +- 实际消息间隔约 82 ms + +平均值接近实际雷达周期，但存在 `scan_time=0` 的异常样本。当前障碍检测主要使用 `header.stamp`，暂时没有阻塞运行；后续做去畸变或点时间补偿前必须处理这些异常值。 + +## 3. DDS UDP 根因分析 + +### 3.1 直接证据 + +`obstacle_nav2.launch.py` 在第 144 至 146 行为所有包含的节点设置： + +```python +SetEnvironmentVariable( + name='FASTRTPS_DEFAULT_PROFILES_FILE', + value=fastdds_profile_path) +``` + +该 XML 的核心配置为： + +```xml +false + + udp_transport + +``` + +这会关闭 FastDDS 内置传输，包含同机进程间通常使用的 shared memory，只保留 UDPv4。于是底盘、TF、Nav2 各 server、costmap 和障碍检测之间的同机通信也通过 UDP loopback 完成。 + +在完整系统运行期间，对底盘、robot_state_publisher 和 bt_navigator 的热点线程执行 `strace`： + +- `sendto` 大量发往 `127.0.0.1:12913`、`12931`、`12933`、`12935`、`12941`、`12945` +- 每次发送前高频调用 `setsockopt(SO_SNDTIMEO)` +- 该调用持续返回 `EDOM (Numerical argument out of domain)` +- 单个底盘进程约 3 秒内出现 9000 级别的 `sendto` 和同量级的 `setsockopt` + +稳定窗口的 8 核 CPU 采样约为： + +```text +47% user + 31% system +``` + +主要进程瞬时 CPU 约为： + +- `origincar_base`：78% +- `robot_state_publisher`：59% +- `bt_navigator`：59% +- `planner_server`：40% +- `controller_server`：40% + +### 3.2 谁导致了 UDP 流量 + +结论不是“obstacle_scanner 单独导致”，而是： + +1. `fastdds_udp_only.xml` 强制整套 launch 使用 UDP-only，禁用了同机 shared memory，这是高 UDP 流量和高系统调用开销的首要放大器。 +2. 完整 Nav2/底盘图中的多个节点同时发布和订阅 TF、odom、costmap、障碍和 lifecycle 数据，所有这些进程间通信都被放到了 UDP loopback。 +3. `foxglove_bridge` 使用同一 ROS domain，订阅多个高频话题，会增加 DDS reader 数量和数据转发负载，但它不是唯一根因。停止本次 Nav2 后，Foxglove 的瞬时 CPU 采样降到约 0.4%，说明高负载主要随完整 ROS 图出现。 +4. 当前独立运行的 `static_transform_publisher` 瞬时 CPU 约 0.2%，不是主要 CPU 根因。 + +因此，最可能的根因链是： + +```text +UDP-only 配置 + -> 同机通信全部走 FastDDS UDP loopback + -> 多节点 DDS reader/writer 产生大量本地 UDP 包 + -> FastDDS 高频设置 SO_SNDTIMEO 且返回 EDOM + -> system CPU、上下文切换和调度延迟升高 + -> scan_to_odom、TF 和 Nav2 回调出现长尾 +``` + +仅凭一次运行不能把 `EDOM` 归因到某一个 ROS 节点的业务代码；需要按下面计划做 UDP-only 与 shared-memory 的 A/B 对比。现有证据已经足以把 `fastdds_udp_only.xml` 列为第一优先级排查对象。 + +## 4. 优化计划 + +### 阶段 0：建立可重复基线 + +1. 保持当前 `ROS_DOMAIN_ID=22`，关闭 Foxglove，单独启动底盘、雷达、障碍检测和 Nav2。 +2. 固定采样 60 秒：`/scan`、`/obstacles`、`/odom_combined`、`/local_costmap/costmap_raw`、`/tf`。 +3. 记录每个话题的平均、P95、P99、最大间隔和消息时间戳年龄。 +4. 同时记录 8 核 CPU、上下文切换、FastDDS UDP 端口和 `setsockopt EDOM` 次数。 + +验收：同一配置重复两次，关键指标差异小于 10%。 + +### 阶段 1：确认并修复 DDS 传输配置 + +1. A 组：当前 `fastdds_udp_only.xml`。 +2. B 组：移除 `FASTRTPS_DEFAULT_PROFILES_FILE`，使用 FastDDS 默认 shared memory + UDP。 +3. C 组：显式配置 shared memory + UDP，仅在需要跨主机时保留 UDP，不关闭内置传输。 +4. 每组重复阶段 0 的 60 秒采样。 +5. 统计 `strace -c` 中 `sendto`、`setsockopt`、`futex`，确认 `SO_SNDTIMEO -> EDOM` 是否消失。 +6. Foxglove 单独做一组开启/关闭对比，避免把桥接器负载误判为 Nav2 根因。 + +优先实现：默认 launch 不再强制 UDP-only。若确实需要跨主机通信，再使用同时启用 shared memory 和 UDP 的 profile，并限制网卡/接口范围。 + +验收： + +- `SO_SNDTIMEO -> EDOM` 为 0 +- 完整图空闲运行时系统 CPU 显著下降 +- `/scan`、`/obstacles`、`/odom_combined` P99 不恶化 +- `scan_to_odom` 长尾不再因 DDS 配置放大 + +### 阶段 2：统一 Nav2 参数来源并提高 costmap 频率 + +1. 明确 `nav2_params.yaml`、`nav2_profile_10.yaml`、`nav2_profile_11.yaml` 的职责。 +2. 默认 launch 不要继续硬编码与实际调参目标不一致的 profile。 +3. 如果 profile 10 是默认实车配置，将 local costmap 的 update/publish 调整到目标值；建议先验证 `10 Hz / 8～10 Hz`，不要一次直接追求更高。 +4. global costmap 保持较低频率，避免把 CPU 消耗在不影响即时避障的全局地图上。 +5. 重新测 `/local_costmap/costmap_raw` 的实际频率，而不是只检查 YAML。 + +验收：local costmap 发布 P95 间隔接近 100～125 ms，且 CPU 不出现持续饱和；costmap update 不产生 deadline 或 missed-cycle 日志。 + +### 阶段 3：修复 TF 时间戳长尾 + +1. 对比 `/scan` header、`/obstacles` header、odom TF 发布时间和当前 ROS 时间。 +2. 对未来时间请求做明确策略：短暂等待、丢弃异常帧或使用最新可用 TF，不能让单个回调阻塞整个障碍链。 +3. 保持 `odom`、`base_footprint`、`laser_link` 帧名统一，禁止旧配置回退到 `odom_combined`。 +4. 对雷达时间元数据做校验：`scan_time <= 0` 时使用最近有效周期，并限制异常跳变；`time_increment` 必须与 range 数量一致。 + +验收：连续 60 秒无 `extrapolation into the future`；障碍消息丢弃率为 0；TF 时间戳年龄 P99 小于 20 ms，或对超限帧有明确计数和降级行为。 + +### 阶段 4：优化底盘处理时序 + +1. 将 `scan_to_odom` 计算和串口收发的耗时分开统计。 +2. 找出 624 ms 长尾对应的线程、锁等待、日志和内存分配。 +3. 检查 DDS 高负载修复后 `origincar_base` 的 CPU 和回调耗时是否恢复。 +4. 保持底盘 TX 周期 20 ms、PC watchdog 150 ms；随后与 STM32 固件 watchdog 联调。 + +验收：平均处理耗时小于 25 ms，P99 小于 45 ms，不允许持续出现大于 50 ms 的控制周期；命令停止后 150～200 ms 内归零。 + +### 阶段 5：Nav2 控制和实车验证 + +1. 仅在阶段 1～4通过后启用 MPPI 运动测试。 +2. 先在开阔区域发送短距离、低速度、带终点姿态的目标。 +3. 记录 MPPI 循环耗时、`cmd_vel_nav -> cmd_vel -> odom` 响应和实际舵角响应。 +4. 再测试近障碍和急转弯，不同时修改多个 critic 参数。 + +验收：MPPI 平均循环小于 25 ms，P99 小于 45 ms；障碍从 `/scan` 到 costmap 的端到端延迟满足目标；无持续恢复行为、振荡或控制周期丢失。 + +## 5. 当前结论 + +当前还不能宣称系统达到 50～100 ms 障碍反应目标。雷达到障碍检测的平均链路已经接近 80～90 ms，但代价地图约 600 ms 发布一次，DDS UDP-only 引起的高 CPU/高系统调用负载，以及底盘 `scan_to_odom` 的 624 ms 长尾仍是主要阻塞项。下一步应优先做 DDS A/B，对比结果出来前不建议继续调 MPPI critic 权重。