From 07598f4b11d7bbea797cf703469634701c3344a9 Mon Sep 17 00:00:00 2001 From: Orange <2314753575@qq.com> Date: Sun, 9 Aug 2026 21:02:42 +0800 Subject: [PATCH] 222 --- .gitignore | 2 + AGENTS.md | 237 ++++++++++ .../params/lidar_uart_ros2/lsn10.yaml | 2 +- .../x5_udp_cmd_bridge.py | 65 +++ src/gc/项目总结_yiliao_ws.md | 429 ++++++++++++++++++ src/map/nav2_costmap_binary.png | Bin 20611 -> 8839 bytes src/navigation/obstacle_nav2/CMakeLists.txt | 7 + .../nav_through_poses_ackermann.xml | 41 ++ .../behavior_tree/nav_to_pose_ackermann.xml | 46 +- ...se_ackermann.xml.bak_20260807_event_replan | 33 ++ ...e_ackermann.xml.bak_20260807_single_backup | 35 ++ .../config/nav2_profile_10 copy.yaml.20260808 | 340 ++++++++++++++ .../config/nav2_profile_10 copy.yaml.20260809 | 340 ++++++++++++++ .../obstacle_nav2/config/nav2_profile_10.yaml | 76 ++-- ..._10.yaml.bak_20260807_global_inflation_055 | 339 ++++++++++++++ .../obstacle_nav2/config/nav2_profile_11.yaml | 21 +- .../launch/obstacle_nav2.launch.py | 56 ++- ...cle_nav2.launch.py.bak_20260809_through_bt | 191 ++++++++ .../scripts/initial_pose_to_tf.py | 116 +++++ .../scripts/static_map_publisher.py | 92 ++++ .../include/origincar_base/log.hpp | 3 + .../include/origincar_base/origincar_base.h | 1 + .../origincar_base/wall_kalman_filter.hpp | 22 + .../launch/base_serial.launch.py | 2 +- src/origincar_base/src/log.cpp | 10 + src/origincar_base/src/origincar_base.cpp | 7 + src/origincar_base/src/wall_kalman_filter.cpp | 89 ++++ .../test/scan_odom_timing_logger_test.cpp | 10 + .../test/wall_kalman_filter_test.cpp | 66 +++ src/racing_control/AGENTS.md | 7 +- src/racing_control/config/racing_control.yaml | 91 ++-- .../include/racing_control/racing_control.hpp | 54 +++ .../src/racing_control copy.cpp | 8 +- src/racing_control/src/racing_control.cpp | 408 +++++++++++++++-- .../test/test_racing_control_helpers.cpp | 48 ++ src/vlm_detect/config/vlm_detect.yaml | 6 +- src/vlm_detect/launch/vlm_detect.launch.py | 104 +---- .../__pycache__/tts_server.cpython-310.pyc | Bin 0 -> 2946 bytes .../__pycache__/vlm_node.cpython-310.pyc | Bin 4282 -> 5876 bytes src/vlm_detect/vlm_detect/vlm_node.py | 10 +- test1.md | 222 +++++++++ 41 files changed, 3381 insertions(+), 255 deletions(-) create mode 100644 AGENTS.md create mode 100644 src/data_collection_tools/x5_udp_cmd_bridge.py create mode 100644 src/gc/项目总结_yiliao_ws.md create mode 100644 src/navigation/obstacle_nav2/behavior_tree/nav_through_poses_ackermann.xml create mode 100755 src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_event_replan create mode 100755 src/navigation/obstacle_nav2/behavior_tree/nav_to_pose_ackermann.xml.bak_20260807_single_backup create mode 100644 src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260808 create mode 100644 src/navigation/obstacle_nav2/config/nav2_profile_10 copy.yaml.20260809 create mode 100644 src/navigation/obstacle_nav2/config/nav2_profile_10.yaml.bak_20260807_global_inflation_055 create mode 100755 src/navigation/obstacle_nav2/launch/obstacle_nav2.launch.py.bak_20260809_through_bt create mode 100644 src/navigation/obstacle_nav2/scripts/initial_pose_to_tf.py create mode 100644 src/navigation/obstacle_nav2/scripts/static_map_publisher.py create mode 100644 src/vlm_detect/vlm_detect/__pycache__/tts_server.cpython-310.pyc create mode 100644 test1.md 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 3ada2bcf636358257828b38c290e1b6bf4bb6d8c..c414579d88cb7ce0e9a66e33378310c6d16a4662 100644 GIT binary patch literal 8839 zcmc(FeLU0q`~Mq-Qz4x?ozN7=r`8Rpa?==7DK(+oB;}@ZRLYQH?q(ciN~m*ID)$wV zdvZHWk(HYfGR(|c3^Q}PvDx-}>zw+WzUT4#{{H;^v4_{*Ua#xAUf272UDx$|zIOYR zjiu5WwKV_$D4jTNVFv&*^D7@kdGJYVFXImQ=Lf&z&S(HoZd&=sJlL(Q4glML6BdUY zf>XG3)5KrSyX_d3!tQ^h3ljD{@}MXtZ~4LFmtV|ejvwFs%ev?H*EfIKHnT5?+VJ{y zL*pCE@TgRS2Fo|UnV)-pXuuOsqgUn;_fym!t8Ut|wBdnK-mU{S@Wh@DzPHY&$-fi22zCq(7H*1eM>D>uQq#QH*L>aL&uAGelkAM5M#jdB=-Tz zJ!>O89wLlSR{PL%X!MC&DE@f$ucr#@8b^i6>=68kRMOfJPdU2caL9b8?_dvasn2Y* zv;diHh@TSEW8j04NJ9N#Q++IJTAp5Kmb%j%3GE&6+0wrcD0{^+igGZl#;N?CShbXg?JOl?mr-+ueo=>9t zqO0qO(%vBf!#TU>Z)( zj1M`RnN4^~m;cQj2I=RkoEcMS>278!)raY3l^!sS-!(jHachGrHbXP-dGp-X!ssJW zGd)U6J%XvJkaA$o@3DERg%iO^^+Nn6qrB7L`qt!ypw{?Ka+R$Nj9!NY*0U$%AnI1 zaP~-=S2B(cPtLym@mhIV3X_9xoq{B$yBC}hSNxi07G57V1s$k}??^|G8O!aACX$3h zqJ%nR{0O@_2mDaLM7=R;R#|;jGJEcQ;6r4XUnt|5#+gNhH^0@^PN7rR%rZHAGwKWS z6aN8c5{%t`y<)YOC9{r{Uptq|uBBbQ`s6YX^w-Qm)Aao58f0~zw-an((@62tDM|K! zc(oB{nWHHzpK?)^*BKg#xC!kPQ+Rl)%P_O#RV3ifvfi*agWfNrpwlpYt-RBPn`>% zzK3b?&h#~|@j8&M(Vp?L#w)&yFQ%CGMu(xCJj9wtU@#tLN^IIn*yjJvsiYL8CCe{kl^$nI1~P zt~Zq*8m;!&6u|D}1y;FUq$wifmY4E@d+eW@aO?0}eyQ=c}e0o^(Uaq~gdDFt;l6Z|V&taGaO5D6}(y`WDj@iST zgT@g*3&a|l?1z;_NvMug)C9W>Ar3y9kd9b<(sLK0={~ebSJ2F%@0jgRzXInjj&Zde5@r_HDc9nVE4 z#ZbzRa0XfxUh3zUxre@5Fj$fCBzs%{RoZI&?gjel&KrwI6I2eXQP3?1+f-IBmiU_m zZUOw><;$67ZR8Er(O`aKJ(7l4mZivsYj4t<3_;)~V|zSAIX4eB1kR@x?M`AExL^hiOmEC0jIT{-YuY1MXOC3R`_EBi~Rs9M=W2D8Moh1pe zu^ZpM<^m_xh8)Xcy3?*`jscP2%w38Bz>9`S&^xc{B-gi!%zD3kcm1h6vTm@5E=tBe5$^9(uj)f0;IT*=%yiq0x<&;HAfTfrr3 zcYH12E436DR*Ml)uJ4R3z<`n*ch7>N5EF$OfWe-rgWyK{H7vK~-G z7I(@-CToAS=~vENrxoU>?p4*+(RLO`? z#|d+AYAY24V%-gQXI8_?R04tusGLPIZkfCoM_PcS~-7y2_uiNV-3e{7m*@1XXR__1b*I^>s|U>WaM;Byap?r zf4^RRy{amhY`PoV6&e1XlSdScp+{@6qBC;16;TTZ#7$wlS*Q4vU}Z^2j=6AA0K2@nPwP#w z$$DtkJD4T(?TGv41|yfpz9g}smVQuVd?CKj=0&=BQJYA>D{qR5?VIm(%6p(nyme~M zX|xfqqAOZ6KY7GG!C9fEb3l+TT}n5+kWqOKRXANx52uJbt50Z}g%=mC`ze3zl+SY; zIUEdhQvK3s@;ewUQF-lw$tV@6bZUy?3f=sM-n(lK430A0JqH-Cbuwrzw=~8gJTf zny-%9Zy;s6;gSp=xz)Vp52oXmCAs5C5$+yIhQI2HO%vJFWBuc)n9cXL`0yA~iJ)j` zJu?str{&k0%ZHZda}ZoX@=@>5aXM)slwLVTt0;F@D!f`NPNg*%2LLWcO=;+amC9`! zG5cpkGA7c64mu@bu~D-*F=GB%Y#+T(J7{;DM;*U}3h~zr8&7+iW!iSVh-Agac&W^m zBcxfTpU|x){H|66HFGwI7GzYwDX&C{@>@sw;A^#lp01$bw`axM@13484s9_e2Gk7+ z1^zxuZo-8Qu>{Q)473vG#U0|1G|_^dScEW?9@(7KWr6@-7?@1A|2LVMlNQ8f!1OsV zWdaQ`D@pomO+ndi33z(|s9!5Mq5$~(_h0#P*l@I}=!4AJm(B{tK=D6?*+RLKMi>+Z zpp1OU=HJQ%ScOb@8i3)y)YQ=u&CGUe1s?v!2TR3@?%!98$S-t_w0ceuaw>hN;?ua6 z3d1L21kx8ww{4-{)tZ;GbO{=G`0^#VC~E`wuPEpA3i*b;*LP%d?9FnVi`v_(*>ecY zdO-JoR6dQ33Jq(kxn`{C0UZMu^5+VwAu#>RH`rRHJWuGNM5wTz%N0A6DF*--j;z$n zl@gPYV6LZQcdbHY(o*5VIn1ErE>)Nvcu2vv*T7{R5J`19AJJykNvnwW3orpTzt}zq z)@y_C9{|7He)&+0-h2vLbK=d`NFJD{uWx2vDpQ(ISg#O?lr_C|L17r)m>1W3Z+n@_ ze5~Io4Is{lX3lt(Z}&>a66!U*ywI|$A)H_*Oj`=AOT+46D?q0uy&TVW1LvzHoLn_r z1A#yaU2nz}tT<4Kbo0r%b48c0-5h`8DC|0NwjLdPXzfeQRl*1JRE9MOb-28^q10r` z4Z3_!LC%JeG3n1vTi4M}`y|{XHfJJ}DVfuNp2>4=W+wFGey>1bboXd?Vz}Z#Vg}7- z+-6B#+KFMzR@o3E;ni(&>I<2?%Y*ApdWwIpXLlY8^^S%_B=|_sh~d>~^yyaxefrZx zH_V;xOjv+-vW9O5+dG=hu|)~UqaiP%G5M^n3X}!KQJ)Vw8OE;DOS2s5d(_V#*X?^I z%1nwqo~VvayNbdztG8w5OvYNVcor=oSFZL#qw{cO8r}-5*5yFfs zG?K)^qc%Ch##Bs57$}b3rz@S_^^|P)U~KGQ5)&yQRnlFVA#?)>W~2DiS*tN6)0(N9hTA9ctk4Y zEEv~^FO@!K2xn$O>!#ePQrDtqbcrhhKJU4OLQKA5vIlLn|4)q3ClHPbHdzYlMIzDS zXtM984#{N4CnMaPk^AKJNNCP6lABLv&W>`FI)_`aiOy$K(2Sqnh46e5bN8AgCW^Tg zfhZou9q%% zNS9|BZ`US!W?=T_RiIcnaYs5?Dndw&>?kGIp0xX7ia!Kwp8@*@kUDtXCGG6xkRx{| zYw{?iTKqTHm!s8e)TNPBP8R}FFPWKX9Fq6DI>Z{MFQaeMXT1@&0zu}h4-YFCni2wb z*q$T#Ly)L+e%3a3Fm3-J;vqUA+w0R6gs300`Do#wD$#WO)n&R=a2T&eC#5zmp zOqQ!<6N`5_ytgfwIQ9Fb#b9_Ht-^?TpRZPrAElZiyy3pGez>K^rK2Vh*#5^uSV~16 zLq5I8qRE&-OT*i%xMt45j=fgqURIY%bJ>)A8mkxM8H@2dp-)q1jt}jTGu0AmPPt1) z-1&{VoP|)|;Dye6A7CiNTwEMr9`$W5qA#18ZiGXc)p(5*DC+!2k%(Z2 zEYYpRo?}?4+@ftQ^OmwmV!`5Dy@(IKCaxC*3P+Vf{ZvH?)@jDaJgN%fF-E2~v^L&Z z`o4(z^AeAok@@@7rKa^Bx5d%t$i!jY{#mzcdDxbH`#YfQ<>wyJqZcZoASFwsyrKHr zzF@H(j_8VbHo!k=@9o}gChrE(SUml7UX);h(s;zUPiB?IbI$;x?q;4z(5ZLR2Ar9@ zK|u%&#M5O5*!2S%zvo1l(8!@%(-F8KI7z9>u7!0d^BJn@`aQ(ksu{Qop9LqXMtXSC zQyKP2Qo^%Z`{!pZiGt-1vU@s<{|vZFk`973Nju0WBV~>vzt0=0X}ZCGI#^>A?}p2F z9O_`T-NUua*0+Y?M_^Y%Q8Em%?kb`Gy2{r?BC-9oE9y4*rdwZ-!=`v*ha&LgdDnM@ zWFJL5y*K+hq$11oY`XPc5?^@1`z*O0E8@3gRS!P#fq6MGWUn6qu3kID*5((UdD zc#L#|!|1rn?yX>-blS81xg9cmX?A4b?eHz7#6)mpwLF_dy1Nky8JJ5R8I9RefVg`HHRAy*TpGEIM5Pom~{gAB^{ z#?(Dg0W6Sky=fW2K#!?~8F&|`JhZ!$?e&zf0XJ`n+laT>e?rV~5c4x54x-K)1y~Y0 z`X}Y;=o%HF+)FO)T}JP6#`tR|GO0y*8E6XF@oIWW+X^Hzn0_+cXae6Iqj0jrv`u^0 z7JodHJ`U|c1^1NllCxCD_QXJh{wc=g8?WhfIQx09Zy~@~ech-F6G5ycV zJts5l>vZ3dgcEtYlWtP?*T($>N&%Og3WMrI@B|UOfwwZj`(#rm0yl42daV-S$9+2e zriM3g%k!*Ol+DMQi&PLYY+642^R+9vpxS9Z_k5JHgEc4>L~v*Fu5XqbKKetzPlqz( z%6}Z&bC}Y9&*YTB%EI-$gmC#655RrfT$Fuio?&0E+veJe-BQErD<5>$`Wx1M#kUTq zD#qaHsa7WOGHCwac=@k^y@7x%54z#+hTsd526u=~GPnT!*HlqQ6xi_p7dC_PDOyzt zqSD3sf9d>g))$oRfC@G%<?BHy8a(1ow@ zbC+ivu+sUfy?b#-ZkzKi&^6>@6RTf3bI&26NN6Z(?H?;KSLnJfI8ZkAW#C2Ko~VHz z0y2;>bZ50F%B2^>NsXBH4L=0D>~)+((bW9i6_Z4sprb!>l<&&DcnrSp7$yu81Y!_Zh2R1otZl;ZqI?bOCYU3*ov#a@ra$m3S)@U(rAYRDs9_gCfN9+&VdgGr=L)d zgj~`S2BZ6n^leGF!8;LC$Vx)@95Fn^Uf^oxnt5_TE4#IlxI9N?_srAHhxB+;Y!iV< z(^iv$J5ZFPhpU=oNGhS^KD{YC0;eptTEFek6i(H0;yEuoC&IQkn z#t-$&>RbPqMYu{cjF^trY+B(4mt#((NtfLDzvfwy5|a!+ zUaU5NW;uY|0Z3C+5s9?5k8;<93FU@_>)C^?h>?MG-gv8?&ZlKQ^|FKoku3UBV7;8c z{4*VXeTqhToclS1PavPD`3Zv)U|3?2V5P}`u^Sg0Z|_W&TC}jmz-pmq8g4TuMEixk zn{j6cq(qm6CoZ@kp%1`fB9YW1+7&Y7rL%Fk+lZwNxB#8j6Z>Zc7-KjGA|z3 z-&3LLX}_2M;F4gJpcNm)yjy++qOH##T;TTT=H^m)@#Z#=BZ0>EkYQf*4%;tV6*in+t^^z~fIHf& zY>nJ}gG1ihQydpkmEdHpd*A+c`=&}Y5PSyzRQh@3ft?n4H27Y)=!%^zfS=S@ig@b} z0GoeyJ7ELzi|Ya}4k-wSpH5NtYzF*drVbm4xlfB+Ai(ez5NAGuioi6vKW~54X1^0z zBvG8{1mH&$iZW;*&$;j|VZURkSI7Z#!(BzdsV!eCL=+sJ*j^9HNG$acNvTKvM$KP1 z&K1$QDViQG|D!qpZ6En=W2zv)eEEO5AcbjJlhy!LUU?g=34ds5Ste{a(ID5bhptq=66gAWNLrHJ7oG9n zbn1fCcaSfrDT`#Y2k<*Tz;*PfA$_-ag^fPYtJ6^_9c^TI@mMz~$3+tpP3qxS))Ln&8mhzIT;@wj}U^;4kj_o7|Ow zd4gZE1MrCGc*oJ(cmnn?L|2%V-oH%&DALQ?2oh}}e;(;l{9kXvSE%(--*5=ga05_e zD+K9vApV2irFWzjVAEXrUkWV~u#c^`Md1Mc&Yd zTut5dh=#$utgWyfz7H`T)X!9cw-46r1{!3(May@CV&n%4r1d&p9OU%e5}RFdQ8|?j z54TAktI5Y*s76AQInBS6ZwDN}ZtpJ_ZnLxi0SQQRT}Ye%8s=X@)F<8eM6yC8jbJxX zq^?~6TKk=%e}-{}Y@Q*ch>@m!^v*e6bVOUGC^h{^vSK91@B1Tx1HWFV|6WaAdq^Lh zv_Z8!&}x3-l^5`6$4XeX{4;zWdm50nmHoIxM%Sc50E+vC z9K9*UaXaM=1-c=qpf-|Ek&~K<=Y;XpE|l)d&yTG4DjV_2u3mY_+rN!CqzX^Q{(OXa ziQ{8=Rct2W%&t_K{KZ9lG29Ase*V0rlzl_!W&;k63&u9Xa6F_z0GriHSOaXR>C`tVKFa`mFHa-b%f>EccEW{t$fHQYaMgj6A%DjY;j`p5y$ z@sciPfT%C&Vn_CWd`Ue(Dh22P`u|&{|7#2XkG~AavW{ROv^MXl&VKtAsq*d{Z|~0T zt1_eZKV@#pqUbPBy)BjI(2^P|#*lLLc?#JxP5P*{Y8?O8oau@7)lB2^-FkN@Gc)tB z9+61EZ|_Gochl;2m;4Ry}OKw zPF2r=sj-}sHMnwAN5XdqE%0wzrDv1fz4%8EKJ1NeZrwZDtu5oA&^7!V^!Yx|OE_Nx zx^YP>NcCVFGhntqp!9Aj)gkQsj97E8`09Z)46E+DUhlrWHQn$a+WOUJO`(D0m9M#% zoVI5|A}m17oo>t9bm#-PW~tCe}A7x|H1`>pn=h4d9C$?wXEqo_{ut=Zx;?M<`C5VN!tt! z!el|2(v(Og@4FKQF5HkSmVMxq{D1t4l@2~#zYfQe$~9>r69Ejax}>-N+mC4`lB&jC zPz0Tf7R0e zYf8OX#x$+G;Z;zy2k#>zV4HrWP2FUhwd=pjN7fS|utM1me?bgY{@aZ_PIYHRl!3U!!p(d4wL|;34h;nxn$aBGi{MQK~nmDeE zsBMTjJRt)%D^ox*UMT{pj8ksj&=>Gf8QBTVgyyT4OY$Gg8JU9+*!r7xAasGkisH3> zk&%Rhoj*;(*9cu$hlhyCtmUCusG0HEAeqHpNdEzPn(?;)59?EAgSC^V$5MWo-7f>L z-z(OzRD+c%DW&)jH3#86w86A4dis9 zWos?D&5vb1h{j|4%DLeQ=`c{<$hD7Z5lE@rq{?k6pzfz-zz;qRA&X?T;>nn_(}Qz_ z%l6ck&+IpAE3b{rjdL^GV>-{S(+}722B^cvIfc4kqV-&q@CXoR&!_OT*06obZK>N9 zm~_iFlj!2vMZ+gP<1t}|9nW^_+6vdNh&O@*x&KCDoX1XH#HJ>4SD0{!=!@<@9W21+ zechVzqB4w?`w|-exNC~mES@9cHXVJkaugVAPW(Nvn0^x}>#(==DG{YS5d{?ZPzug0 zi$QFHnqNqb?rPMH{Oo6;gA1xrULOBE!qGPRFJx=bHTRQC`Dh|q>t@;TQd*&XM;2&0 z!~BL=6}#v@txgZoLGEE*@)dGw;`tQ`4{+|sB>_%Cp-st;`NnHTtB({TZBEQn`i8QN zMyHicGDg%+0v@WooL+gi6mWL;fR7(Sb_xWx_0t+@#qX^A#9>6hLvQ9g}rg2C=A7aFcDS2 z-u>(P?0wzxA-Ac5E=;Oz(nzm*NpN9&KD`Rp>}so1XDZ{FAcL>{UeB}6uAIvE>bFvk z=BGakD9Y3)!-hhGf9EHj1x0{}%}kFcXKty7o{`meAa{W-a=b zr}j79rD_j*5pn|wo2W8>VQacnAVsql(dqXQT5-*OM^`;}S6!;g(j&Nm?#j>+ z?QF3aB6GKC%n0JGC)FA(Aa~{vf9I>VxT(owxS-b6D%4oD(IYhx6TYMNT{`%4a{A@t zELuTx?4K*R18oIZN(b^A|9)<;-m@gRZy_q%l7%WOs8 zqCgea-rX#D1(cvt$>IW$caW#oM(ILQG%Nf*=ttjjt8ho>Hrrh-Gs2a(2gj@y=w=6d z_359VL;iZd_~4bgAuoWl;}B+RG7+3LO^UmbZZ;mQsh^JC{rgK3mqmL$UV#d?Br!de z6_M1B@o3r`BpePMM0%c;XZ1|@tA`~ymoqY=Q0U%I z5V6B5&2&|Mt*Vo>n!CenwWYwbFY6B0>Snbv-zUvrJ+IJ~pkB{yKU;sLa++1{n{;FR zEq~LQ<%;b94!L1^?RxEP98elZty5~U-dxlYV#3`kV|aw091c^d4tC+g#eXmQd1 zQ{n(ZKpqBp8HsqfEsixxLY08g6v_}8W{NuX(Mf!$hDTuWMsb*@Uu@!G_`cbge%fTo zqy8GvVTr!zkNw_$%T^R@K@m^Lqt{tB*^QIMC6*cI#aZ-@6-v)kJpxBLN^h49*Ua00 zuyUnvN65YRNME%wCW1&eNDpgFF((LLB=1ZJ!_av5C5})|2^_EZ8ijJD_ zyBC?Pp6R{6byIoYT)MVZ8|pKVZ_ckH|?Lx1Hr{oZ4cIb{%*Xf zvUA_Le9o=5$|QmsR0^h`2kbc4{_4nH8s|`^f=KwgG4vZgr2xXO=zPyixpDKxaKV5j z;~t&0O7SD#@{7oSqLCq)gHtuHrWRD_?2)$3OuO_5#b7Mz1hiX!=xLOT7)~ORIZtiEd4IBBZ&*f36t$fC{wND7lUGV`1R-pxS;u-z zittAL!`AXQb+^~n*2V*I*g*Aj;t9r^r^28>qC3JNIc&dI);Wxi{kQYst1`a`sjVk* zshs`H=T`~pE4RHgVnh#y6!;6p7c>%$dp5lvkU}tO8PuhHQbskCZX3E5x}VRFB1OjX zkxYPWx_%Y>WUX*JUZ&Nn?Krb-t=|3Gml2tVAzjM`q#67zsc7}lI9*`2ee@b~^Q2dD zNC))O89~wB@o6B3pn5Jk;XXYx9Cil9YlrP?Z&#e3RLmxZ?hBawk*dvEq+Psdf?DG! zg0Pbqwt2Red)7ya!IiKx^rpev6td{#E(?*O`jhbBpTR9DEBU?#kHiw{-7^Wp=ck5v z&afTM?cDR@TyrOVBS$;4KcG7ltaLSZPk$ zot3nmNZ^mLxf&8Xl}t(!v)a>LBPRTS;8nF&DE%c&CVt%17Txp^l;dr^RE4WdFW)fd zKM&-1o=h9fr0!$Bm>YUh3}vFb3>Xa~$FzHg?(vVrU)quX{eg!ZC|U-uw5BuvrlOm@ zP73>fnsEVuSCwzA*muXy<+IXKVYsnr{^IH1WW${E?0)B7N-E%QBPaleX)_catxpTP zvK>6ECQ@%`{nz__O=%>)naqgkuij@)xfn6 zgbVvOsGZ|p-L#cKWhBR*nRB6_e~4k&p`RHy!pZ_+;}X9AxX<$?0YH{jqJNvIec7O( zw&{`eFZ#`ERKTF{pu;flGPg%!re998LP;itr0NKs^E;bf#c{gvY7h!|2#Vo6#<*JnM z>(^S<(YK@_!^+zcX+Z$+l9h0HRD|vM`}?3_##5?PQiWtfQVIswObC|B8=)VxK$8RQ zIzblYh{@WC_&E4#;D0a%1_3OX+MRT-Yw&Q}bD5p0h$*ZJ-rKLq-E!yD=@AD2M1QC* zW8*CRUdHjKzRBt~;v(r+5?W6=|;s>;@+oLhOS!^yT<~K-5rt-{V z-SRV{FDu8%Y^sT5ETu~m1z-`VzT}8+)!5#}hSbgvASFcJ2$Yx@w~w}i?$TYl1LURZ zBlvpd(0oG&>5!6v$E+O60kL77qw5rGzzFB&mJPMpYEOF?1)$>{z3sN#zb^Lzl3~0$ z31lgFIWzxWQnaa!zbTribU?U{5#`9Jj#>`tD{J7mMf=l>g?F! ze-cc5-N6ie_r6Lqgj%$JrY0&y9>Pri*PIk!I37_<=O$DUKV^_C$R9S0KWJN0oWZx9 z!{sXYx~F0~eo~A2MrL8x&XzRJmd2xg*Z)vcRc*<;4>d91svt*DJr06gJL*&UF6A!+@y} zrKH2PDcms%ub#z{wm(bzm6#f=U#i;WlF4y`rZ0u%MO@LyF|qUbW^8_yRO zI!HU$q;1&N*-_&8Z}?6(XUnddV$QubQXK6K=3!=~;|=+%YKY%zz%*gbwlMzZXSnn2 zxmGx#lM0s=?1UiTh1YJI`-4UY2HgIwKLP(3GAhEreCGa4?MkFdm1QFHdkjnAw&3U+CY= z!A*(NPN$xShkEN=2QM^$ph!8@oaXw#O|)Zwqq9hN5v4jLp0q?r&9##kHH>Hj^dyk- z#Qw)<9i8NVam6vqRi2WMgtA#aKhplv{9buA8cJ>@#wUHO=1U)gt|3IFBfOGcB-p1w*xBI8`gd4A#ec$-sENOKv&QFM2SpE=e?Xc9n2nPGni>zE zR~4#LSX%bPUBB2!H5Kw33he2vxc>udJ{vSDM^@;~UBG96CU+N4hjoWp*bf%EY!<_{ zz@FfWU9vYHrrYSLJn0#U&#oo%*=MKs1U$|*&0!@W4Sz~iHN(}*J?~*U4vOwA96YbX zlR+auo`QodU6k3wy#5NMrylCCKtlENNn;5KhSjN#0x^pm z7wIOZuA5a*9+;ybjl^*k{AVmwFmFP?Q-USUo7yn`lC+YEa<9Mv2EERDrb5-zr=4cB z`YHUu&r3+6o~|5<)@D6%NzrU_^0LbHNb!`?w=$3ev_vbI`J8DJh@QLpymNpBV%W!i z(u!C6gg8uqV0{yF`E2hbINgPKP`owbXq&_+1n>}>0Cl6)$aVnv#I4XIo$%6P)RS%lSt_PEq9g1u=tc<}Dq z_JMz7qWd&yOLGFIQ)9QP;tYOS&cBu$9jyVwZ|hyzuMen`sGY!%bFg|wScZxEiSCQ0 ziGcdu)qNo5rRoc9(6E8jUUb{xl+Awq8^^zr3USdHDsJ`nJ>zrYeVHgGQ>D=HH%xM8 zZkF$>+FjeDQ*&{u6|Ekf8yr)!&=DJ*q5RS{TjcZv_aZj4r~ijFe>~jyM3RR>jq}Rb ziSe0%yGmQu%x4KdL#F;$l68I+PkXryxn0y7Hg&#_mpEBE(Z=q4dC+IPj?i6kYi|Ly z=_lyWtJU6KBeh@%{h{P(2>kGR6%m=S( zrbtLM3#nvky(GIL+*Wr>o>JhzYV8hcUUVU;SJPqU2*=eb&q3f{Rp1Ef=p#d^tVsFh zWtvN@*bHaixG%j?=WqX$g#uPC)g>!_quR7>|4l?MNaLxXnxMjPuF_d{JkErS-`oH( z*`o|E0t;S zaN?Y!aVLeWaTUa1>K6@LP}EbCJ5j9JDVOJ)B%vRNQ^o^4uBAP&U|(5!jzwDwj`p>jp(0`+cNhQ@-+;yo-e=+Oh_@s>oD79e|AWB&yYpw+Yj$ z-px^zJE)X&QxskP*ss>s<1_3I8JkFib!UNOx^;!vJ&RREj}xt8#h>cmIE2O7J#uP! zJ-NL8C&}&(dD%gdruY>Jd;?hLwa>6Z4jwMj$G#1!-ESV_lKRm+=5W*BT8vuYAZw!F zQCm*2zgS!g!)6_H1mw7Nu?>wd8tMyBnLe@>2D`?Y(}OkFHbVdVy^JP zpA}V3*-~{9cvIoGM$XNHzaIW-;`JTotfvg2x7Ap%pu1t>iC;u4rqcU&>Ckr-++AT2nRP?xIb}3fe{fVE^{fnKDV8wAUEzl^PPWC}v@Mp*A2E)s9{&MNg{QXR z2qh3u$Mq}npGOCXU47Nd;t;n#Y;IElUkaV~J6|``=iRNNXDfb*e?0 z|Fa;~7Y^2{XjY-ymy^-Rjc+b#>lJDkVG{DkuVAm3{b_(^8O)-oHmAa1Y?PZj@Tv@X zd&jM`=p*ZGlQ7y2rS;GkMF+98-rMjoY#~d#&vHtCnS$bNe0tR82cKN%^48Y{(>*#k zr4W@=S(l=YQZ23s0S+*6fC-YNVZ_YeQ6vH+C_!)P!cZz zV|IzqjD0Xs`bkXJXhxA>{qy%7uH9DS^hZUu{}b)U$qNro?7{f6dU~ zFody0VmIl4Ge{yxH4{uNE{6k4Dvh?Ox5$_{m!8J+(Pec?4y0wKd}qAp#pz%bn0t+R z5;q!1`H4iy=)2Yk1~u2}QCLmSZ}2v!*^In^v|a6L5$06me3fK0fR%2n>$^sm^%uOx z^J?O-xXww zCZlvLSf{hDiHvA#cygev&V{H{Rf>l!+CT`Ihn~!24?28UhAgBs^@xso*-*5T8`bC_ zo>`v6qtV$z$kiSW!?-@&R!sggj3x1VI1ecE^mhHPc&s9J2zQ9Y4Vgqj=bOGgY=6?O z@TVC)_aRfZ@s&i5+ty*gX9VnJT)Hm-M&28(_GzA{v z&gCT83W#JCC?V9T^njX88K88lt`5FzQ8qzo5IaVAQk^eVAKk4?f!0JsT2{RR+~3M! zM=ra~XE9fT!pN^jyG4iQ6skm?_fTE}7gW!DgPgW`PWL7@jU{x0S?rG*eiH?-njW68 zP5kdVWLtXmmK?T&#s~U~4r=h&ZoXm^_s>r!1F^#TC(#2 zIgrQnj3a2MyWV}e);JURx6l))@6>&^l`j;^&yrceCX^c&3~k^f2W&n+L|~aX8ikdG z6*Jy?TH~$k+)NgS;4fpQDkBeBnaC>Na7X7PG`J7(Zu?@?(P3xPD>Lyp1ci~7xUu$5?kcGpWgv!3QpQ5yb*4m+QV zatbB+vI^clb^quH@pT~$F;UZoY?~O5Lw4j}dE2{O{-`7dQTy14dqc(Pb{6Zr@mJ-Qkuqltf$SSsDNMP08%@LH`tHdO#&|a<}vA&9KJ-ky_V9qw~Y#TRo z0Vp*$cw$-=*=PW|^j9$;&c%g=h3-2D5+sJT3UmtghNZXEvj@joBst2E@7~y>IsVGcFI^?mkzTZUD=g-#j_;UWbQ?IuDyO@yt&s(+hQ9rv@EaS01 zU?t}yoUE8|F;Ex8>A!#fUS8H2PuF^R`oh=??YN(hDO_oxzzBuCMUqC$zMCHv6dj6E zc3`cFiI#@9-JV*{&xS0(X!k!DQ69~nBnNRZJ_~rwvEuX$4JS+x@nPrqnp?0Twexey z+lO^M%5~R?>Y;CXK$gX(t3WM_aQBPO&aW8mNq+&X&30VX_4j11_X<-eG-)(S*vaAW za;njn5T5X>q_w%+VSgWgBLV(8jM~mAZ=_??s(j+p^P;OJ#Pv9rvCj3Wb-MQK3-yKM zx4Q6OpXA9j4Q1EKfPr~WdGdJ=Qu-1e+b$)^Ea~fID!bk>y6FLJp3YKf!Nsd!nP5%% zyYKJE{thZ<5Ivs!J0;<-s3$(a+xOwWJ-M>Y3ozX$2WFTXXUMo{w4d}EJd?T0&QD4_OGS?NQRhOS)&?df8Atz};9{g<~Lj~ux0D#dd@cds}g23yxnarM~=Nj&t~dB^+_jqdQ&90JgJR?drCLoXFf09|!^zCj|l8_bfg(2>3 zN;s?4$0A$PdXD3!cv~AED&?vjzV+3+3X}q*PW6hO-7zAGhOqNK)a|NX(-c5fB&k>K zybLlxiqz}n`QChO@jt&~9+?>LDl9~pCGY_M{+puOkzLlJ)|8tuUIGC81@iWw9mxO` zQAxwRt`+s)VsB1+_oDFdT?Lm4tURR^}}mtD)TjcRr1s+`ccTu+fiLv}>-|5XD-SD0*{PypXat&n~x zNt%k;CkZBNt>H$${`JlFe$R3}X8 zbq^PhjRgySs{a)oNHtxLiiwF?t$Po%bFw43mS!d>`@yCJgnRm-V>YnLNV0R>&mgy% z2ZQl)tY-7`L!>q}@7Fl+K5&abJcvC(T-oQm5D(bBaAW)N68TIyF+^A{y%~CoE~QCB zqxV@N9FsqpI1~-o36fSD!>gw2?F*tLnD+ukeePg;hXPn7iMYba+Lft+{Y!Mbl7{*# zH7O40bt7>Z_U6fgd1K52{)3q@O!~aKbA|Sfy&OcjH~p zIff^jpW5En-#nxBbM++EH(OP*k4n|3jFS1=;0`r3H2h{&?L{O9oc}fAnuCLaf~rQ* zhd!ND%#usJW#{wEXLjG^E!mGZ31xfx`+u|kbL<_f;^F7>Z&cq=>@rCHI9&OWxu?j~ z;GRi;jzLX|n4(#@R1CxS&(eH~Pd`PfS^TEkUD}iiO9TbW<_@#JSs*|Ok<+#dll3Hp zn8tCR;SA=Sh*8021@EmeA|{AjYG>O$$dTNfoSBN#{)(1=V?4b(>p`Lcl`M&^erJ6m zxsCr3rG0}s|NC_u#A0<+Vk^Et=^#P#rJa6&dBf5INXhAihi&g!#?s0$mC+SFi%2gI z3kaa0_rk)$ND41B;WUisiN71Rch>`hUy$3MARuOrk8Y%4h_e_!+tB0X8mCb6O`}D- zXZ5zkaa*;Eu}n93+Lk(3+zV?=Pdk=~D6v|Ns-rL5Ex~;2oBJ1PS#bLPj-Sf>v3;0! z&Id?5R;3w(8W{Crc2WLXi^4n-1F{!L%1-)%Nh&=J1SR*vT&} z#-^_V6u3$)o3y8@@qNraSSYvx1AU1(Kf@1uwjTEochlQSPrk@aG`$d&`cp%cR1H%Q zC3G(MVV%7<^uWCPhsO?ZML|B zKGy$2!EkdZQs5G5Hh4BwL7Y0?o>jqb&o(;-Q8UQ{i;#Bg=~GvfYObT9H6@;Sz(kbMH73h(On+q_ZXpOqoyH>Nu9zw zokMwRE8J`Y&LF$=uM-b~w!|>%jW=g6Q18%WImlZyYl^qGp@tFQa*Ne&k1soG>j1=p1v!51dS!^PoyZ)<2KXKPaw0!j} znvc|oBBGk}&h_b2!cDi%*N;_g-Zl(;^kwHc#e3&$|H!;MjQG+kQA`u;Lgq@HyFJ$3 ziW~OmUnRROM31V}&ljwU=WsStH%0PQa|FpF@jHp@evifUeqy6rCK}*`iv81@$Q^&{ z!`Xn=q{#|vp2Q)z4#$EsgaWWxlz2OKXZW{5e%mFO2p_*jR(9%1nlj8Zx~bc&L|5(8 z(DZX#jdC`J)fqnCIQ(*t-@C!O%(WfGci2rs8a7~xR)eb(&AjKuQ|4aw2rZ)x9c()d zpJ3VT#*51DmBne(Fav*$Tsk7kHG_tbb;L5ft?L_{9OrewPV&rzZBPpU*u$ES^Ut*b zOWXMFJKsd*LxFjI5<>csh0f~*e zWweC-cNy@~09#t&H?&E*UElEMihw4gN)$D4pfjAyiGA<*ff2C#XVyvyych6~a+NSi z2{be0q5%e%F5G{Ae<8mXHcbd>z*G62ay<+1zoq@hCspmBrX&jzbc=8d%%tjv9p9oL zoitZYV;6I?X9!RdU1Kz`#jdRWhAafkucpJBI0Kz&kE8_Vy@I2eG-;lF=#e~}fmdPd zIw*jG0K^E+j?t?PR$BMCC@1?dF!qaWGn z@l14zeRT6gHzqrXZ_JmPZ+OqQ*SqESCkjoevnaK9-?j*RL^Xw>PK*-W?HLQ_D#PebWpGul(){oTCiX)#|h&L;*>R_u5!bJ?iqf8K#5}I`#0+;v`P%TB zH8QT=Ew&(uR?GC8m$h>xCxWkKJ{>gBdt$rrc}9NQAil7G5_U}#$m35OIyi2OGSS2J z$A0uF8<~Ivr-&S!KHbcmdN?CLNQBgp^i-fo6_cW?e;44z)fTbnf$xe+3L=pfY4=%N z6b~&IkBSLCD0Wm?X5FRx8{}3#JYEg!CwT8Yx5izkhe@B$8O&D6N@QMI)YR&aiLbDf zNGe-ek8n)o6<*F4q+Q|opx~EmbNk4sSK;8(454li@$}D3!4ul(ySEa)J=V*1Cq-@7 z91p{Fe@aa7S_Dgxi%B{GU(# z?JWv!_O-~JJPBsTd&O&JE`ahs|AG80u}L}P&rZMTB)2lttAJ>$w2C8`HI)_g-V?hT z*cCF;VuVJ55+A>|fLXkNJcArtQ28_TYs!dy9}SD~q6~R#)gZhw;n>a3YBHuud5$q< zv1g9Bf2Bx1%KkC`$@$8@2$H#T!w2uP=j%`%1GbUIXid2;v}d1N!2-%Xm39K9Br8UW z={|4^h^NY=zW*RzAvk6!wK+ki8nla#`-8vl^MPZ-^M_pPI6|7c*yl4W%sY2-T1zQ1 zb!A|eE?esnmmVGFS#n;81?TmY-Lkq^X5_ZiF^!n%rjC&PM-yg!w^QQ*?(9e}d{)f( zT$oa{^hc!H(wRx4bVzx8^T9C->qC8qOy|xPGB*bH?=1BielsV;Uc>}N@6Pk`@8US5 z0Yb|tEU!l|zbhn&Y32j`i+4!Wwn-nmhLAJ2#lpfKB$?dOhp98e*In)5d!O`%vGMaX zGA#Pro%}g%>&ik7hAgT;vrBhRjkP?9M*S)yiXQMWvHkdG>?$y-9}!DKRKWLN5-vy+ z>8^hM7jF-B#Py?78}VuVd9Uph`kwmHq3gY!QI8rqWG97YZ;>f5);7=6M_k?FmyY_j zBOi=-sYVTY^$~|z@1XC+ufkqDS&=ij7QfDR@-o8s^PS+jgO4cxpRH~!^4yfK3+C(U z-@4pRIh^krMU}%mG#cK~pIbX{Y|%^xBoY*Zbc|`%exfD`QZC?W*|C#`=a&<&E4=q% zwH&Y`UkhDKX!xY)yxD5@NUbGpaWR#}OkRzX+x26O)$vMlbH#H+Zz!{4e=!Up{%s6i zxA1D_Rje|zBi!jmTKN!&zeCb#hb!LhO+oZ@EnG{M`NtNsUbT7ja)vtGV!IH<(1%F>hAg%ojD) zd#RvMc^WL)<-Pq#&%Q^}Rxjb7axn1(onqNBducqFVHTe7%r6*S3XKrbK2=+8! zXl3Ic4tx&X4)VB_Kk)zurbE9rGOzb%x$BJ-l6bn2M}9CS$W{0J4y0kVGH?_DRv*%B?$H?qtgln9SuDX=QI zAxA$Kv^cJpQ14~`WEwk^<|KVTWttpo`^(tGGS_yP4*pE&N!+Z_l9X7ZWs2J+hHmmm zCk5uPgykMhv7yIT_Jtx$;yS<_?@Rj08E2e4(o&=qbZ?s&h%$fs?O$FB28#+wSA3kd zTwT!oxOFD>M846)N9;#We^+aziEZU2aP=2|*c)H9(#muh6leN$Op+YN(rnIM)p%L9=eq3+D~=AjM~XoPDG14Zc!-ZaEDM@ zJN|Gn(2lnj4;x^j!%pF$*!ts%6wYRwX4AAt@n}BCh!?-~a_Cp^qwc4j9m(J$ma5c= z3}x)y*bUr6_ppK?rZ@Lun~gU>Qt3hG>r44~mDjrkWw*zC9>ES8SQ_Xw&(+bj{29`@msSy1)O?L6->?wkSKWF?TSL!vr8`O~f!*8Rl z9+@^7y7>g?Ej3^p`|85Jq7|W{lQJAe$PvgTP#HvUC5dieQSi&MvIf>9ndj7G_I}zQ ze7Zq-R%J<*u*FHTcnWwj)vD8?ms_Ojc4}7!0qv{NP?|D2w7lEJLSlBSs;V6)ZF2<~ zdxO4ajVj743qRS&`Kp~!qxmC3Z~1BWI`Y|{67VXUsPvCd35zBR_pkD>43jl2Sj^?C zJt%ucvt$~Rc~=Nq@a^>i&Vv09!)CWWX6cKOj3k=HuiVGSK8&S z&bW_%p7s1(kGvfXejrdlp}7yen7nBSD#@3)hYGQ_f7or|Eo>bVsj0 zky(dN^-v;1CB*!obZSxg^TytvZRkR8r>(oCjyBE%s!K*~=_WV^~G#?l|F@w&jp^_Tu)muVn5BfQ6pKf!17`h^M z#RKu{4&N9~i~n)_#tZfaMja@g64BhIG4l9_w2;Ri`;`{qw3-piJP%ACLhTFX^Y=#1 zX=n|jmD?6cV-EO8rN8wDFC4_8`3A<8eF2x-$=#D@A{k-rECb@cm@Qrp`Mj=1WP|Wn zmZ#*w%j?uIX1Bh`rNjpYsN!8Yo9ALe#oms8SNX5M)%cS3Y*k6naR%%%An@9a)2(fs zC)wOsc$K;V{ZqqEuwgbv-A(2OHstxEP@?NxBpuLrx1n|fUi5K;Q?1nX>jp+ilxPxE z>r&1TU=0BIW)_I_>KNlrG}h&reyunO%-OB&QAJgG)M~88M%Nr@``{^mA=#TKS?JQ1 zs5Bq>Nslq#2J)y9w7tBixtcgGC)ZI8-J%9uos{UU7lXfz9(l-#Mkr`%kTbS(0qS4b zn(nKL`g+Fb-4=PHFXwmOhQf@zB@%|r5{ctuDjb-aIVQjr8kq;g_piKS=w#UkTv)@OwVUw`RMT0rY}@#0I(E|<%pP)_<@lhD zeLyBJwJSNe@*M+w4GbOeMbrln*Q$+9XgzYniBx6hkquCE?Zz{7VE$_Zxr$7PfaP-9U((HF3&J0L{~{7Muf^96#IZF{!9Ue z(Km~F*U~B{`10SorQ5_$jERI3E6Gnd!Y}F}>yvex-aBF(VB1c_pe$Y)Ms?1Q!VMLw z(U|ipf>ud-wO9Y!+}}j7<#Oj9yMq$>0<`S!n{dJhmFBK~MQad(;2O74FDPkkc2#Gd zVoa7K!R=!Ctr#P4P~dzcZihirBh7eqq8&eq)7_#DzAa^z9lYgSXoZWEV+`lfm)O7h z@i#Kx8A8QHiTRXrqA;^~Czz*eg{{zL%$gqn)V0`~YBd+l8svgRL;B^iE?YoG93`1d&qJP6<#ghyR*!M0$U~2qZ?2+O|BW8d%P)PyfJLzOe1j1;<4vkJ%lI=$Q5Z(y3kO}3 zdE|Qe7KXm8^Pcz$Z*!~Dy||q($tt3I^aFY%?E$|x%e6v*hcJt^tnI6XmEL=oO)^{1 z#V;zd4*1?(@pxqcIY)Bjh(X#?jKS6IVuXKk!cIj>HGq3XjQjMy`KncEc|mPBtx~!L zeSHC-&9`{d&xo;bbIgH^enhF%wb+tGVN=f=xUzmqN#3&9rP51Nge8WSw)}?IZH3$& z?%FI&EB z{ah9GPZJ3yjJS58^aJ|4CRZ8%ETZT4;p>}g;HzLqNriXNrA!KKIpd!DdJ{*}cs-Xb zY>wQ?AS3BfP0PG~<{uL%Xyr&uP16I~6-`~)UvFxA_1wt35l*cuEtm4%j+yjvO-=Jt z4}=S?ddtR9)mk#h|1-?W_j${|{VETZnNOmQo~$hRaid>Vj&N$nN0Xh%IX35?zvS<8 z_fEm4=3p!|zJhULGte|8wlgz_-fNJ>pGs;~*r|*Kx-LpXE_xnzW-9;HGRE))o=4dLTj5|<;H<3c51rKuOnY~Wvqq%8QvPV7NdT<1(tE{JPNYa{YWB%e)i9* z(U1VF7eU`&&%A09oT+m`4u^&sZr{{`KN9!do-@|hVb(0^1H-7`soL>(e~J{2p>)IK zIGcK4tpXezG{WpBajyKL;P#t8jHjp>cj?M&oysPp^pB+`T7efK~;eUC=I+iw01ngKphrt?U$}j=2jbQ`IY%0 zy~;o*`+lLX2c<%DDEszpTgm+eAG%p`6#Mmqv72Q_^wjjpX#5KYITLm5d)Ho^>o zVm^*7Eb^}2bha`F_3{bZgLll|c2OGiDSP&H@kT`ZS1hmRR_3;ITS-ri-IG(WEB)O) z^;thVnoqnYYdpo`3MBFIvIEg-!({p%yT!CBRkuUjmcsTVKZo%gPc z2v*3d!$-#cj~)rt`RqEKge+5US68yH@DA}#OVUZE)9_En*i|A0(($==rAWJL1#FZg;-xw}A9oC>5rEe+x z=!i_E+tDMB`X0mZ>L0yF-U**Z6*Z$(Dig({rTw~!g-XXKSx`G{^l&3Q_KW22IYKbX z<+L{^vsq+tdpQmLihMA85tglGcL0aLW>=!MHUk)`^e<^r>p!)8g8TQ%VadF%N58H_^SmfC~*|(9+nED4m zZO^LS@Rtcj>19Q>2TrJ2i{7H6{7C5I026K+%a5z}n)GCWzc!I+rPnBpR zs_N5JJ;Nc4lj6yhLt}T|4{jh*{CHxf@u8T(^$*?INiM~Aq}9@cybO;#*U4wvTGJVF zkTp+9LDfq$FM*MII*;Wb{xw+Ar-b4@wPxlZnImTzQKc;jbJPBN67o~dhXD|wdLX{) zQwaS>h0rm$`T@&{T2$Mef<=VM$eH@o2K73v{8&X>v`$c%TO~&l!tAd)?88<~c(ghV zgw>C2iePCRHZHjOL`rx|XQKHz?@&Eo+2NIdz67}fJw)LQ+f-Bt;~1CCW-CX-a~V#U zn~aN?e^($bji5a;U6!W*@bilOe%yJ(W_JamAlz}XyT@f4KNjEtFB~NbHTdfJ*RHS_ z;S$Fqz@Zx+m))i(-z~B4>%9?=-k$mNcJr6zjTf#Oeej8?e^jaW)gaSfefgg){C;Ek z762~uUi>e>M5`7v!mLEqGI_W2esO9egn>|AWW1`WA%^5AKY>IoxsfYqzdb-arh!!O zQ~l_d;>`CbgZ&`N$y^yBk$IQxQ_b9Uw$T=nO%{mnesJ(c2RiY@>T9QK;P&SII|HSa z*D3fTK>S^LB~w@Bip;mPf6TbYYEK$X`J^@pJEb1DnYxj4!y-UenNj%t+`t@{`W!W> zqYNsWWO?Y@4wB#lU&Y_sAs(;XTNIm7M?Yt!62pG9Ub!=i)EgoZjHNM6@Kp)NpXk*1h17DKIp_P8 zYIUDB!nsVW#up^(3RuO?4Sy%`@atclaod^%t|tv87=3!h@2{ELLAg;Yv2?4`1*Q2v zJ}vc}xy0BBPD$1F6VJNv+xM@j=G(Gd4hupyY>Z=Ckw z{9M2REU=Iu5$`CQCbyMU`s#CvDM7XPPGLY^(QeVD{<+<(-M4iLNJ+iykOKKlF{|N! zPWsz$V|?$wf#sQA``40BNy53TtAp~8Lj5;ShJa>Q**_n!E-HcFu#ujieD z@2Tmm<>XBQcs3mC|KU3HY%bYRJYJH@IyYLW4+ojXebLPkSfSfveOB5MW9T8lj)#|8 zX4P=Ig9%%ZT$i%8Ml&YMSCgdY9;CeQ#M5;vp_+^b#JXqocDjEP)4{cZvTla}0dHYS zqePW6&BDGN!&I>|Fs>@{?U*GvSN3dbS3T<%iJ8D_oyX%uSl zX8g&kRP<$i4@*cN3t9mAzXGrgPx1l1(txj}v=wY6Qr?MN0$FS!cU?DPyI0M&H7#)J z%}F?v;4Wi+6--#@t22A0$jL;`)zPXzl_HiXXbq0SNLj9&Hjv&n*TW;k%{;FyJGMw_ zX`iqyMtCe!-VHE@uzeDL=V)+B9y}}yDC8b`uz0Z_wPNk%} zTn+{$Aur_-$i?_m^(`s&*!xO)7E*uD{g!(FTuuYk=$T>RU(cly${ot0rX+X6>!dv= zJyE65fxZC}gQ}b?CMD)3_G-lXr*)rw>H3_dDYfWHExt4^s%j}EsY0?~crDz5YA3$}K7TSraDp++1uo7Gw0UjkQ4-^Bg%|W9>_gHOJn8TKC$xb1wcH zD=YQy%TlMtRDJxt-Xr>oIzCd1O-jF|#&0g4!M`S~SLyl|D2uk--0cy9(5%!=D|F}X zvtk4`=^^A=pgfym)s0+?XlSolb>J@gN6EuGc|9y)UZIC*UqR(qX@aCOM6JQ?6H1mX z)uNg~XnD=flZe%aL#yk3)VtRtxlzW7=8Wu_P`nDW=VA>@DRh4|ZLu@J8gK-t)^Vz> zUfSw2id2YXW_v)s>P3}8zvDoO#bH-jNprV2jX+|Im8|emf|pNdt&d1TOQCt})mr9S z-{&Hi(~#@CTuz6t^i%Qf*t1LZo3WTkjY&U#zVCbVI?g;yl^?5%Ha+FnMU_HtB~1DN z`iNyLk7V}PkX%f}%9CnO%k#)my>khafXC>(4vl@a^geX$Xty5jfVMu!S^p_-fd9W# znbh~Rx?|+R!u-E1XA5-=(PpLg7^D8efRYoV3G)iwLL-BZXi^w6GMH;-h!3}26~Cbpl4dY7_YLl zC}@rHV|IOFIWH@FaKZ(RwRc@gp@TK)=|O4T=M3h+u|8MkZb_vvBo`X7odK3Guh8!} zi8aw`arVs8ZmGvA+|46Eb(hAjpFwHY0vJzfj#yCi>J)nG{?VHF*^EgPI|L_;>1(I8 z-BRzG+jrWFT$|^{B&WY}dt0S|LWp~GC|;gX^Nu3pD0QhFkF74{)FNn<>a^lFb(+3FPfU9hCMbBx^g>70(qq#5pk8FyevfLiA_e;$ItMBMzUZG#C&>4UZ`WpaUXr7a)y5(|lEG~N0x!k$r z&L!54@AkAfpi28-d&ZnnD{qNsNS%dHySE+4!pXzwbsAx%3B{ObOl(>IJPh$S|;wSZDv--!wU3VRB7GD*oxJ9ug0TudWCYWqSN|5 z6lPeEH7Bjb;*Jz6B)pINycpCNY54rqwP8Gsv(L3Q>mQc+JZA2k2*+1!JaOkIH$S5^ z_WT@xnPamy2O(GgQfxiEHuv8efQ7CAjRT`ZO3_IR#9jhcQaD-cKM+S-NTxTO1%h@` zbJ|M%C0IhCmsX^Ah+owbv#>QI8TXP<>ev_GhL*9Xr5>}NA3`~3#R`_{TFVj&-7?x@ zYMken>L$5bu64~zw_mFKS;~C&cxlCViPl()*4{V7di`gU3RzIDT@lp^y>_tIcHE2@ z5hpg>yMCqeW54J2ou^_}v48vbaO#$`kW1I6@Rdx8UW#0D@2v&)op!cWD$W`9!)Te& zN@>oOWI*nI39mz`Z0>?gn%wyvvG>pA2$cS&k+AJmEA$#Hbi&F=xUh3dz4cim55wR- z%erzN2_?A+w8KqY#TcH;atb|!nz4N6fw=0s_Yg3gjCvqPYM*|%uNc%VDz)SgfHsyn z%e@5%1U;0jaIUbD+77mDv=5rZ>g)dxwBfg-@Akd8wkKWxn~k>m#E7+H?02A_r7ZN^ zHY$oMcZ9$##iVlA&4}+);!oGL;XSE#%_-SZk_%_gXc!Bp#>qIbeQsPoMb9|s3WV*D zQ|c{kCGFLzNgSmTn{%b|b9pVL1l{oO*Zh%G_@#H@ zrO+iN_w1x6YXitRy34O7>)V>JF1hAg->1f#W68>}*yp{WEXJrIp%vQ}W9B2O6*^Y{ zB&z%cU%8eP_e*U%ExY~5|oKvmP zDU^h9i&ZE+SYmaxU^Pj@Roc|DoU4zCmlg)OYbfQU%4|ieK$D_%0R?Ep65K4brRF-$ zsGE6(z8abbD21HIfs#=!soEkp(|h|E1f%t zXXPZ+)>CsoX|CIy>6yb&%Bi4^bIXajT7iWgKy8rNb+JL2+j_xyUJKl9A=(KokKDG8 z7_An1ZY5m$`;7Q%%gQ|#t_lCUw1DZQP$XizoN_rF$Kjl#pBtZpGEY;v4Y3$>?=cut z)SUW32FMevV9R4Or&o)TOi&Yk)%UkGLXjQynIjh25ut+O_9`sbw)TB5o ztd?~Q)Z!L65M`}PhuXh6$&Y$fHz(3u3f;LmN?q$j?zO;>;>u#@B=XFza}3EkX%a%*9E{mH8?&+Y{@UvYULvBO%VwTp}Bo*#hqqcQz z{JOMvI{X{u+1KA<)_P69`S;f5d{CY(k@pdnN2y=$vFv-e%v=*(m5#CY zZp+p-;4WZBD2{M)UxF(oz$6y1l)7;6>nw@1kkt;?gK@QcEP*g~eeJHE)P3RN%YrQB z&_f=~Y$NjuJ*3o_;fht0@X%Ht3atwQ>G!oFbS(69s5B^<`jp=+ygP@U<88(^(izVj zaZl{N9`73SfQ(`}g-*eOz}JfPZA7EPLov5<3C{Jk78$2p&O)>2yJ{t+1SPiNr7Usk t;)-J6g!hGE6pA}uic#aidT1xb{|8+#9>w<8TR;E+002ovPDHLkV1g5@a$W!c 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 0000000000000000000000000000000000000000..9324343240a9a04c17757239c3d9c57109e687a1 GIT binary patch literal 2946 zcmZt|O>Y~=b!NY~TrEjSrqv)Z+AU(BAyATT4n-lvNgdm59m}#2+A+e*Vzo1(R$A^d zvrEZbP(cA1O_4%q(OZ#zhzF+)iohru1Vw!4vHxMNZS1B62Mwu4VE>)}6)zqqYb+sB@16DmY z`&QSYc-M$aeYtwKY&n1X(*qAEFMBtl z0TH-Sf<_xKn4k3ReDvd1&L+e~`SV~~WbHmEdKQ^sL66&U~=Mc}Oz zM1WnbGfMIeo<-TkG+B>&#kuxI+UMxFc)bOg_(aP30wZH&g=dK_3#qM0BW8)*9wv#PJ7KP=YprQtauDKXFaeL{QmXY z4EX13kMG02aL}P`6(vOr19fV2Pw;?^ys@{9VR8PXs{skuIXhpJ?;s2^pD$`FX~3eS z=c&+G_KMmyj1eyeG-uxRtBq6A%!UxC()Rr*iE`hU3yg#8zhx(%#&We;b&nGbE!v3F5PK(`$`^J4MFIfE1QDQ0%JtF8$rTiKw=(99nwcS z$ODYj1Aa!7tf$hscKy=j75~PCn?Dd(Nu+^tOH(P|(4>*^cwLm?7zfeV>71VSW;P>iE`RBwQf71S2~GlS?g4qsF1DRW+A7 zv`QDXGg_UxbdeT|$2rkDvSE0XTkw{kfE5@QVPrppv>DS)NRA2F)yH~PV|1+fkRUMY zV<=G&mIj2h$>8)TY>NxI1)*JH#y;JJ^w~9Y`=C5FnKjXVy0D99&gR@$A6sLCmG*T? zZbLvD@ZSSCvA;#KS9T!C@&7iA+}*5T(S&zZ-Jv^R1%)lWTS5(< z-N7xuh}tV_XqjN%lVzWFjWC>P%#fKE9TpIf1nC1WPfWX*p}MP8W!kml=z{l z#?l%DgE$x!~0nRlGRu3z*aEK@@ZLO<3c2EeN+}Z2{7> zeB2-kt+_4ru%7^sK~K1~(z)z+uB={d=u+p&ws;Xl6j;HFdb;JkG<|qXvQgA$7${%q zjN`J`Sds>63dx^~Xdp}3FbsK?$xt9 z)Y3*-ynwip;O`81mP=cJ8fgM`xdC|bbv(3zB^0H61N^ZrDVG|5S$q|b)$}!l>M-Gp z>`j;)YETRQ2CZuvgnH`u*qMh=?R@F_{yW1Uo}Q>0qePUIS@jBe z9Q*zWEwK6sr3&`f)2IppHk73KQv$*NX~bx9Rcmqpa4G~g2sRFMXvPuK>{ zB8`3!CD#DT(CbjBm0Qud+M#x68!pwJ*x+pY(4pcDd;y-pH@@GXtBnO&@_m+uV05&Z zIET}>acbgSM+x6jjPWB!E%2jY{S{kPL^Bht{Zz&PPs|Y;} n)#G@T_AAp+c@@8|ltu=%O&bEyrY>lK1Hx5zzGJ%(&WHa2&E+QL literal 0 HcmV?d00001 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 4e6191398c7e0a97092182ca11665b050b7c19dd..fa11e86f505bb09484e3b36d17259226841d1f84 100644 GIT binary patch literal 5876 zcmZ`--)|d7e&1gtm*kS7B+8a$C&$Y6imZ!cHpO`VI(329W zDJIkyWd zm8G&OjHMQH%bG|m_~CD!6UiGP_xu_7krq*`aK)+)&Q5k)d2*qeFR&jSuA_n;6PR*wLXp$&L+VooPF2-RP^Z2k$Rs zP0C3r>rytQY)Cmna-`S^S+itMEawM3N!e2v)KaoGHmI4hRvgsQvUX%p%gEZ~pq34- zNc})zQy*#UI6LtWR@u3T$~?pB*&|_Ldvu4SF?I~)I7S!4i3k?gkFck9$i}0xZ8Cyf zKQ?HaLfi51M3g`sy7p73KOKIJJ-tJ7m!Mgyf1Q1el^&|Wf1vM~9cB9)?CbIVebvYR zb=dcewDTJnX~N!kQe%^k~zBWw?BI^-6?6B4Lsy%Bat~Kjzwp-@Do>D+NsVy72&9=)OJkG8C=uheQcb7z_%3Ez4tzN74&;LZff4L;mPA6>H)bmPD zpSgWjB<8Ozuik=VFxQ;GwL82aQcl~mH{I?odBR?1CJcj_fc!uWb^MAi!!u+n&bdUA<8XTz=cDy3^Csy;0!1#9fDVOHqlo>ZM)^5{!STXF?!b zUMimyiJ*gb5T^5Z;y`nIfvfWv0c)^Ct zIfWOI85|EZi`+nEV0%jTV@K?`K@_&I^fuNV8zI+RWN48ZEjo|F@ci{wIVp_5ZLD+J zPx3R*%Y1v!B%v#55FnL;I{2dm3 zU~IbG#zJ*li`2Rv!tL&8eJxCECnJ<~gbO$<^QPjTFqAOe zJ*O&t9U}^GOc^+>EK?&LjtCB3Rc=k?N`r`E!{wxho&)=!K@ew^w^@NxL7@y!49jvv^HgAh2dAr!;u z=*bQFDIz?!fHpVS$OIzw?F2e_&Dg~QfyJxP(;jYx1Txyl%&YvoE0vP}XKCtsDHvI9u zKmX$HgU|o|;pdf;YT{^G~? z9{tUqJ^G`cK7RiPkKh07(a-KZ`s9-@cE11lC!hZO?!DDsYDEUj=f2dZ&#w23pnS2^ z%P#l;hU-_|i=|h}M>(C?I2M#ilE>cIa=6cbi|T}`7#k^Sgt5}u@-t``seuRP=crZ~ zxKi5-5S9_SxOBTP;_VJ%hA?Ds&X7a5ouKPig;8y_yJ$yD6gfn2VzY5*;g{*Tbo9%n z1RuXbk1&`URELh9;HY!=ZJ~S3n$W^C{1qB?2$rYOHjgV%@O0bB5G7d8psGJB0MzjIRB1OzTY@fb@Z=bpX*6IfgD4o|Q1BaN?tC zgcAz}tk!XwnEF6Fro0753oI&iy>EnsH~l^^w`uSfqhb9xuoWi2HxotWz6wc;B~9xm z`>8NlPep2!j8f|wWRqzR^}gzpzP6{aoo0HZqBp~gnzp0ksb)VNX1A>~>qD)ku^dV;`}z@FN14I$ z4-K5JUuDH9_%Jr{QJTC=WYv$eBgd5b ziTYCr1V@*4DTaYq<>296-fn@PfME2D>3X}iOLwM8eg4*92HtA8B2{hSv3^)ih=gRt!ffIU zgYX71t0F=rQUlW0GiH|;O8hF?dRkBt>YXpul84}jxb@m)PDxz(Y011qVL;Vw$7hmv zg6M5Hu$gEY0+DkEhaRNt5>=BRK3>linT4g*`4xK>QH{bJFiv5vxNz;}%EIcr{X5I^ zSMAroJ-fKDdP^kZ*AnTq=g$umQ(d+oB12kF1&gBFyLQDPaRiq3I!3OF+iB8(3CopzuIwH_SsFmi^s|w9MfttceG6JvWU&lp$U)9Q z&PKVSf)>4>M@v3JYTO6hqC6mF;hV>1G(wI5I@zKEw?g%-yyv44xChMs_O~K@W9g{; zJ80VCC#Q-s@x5K$sqs2vDl0EzK)s=HsS7vVs>#EZU0?+X; zH#%G-<{aPeiV36w_|St)wnJZg<{K`s6pGBX>nqEz+AH&OtAL0{nUJv7J)h0N3A8#q z5V@84)fIdGO`vdTc2SJay*j(FWX~=yuUvoAzOis^d2t@8B(E;Iel6S(mea-=yR+PC zbgwKEy5uyoh@HFx+l95(>iCphtsrTaky(POT>J_+{0_Zjm$Z!O)8SG6T_xNE}*>51UE2G znWp({VUknNfsbkwWvEdiK7;iuQrH1QQj{NM!MSed@=f(+g)op zoZ*utB3`E*3JbI;;AWK{mQGy>a2V{E&uE|n+vUs78}8aXW=Kx-(hAY?X(TQH46 z=J|2*J`&(t4bt&pm-G!3-ndW2uhHOS3tw0rPkxt>(FviU(?sU{BK3~8z^^^O>d0B9 z1Abc)P`NhJLjD&t(^vFRO7dmRZ^p;#TjXqlXHd|q74W*I{Kt2+=*k~v(x!#z@#KF2 DuA?x| literal 4282 zcmZu!>u(&@6`wn=ot>Rsubua!)i$((ZDH3AQIrN!9s$vm#6h6~v3wYf_m1t^%+7A_ z+_CX$jnX<0Xqy-$sCku_p7=Tls!$RtHGHcfX{t4qpss~lQ?Kbw zqh`pq9-7TeEhBX!w3^vkR_Y|QYqp#tSIfz_Q*+>LhWTcpRv3&Ksg20KVy!6OqqVVT z6q=#dd4*cOx}?>%QHN%!eV){|`#Wgvtjf;#+m{sho;)v~a{dnNR+%glp-;A`Usi>2 zBH|Ap6z21s1!2d8?*Tjq2x>nobRB!rMr9UNbK!x>CzLuavXGdEw-#=UA%_i?>*v zY?|H#Jr57A61Sm;D@#fb=3XI0=_);ys#IHnTbDYlG1N_(8R{0z4t1O6hPp%ZL%l#p zhI)~X4s?yGE2N<>5||&(A$3*irqng5TT<7hZlm5+Ttl`T*^}ypL60eWii1{$TU~NR zp<~xnx{YqX2(!D{E=rr{mf&12AOd8b7kVQrs1@P+Q9jm%wHw>$jul*OyR_}-g5KCU zuoJW1qd{kx7tCy<5bgDv(ffvL>^!{Gvl*iXB^B#hnUY3 z771sg6ODJWfDPdQa+8n`hUcodergiAHUnfsm;VY+o`Jz@%geXF_^^NF^ILCTSiSkd z+SNB#Z=PGd`L|n_Ut9h3(_4Q&f9uVE_CNpiSD*d9|H0+8KYX}$>An6NANQ|bKR6DC z)qlO)fBm&D&;3T&P45+0@T?!jcPPx;(W-M3!g%`BbH98EBFvidV&6?zC^B9uE#bu;>tsQbcbBU2T8?#cU>%(MSjT(-kACcHQZ9c_ z7;)0}nXp_phyw1qVwC#z&||*a_L$cM$1#zg@%g5vG3SMe&kC>=Hi`nMPGK1k!mNjZ zA90pL3&x)?rz8moC-k@z z5QKr(a_8gfR1$<#qACr9d({ZAUTH6|y)Y?WXnY2QLmUmB&HrlJDuAs3|EgvYl~~`J zx}}-KCc0`9b6wX^7Bx$S5kz0dM+b|w-r2F4Dk~cvudA@y#vfiYI`23<@!vr}R4dCT zh|<}|Nl)#PhQ@)TR@6n68$GiNvH@JVs3D`c7O+GXSi(*@_C+PyuPZ#a@CZ>Bb(jO$ z2m2kWB4=C!ek96^%1gi&i~6g2luzIDWFD*(I9^wlw3JKkZBHoi>3R4X|6jsvg?yt; zCL^%tG&=jA{A2CXyKC=!oNOQ3Qp25h{&f4sjiaS0 zFp?nRM}$_NJ1DH631I~~y5bpS=CXJGSHD>4FMm>cAVIb(KUnI2 zcCP>4@A`i^f9tK+E0u}_-rD6}fBD9(cdn-h?Ch%YW?M!(_nYn1<&{QzCfNmZtls=+ z?efP%tge1>t^dIvkCqbTRj~G#meX7VrU8J#l&;sm@F`#z06faDa!RQfVaOgtfuNSq z6q$LCMZmJ`FqlLpXwI{M`yvln4GX4pN0JNKkL8E~K?=1M3zPZH)|}6d;&hLID4P`@$=l}BoOd{*SBKP2iQrvl#DWBBDg_`V-(zknyMfN;oeVgYp_RTaO{^#a*R_J6Hf zHrWoCH-YF7{RbrO^{+a6w;+BC*?$M{7ECVz{}Cu0AWzAXx;zFDMIKXo+KRG5dOFr< zi`r&(k)YhW8I;`U87l}orC~NQ4Xcp_&8AS(Aq1&$Ri#idQY(dNKxp==I-^n>B-DNl zhvukrQD4MLE)U^Qp!syhuD)BLg)2H88HAkC7@@^IN~72q1vVR<6e^qQ7zv+#=!nph z7^w*h5vUJzz5r?aSvcB862w|T1JT)q>~&PqK^4oPN3fJQeCWZl%1**~b_xW}z*?a% zGW8ZzRT01QeYEPrkhO!bnt%={uVR_oMTQ4Wzm@O=vlEsdYnQGA>~EjD3J*yJ)-=#u<+ym1su~xRQ)QX_6 zf>O(NvCK_E)E(FTRpNzdkE{e_CSVvY47VWigIc5RhoS3k3M&jGCUXa3z6ei@Y;VD4 zt*T`gcQCYX`DuhHe;6Pf8XU4g7k=(tL@rf59%BMep#+Y4BF1zc_zs;_nsiP2~Mg{hXu zs4V|k(&oYezTvF3;9}tbm;e9( 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 权重。