222
This commit is contained in:
2
.gitignore
vendored
2
.gitignore
vendored
@@ -11,3 +11,5 @@ log
|
|||||||
.vscode
|
.vscode
|
||||||
|
|
||||||
datas
|
datas
|
||||||
|
|
||||||
|
running_logs
|
||||||
|
|||||||
237
AGENTS.md
Normal file
237
AGENTS.md
Normal file
@@ -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 <package_name> --cmake-args -DBUILD_TESTING=ON
|
||||||
|
```
|
||||||
|
- 测试 `racing_control`：
|
||||||
|
```bash
|
||||||
|
cd /home/sunrise/yiliao_ws
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
colcon test --packages-select racing_control
|
||||||
|
colcon test-result --verbose --test-result-base build/racing_control
|
||||||
|
```
|
||||||
|
- 不要在同一个工作区里同时启动多个 `colcon build`。
|
||||||
|
- `ament_xmllint` 可能依赖远端 ROS schema。如果只有 xmllint 因网络/schema 临时失败，先重跑验证，再考虑改 XML。
|
||||||
|
|
||||||
|
## 比赛模块总览
|
||||||
|
|
||||||
|
### 总调度
|
||||||
|
|
||||||
|
- 包路径：`src/racing_control`
|
||||||
|
- 主节点：`src/racing_control/src/racing_control.cpp`
|
||||||
|
- 配置：`src/racing_control/config/racing_control.yaml`
|
||||||
|
- 启动：`src/racing_control/launch/racing_control.launch.py`
|
||||||
|
- 点位转换说明：`src/racing_control/点位格式转换.md`
|
||||||
|
- 设计原则：
|
||||||
|
- 只做比赛流程调度，不重复实现二维码、VLM、Nav2、底盘控制等子功能。
|
||||||
|
- 通过 `/sign4return` 控制二维码、VLM 和 Nav2 参数档位。
|
||||||
|
- 通过 Nav2 action 完成到点、路径规划和路径跟随。
|
||||||
|
- 当前比赛流程：
|
||||||
|
- 导航到 `qr_pose` 附近。
|
||||||
|
- 一旦收到可解析二维码结果，立即选择顺/逆时针路线。
|
||||||
|
- 识别二维码后不再等待 QR 目标，按配置前往 `post_qr_pose` 和 `entry_pose`。
|
||||||
|
- 进入对应方向路线，按 `clockwise_waypoints` 或 `counterclockwise_waypoints` 执行。
|
||||||
|
- 到第 `vlm_waypoint_number` 个路线点时触发 VLM。
|
||||||
|
- home 前一个点到达后发布 `/sign4return=10` 调参，再发布 home goal。
|
||||||
|
- 最后返回方向对应的 `clockwise_home_pose` 或 `counterclockwise_home_pose`。
|
||||||
|
- 使用的 Nav2 actions：
|
||||||
|
- `/navigate_to_pose`
|
||||||
|
- `/compute_path_through_poses`
|
||||||
|
- `/follow_path`
|
||||||
|
- 常用启动参数：
|
||||||
|
```bash
|
||||||
|
ros2 launch racing_control racing_control.launch.py auto_start:=false
|
||||||
|
ros2 launch racing_control racing_control.launch.py auto_start:=true
|
||||||
|
ros2 launch racing_control racing_control.launch.py enable_vlm_image_relay:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
### 点位与路线参数
|
||||||
|
|
||||||
|
- 点位数组使用 `[x, y, yaw_radians]`，坐标系由 `frame_id` 指定。
|
||||||
|
- `racing_control.yaml` 中当前关键字段：
|
||||||
|
- `qr_pose`：二维码区域导航点。
|
||||||
|
- `post_qr_pose`：二维码识别后的后置过渡点，用于后续调参。
|
||||||
|
- `entry_pose`：进入正式路线前的入口点。
|
||||||
|
- `clockwise_waypoints`：顺时针路线点，不包含 home。
|
||||||
|
- `counterclockwise_waypoints`：逆时针路线点，不包含 home。
|
||||||
|
- `clockwise_home_pose`、`counterclockwise_home_pose`：最终 home 点。
|
||||||
|
- `use_post_qr_pose: true` 时启用 QR 后置点；设为 `false` 时二维码识别后直接去 `entry_pose`。
|
||||||
|
- `vlm_waypoint_number: 4` 表示正式路线第 4 个点是 VLM 拍摄点。不要依赖 `goal_004` 之类的点名。
|
||||||
|
- 二维码方向解析：
|
||||||
|
- 文本包含 `顺` 或奇数数字时选择顺时针。
|
||||||
|
- 文本包含 `逆` 或偶数数字时选择逆时针。
|
||||||
|
|
||||||
|
### Nav2 与轨迹保护
|
||||||
|
|
||||||
|
- 包路径：`src/navigation/obstacle_nav2`
|
||||||
|
- 主启动：`launch/obstacle_nav2.launch.py`
|
||||||
|
- 轨迹保护启动：`launch/trajectory_guard.launch.py`
|
||||||
|
- 运行时参数档位切换：`launch/nav2_profile_tuner.launch.py`
|
||||||
|
- 关键配置：
|
||||||
|
- `config/nav2_profile_10.yaml`：普通/默认导航参数。
|
||||||
|
- `config/nav2_profile_11.yaml`：任务二路线参数。
|
||||||
|
- `config/trajectory_guard.yaml`：轨迹保护参数。
|
||||||
|
- `trajectory_guard_node` 订阅 `/trajectory_guard/input_path`，属于备用链路；默认比赛流程不依赖它。
|
||||||
|
- `nav2_profile_tuner` 监听 `/sign4return`，收到 10 或 11 后调用 Nav2 参数服务切换参数档位。
|
||||||
|
- `obstacle_nav2.launch.py` 当前以里程计坐标为主，默认 global frame 是 `odom`；静态地图和 AMCL 相关配置留作备用。
|
||||||
|
|
||||||
|
### 相机与二维码
|
||||||
|
|
||||||
|
- 相机包：`src/car_usb_cam`
|
||||||
|
- 二维码包：`src/qr_detection`
|
||||||
|
- 常见相机话题：`/image`
|
||||||
|
- 当前 VLM/调度链路期望的图像类型：`sensor_msgs/msg/CompressedImage`
|
||||||
|
- 二维码结果话题：`/qr_results`，类型 `std_msgs/msg/String`
|
||||||
|
- 二维码检测受 `/sign4return` 控制：
|
||||||
|
- `0`：启用二维码检测。
|
||||||
|
- `5`：关闭二维码检测。
|
||||||
|
- 常用检查命令：
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /qr_results
|
||||||
|
ros2 topic info /image
|
||||||
|
ros2 topic hz /image
|
||||||
|
```
|
||||||
|
- 如果 `racing_control` 到二维码区域后不继续走，优先检查：
|
||||||
|
- 日志里是否出现 `QR result received`。
|
||||||
|
- `/qr_results` 是否真的有输出。
|
||||||
|
- 输出文本是否包含 `顺`、`逆` 或可解析数字。
|
||||||
|
|
||||||
|
### VLM 与语音
|
||||||
|
|
||||||
|
- 包路径：`src/vlm_detect`
|
||||||
|
- 启动文件：
|
||||||
|
- `launch/vlm_detect.launch.py`：OpenAI 兼容 VLM 服务流程。
|
||||||
|
- `launch/local_vlm_adapter.launch.py`：本地 `hobot_llamacpp` 适配流程。
|
||||||
|
- VLM 触发信号：`/sign4return=9`
|
||||||
|
- VLM 结果话题：`/vlm_result`
|
||||||
|
- TTS 服务：`/tts/speak`，类型 `origincar_msg/srv/Speak`
|
||||||
|
- `vlm_detect` 的图像输入可通过 launch 参数 `image_topic` 配置。
|
||||||
|
- `racing_control` 可选单帧 VLM 图像转发：
|
||||||
|
- `enable_vlm_image_relay: false` 默认关闭。
|
||||||
|
- 输入话题：`vlm_image_input_topic`，默认 `/image`。
|
||||||
|
- 输出话题：`vlm_image_output_topic`，默认 `/vlm_image`。
|
||||||
|
- 启用后，`racing_control` 在发布 `/sign4return=9` 前，把缓存到的一帧压缩图像发布到 `/vlm_image`。
|
||||||
|
- 若要让 VLM 使用该帧，VLM 需这样启动：
|
||||||
|
```bash
|
||||||
|
ros2 launch vlm_detect vlm_detect.launch.py image_topic:=/vlm_image
|
||||||
|
```
|
||||||
|
- 当前 VLM 拍摄模式：
|
||||||
|
- `vlm_capture_mode: stop`：到 VLM 点停住，发布图像/触发信号，等待 `vlm_capture_wait_sec` 后继续。
|
||||||
|
- `vlm_capture_mode: pass_through`：保留的经过拍照模式，按 `pass_through_vlm_trigger_radius` 在接近 VLM 点时触发。
|
||||||
|
|
||||||
|
### 底盘、雷达、里程计与消息
|
||||||
|
|
||||||
|
- 底盘包：`src/origincar_base`
|
||||||
|
- 常见启动：`origincar_bringup.launch.py`、`base_serial.launch.py`、`ekf.launch.py`
|
||||||
|
- 负责底盘串口、IMU、里程计、EKF 和 TF 等基础能力。
|
||||||
|
- 雷达驱动路径：`src/LSLIDAR_X_ROS2-20240228/src`
|
||||||
|
- 障碍物检测包：`src/obstacle_scanner`
|
||||||
|
- 消费雷达数据，发布导航需要的障碍物信息。
|
||||||
|
- 消息/服务包：`src/origincar_msg`
|
||||||
|
- 包含 `origincar_msg/srv/Speak`。
|
||||||
|
- 当前 `racing_control` 期望里程计话题是 `/odom_combined`。
|
||||||
|
|
||||||
|
### 启动编排
|
||||||
|
|
||||||
|
- `src/my_robot_bringup/launch/master_launch.py` 是较完整的分阶段总启动，涉及底盘、雷达、TTS、相机、二维码、障碍物、规划和 VLM。
|
||||||
|
- 当前 `racing_control` 流程会直接依赖 Nav2 actions，并结合 `obstacle_nav2`/轨迹保护相关节点。修改 `master_launch.py` 前，应确认现场实际是通过总 launch 还是多个终端分别启动。
|
||||||
|
- `planner` 包里还有较旧或备用的 Hybrid A*、Pure Pursuit 等流程。除非当前 launch 明确使用它，否则先把它当作历史/备用路径看待。
|
||||||
|
|
||||||
|
## 共享信号与话题
|
||||||
|
|
||||||
|
- `/sign4return` 类型是 `std_msgs/msg/Int32`，被多个模块共享：
|
||||||
|
- `0`：启用二维码检测。
|
||||||
|
- `5`：关闭二维码检测。
|
||||||
|
- `9`：触发 VLM 拍摄/推理。
|
||||||
|
- `10`：应用普通 Nav2 参数档。
|
||||||
|
- `11`：应用任务二 Nav2 参数档。
|
||||||
|
- 新增 `/sign4return` 数值前，必须检查所有订阅者，包括底盘、感知和导航参数切换节点。
|
||||||
|
- 其他关键接口：
|
||||||
|
- `/qr_results`：二维码文本结果。
|
||||||
|
- `/vlm_result`：VLM 文本结果。
|
||||||
|
- `/tts/speak`：TTS 服务。
|
||||||
|
- `/image`：压缩相机流。
|
||||||
|
- `/vlm_image`：可选单帧 VLM 图像转发输出。
|
||||||
|
- `/trajectory_guard/input_path`：轨迹保护输入路径。
|
||||||
|
- `/cmd_vel`：真实运动控制通道，避免多个节点同时发布。
|
||||||
|
|
||||||
|
## 调试注意事项
|
||||||
|
|
||||||
|
- 不要让多个节点同时发布 `/cmd_vel`，除非明确 remap 掉其中一个。
|
||||||
|
- 二维码区域后不继续走时：
|
||||||
|
- 看 `racing_control` 日志是否出现 `QR result received`。
|
||||||
|
- 直接 echo `/qr_results`。
|
||||||
|
- 确认二维码文本能被解析为顺/逆方向。
|
||||||
|
- VLM 报 `No image` 或没有结果时：
|
||||||
|
- 检查 `ros2 topic info /image`。
|
||||||
|
- 如果启用转发，确认 `enable_vlm_image_relay:=true`，并且 VLM 使用 `image_topic:=/vlm_image`。
|
||||||
|
- 确认相机实际发布的是 `sensor_msgs/msg/CompressedImage`。
|
||||||
|
- 路线段末端卡住时：
|
||||||
|
- 检查 `/odom_combined` 是否连续。
|
||||||
|
- 检查 `circle_goal_tolerance` 是否过小。
|
||||||
|
- 如果临时启用了 `trajectory_guard`，再检查它是否正常向 `/follow_path` 转发。
|
||||||
|
- Nav2 在任务二前后行为差异大时：
|
||||||
|
- 查看 `nav2_profile_10.yaml` 和 `nav2_profile_11.yaml`。
|
||||||
|
- 查看 `nav2_profile_tuner` 日志里是否收到并应用 `/sign4return` 10/11。
|
||||||
|
- launch 已启动但按空格不能开始比赛时，可能是 stdin 不是 TTY；可使用 `auto_start:=true`。
|
||||||
|
- 更新现场点位时，优先修改 `src/racing_control/config/racing_control.yaml`，然后重新 build/install `racing_control`，确保 launch 读到安装后的配置。
|
||||||
|
|
||||||
|
## 关键参数清单
|
||||||
|
|
||||||
|
- `frame_id`：比赛点位所属坐标系。
|
||||||
|
- `qr_pose`、`post_qr_pose`、`entry_pose`：二维码阶段到正式路线阶段的关键过渡点。
|
||||||
|
- `clockwise_waypoints`、`counterclockwise_waypoints`：顺/逆时针路线点。
|
||||||
|
- `clockwise_home_pose`、`counterclockwise_home_pose`：顺/逆方向 home 点。
|
||||||
|
- `use_post_qr_pose`：是否启用二维码后置点。
|
||||||
|
- `vlm_waypoint_number`：第几个正式路线点触发 VLM。
|
||||||
|
- `vlm_capture_mode`：`stop` 或 `pass_through`。
|
||||||
|
- `vlm_capture_wait_sec`：停住拍照模式下触发 VLM 后等待时间。
|
||||||
|
- `enable_vlm_image_relay`：是否由 `racing_control` 把 `/image` 的单帧图像转发到 `/vlm_image`。
|
||||||
|
- `vlm_image_input_topic`、`vlm_image_output_topic`：VLM 图像转发输入/输出话题。
|
||||||
|
|
||||||
|
## 待后续补齐
|
||||||
|
|
||||||
|
- 比赛日标准启动顺序：到底使用 `my_robot_bringup/master_launch.py`，还是分终端启动 `obstacle_nav2`、`vlm_detect`、`racing_control` 等节点。
|
||||||
|
- 当前最权威的 Nav2 参数归属：`obstacle_nav2` 的 profile 看起来是比赛主路径，但 `planner` 和 `my_robot_bringup` 仍保留旧流程。
|
||||||
|
- 不同部署模式下相机话题的最终类型。近期 `vlm_detect` 期望 `CompressedImage`，换相机驱动前要重新确认。
|
||||||
|
- `/sign4return` 除 0、5、9、10、11 外，在底盘板和感知节点里的完整含义。
|
||||||
|
- 现场最新点位来源、采集时间和对应 JSON/YAML 转换记录。
|
||||||
@@ -15,7 +15,7 @@
|
|||||||
use_gps_ts: false #雷达是否使用GPS授时
|
use_gps_ts: false #雷达是否使用GPS授时
|
||||||
scan_topic: /scan #设置激光数据topic名称
|
scan_topic: /scan #设置激光数据topic名称
|
||||||
interface_selection: serial #接口选择:net 为网口,serial 为串口。
|
interface_selection: serial #接口选择:net 为网口,serial 为串口。
|
||||||
serial_port_: /dev/ttyCH343USB0 #串口连接时的串口号
|
serial_port_: /dev/radar #串口连接时的串口号
|
||||||
high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。
|
high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。
|
||||||
compensation: false #M10系列是否使用角度补偿功能
|
compensation: false #M10系列是否使用角度补偿功能
|
||||||
pubScan: true #是否发布scan话题
|
pubScan: true #是否发布scan话题
|
||||||
|
|||||||
65
src/data_collection_tools/x5_udp_cmd_bridge.py
Normal file
65
src/data_collection_tools/x5_udp_cmd_bridge.py
Normal file
@@ -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()
|
||||||
429
src/gc/项目总结_yiliao_ws.md
Normal file
429
src/gc/项目总结_yiliao_ws.md
Normal file
@@ -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 <package_name> --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 内容） |
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 20 KiB After Width: | Height: | Size: 8.6 KiB |
@@ -107,6 +107,13 @@ install(TARGETS
|
|||||||
install(DIRECTORY include/ DESTINATION include)
|
install(DIRECTORY include/ DESTINATION include)
|
||||||
install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME})
|
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)
|
if(BUILD_TESTING)
|
||||||
find_package(ament_cmake_gtest REQUIRED)
|
find_package(ament_cmake_gtest REQUIRED)
|
||||||
ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp
|
ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp
|
||||||
|
|||||||
@@ -0,0 +1,41 @@
|
|||||||
|
<!--
|
||||||
|
Ackermann NavigateThroughPoses behavior tree.
|
||||||
|
|
||||||
|
Planning failure is retried once after the global costmap has been cleared
|
||||||
|
and given one update period. FollowPath is not replanned periodically while
|
||||||
|
it is RUNNING. A controller failure gets one local-costmap retry, then one
|
||||||
|
short backup followed by one final through-poses plan and follow attempt.
|
||||||
|
Any final failure aborts the navigation action.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<Sequence name="NavigateThroughPosesOnce">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
|
||||||
|
<ReactiveSequence>
|
||||||
|
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
|
||||||
|
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</ReactiveSequence>
|
||||||
|
<Sequence name="ClearGlobalAndWait">
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
<Wait wait_duration="1"/>
|
||||||
|
</Sequence>
|
||||||
|
</RecoveryNode>
|
||||||
|
|
||||||
|
<Fallback name="FollowOrSingleBackupReplan">
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowWithLocalClear">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
|
||||||
|
<Sequence name="BackupReplanAndFinalFollow">
|
||||||
|
<BackUp backup_dist="0.04" backup_speed="0.20"/>
|
||||||
|
<ReactiveSequence>
|
||||||
|
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
|
||||||
|
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</ReactiveSequence>
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
</Sequence>
|
||||||
|
</Fallback>
|
||||||
|
</Sequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -1,33 +1,31 @@
|
|||||||
<!--
|
<!--
|
||||||
阿克曼底盘行为树：无原地旋转（Spin）
|
Ackermann NavigateToPose behavior tree.
|
||||||
恢复行为序列：清除代价地图 → 后退 → 等待
|
|
||||||
|
Compute the path once and keep the same path while FollowPath is RUNNING.
|
||||||
|
If following fails, clear the local costmap and retry the same path once.
|
||||||
|
If that also fails, back up 0.04 m, compute one final path, and follow it
|
||||||
|
once. Any failure in the final branch ends the navigation action.
|
||||||
-->
|
-->
|
||||||
<root main_tree_to_execute="MainTree">
|
<root main_tree_to_execute="MainTree">
|
||||||
<BehaviorTree ID="MainTree">
|
<BehaviorTree ID="MainTree">
|
||||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
<Sequence name="NavigateWithSingleBackupReplan">
|
||||||
<PipelineSequence name="NavigateWithReplanning">
|
<RecoveryNode number_of_retries="1" name="ComputeInitialPath">
|
||||||
<RateController hz="1.0">
|
|
||||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
|
||||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
</RecoveryNode>
|
</RecoveryNode>
|
||||||
</RateController>
|
|
||||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
<Fallback name="FollowOrSingleBackupReplan">
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowWithLocalClear">
|
||||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
</RecoveryNode>
|
</RecoveryNode>
|
||||||
</PipelineSequence>
|
|
||||||
<ReactiveFallback name="RecoveryFallback">
|
<Sequence name="BackupReplanAndFinalFollow">
|
||||||
<GoalUpdated/>
|
<BackUp backup_dist="0.04" backup_speed="0.20"/>
|
||||||
<RoundRobin name="RecoveryActions">
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
<Sequence name="ClearingActions">
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
</Sequence>
|
||||||
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
</Fallback>
|
||||||
</Sequence>
|
</Sequence>
|
||||||
<BackUp backup_dist="0.80" backup_speed="0.18"/>
|
|
||||||
<Wait wait_duration="0.3"/>
|
|
||||||
</RoundRobin>
|
|
||||||
</ReactiveFallback>
|
|
||||||
</RecoveryNode>
|
|
||||||
</BehaviorTree>
|
</BehaviorTree>
|
||||||
</root>
|
</root>
|
||||||
|
|||||||
@@ -0,0 +1,33 @@
|
|||||||
|
<!--
|
||||||
|
阿克曼底盘行为树：无原地旋转（Spin）
|
||||||
|
恢复行为序列：清除代价地图 → 后退 → 等待
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="1.0">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</RateController>
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</PipelineSequence>
|
||||||
|
<ReactiveFallback name="RecoveryFallback">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<RoundRobin name="RecoveryActions">
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</Sequence>
|
||||||
|
<BackUp backup_dist="0.80" backup_speed="0.18"/>
|
||||||
|
<Wait wait_duration="0.3"/>
|
||||||
|
</RoundRobin>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,35 @@
|
|||||||
|
<!--
|
||||||
|
Ackermann NavigateToPose behavior tree.
|
||||||
|
|
||||||
|
Compute the path once and keep the same path while FollowPath is RUNNING.
|
||||||
|
A new global plan is requested only after FollowPath returns FAILURE and the
|
||||||
|
outer RecoveryNode starts another navigation attempt. This avoids replacing
|
||||||
|
a valid path every second while MPPI is still evaluating it.
|
||||||
|
|
||||||
|
Recovery actions intentionally do not include Spin or BackUp because this
|
||||||
|
robot uses an Ackermann drive model.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<Sequence name="NavigateOnce">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</Sequence>
|
||||||
|
<ReactiveFallback name="RecoveryFallback">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
<Wait wait_duration="1"/>
|
||||||
|
</Sequence>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -6,12 +6,13 @@
|
|||||||
# global_frame leaf when a different connected odometry frame is required.
|
# global_frame leaf when a different connected odometry frame is required.
|
||||||
# Global planner: Smac Hybrid A* (Reeds-Shepp)
|
# Global planner: Smac Hybrid A* (Reeds-Shepp)
|
||||||
# Local controller: MPPI (Ackermann)
|
# Local controller: MPPI (Ackermann)
|
||||||
|
# 速度快但是雷达容易掉
|
||||||
# ============================================================================
|
# ============================================================================
|
||||||
|
|
||||||
bt_navigator:
|
bt_navigator:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
use_sim_time: False
|
use_sim_time: False
|
||||||
global_frame: odom
|
global_frame: map
|
||||||
robot_base_frame: base_footprint
|
robot_base_frame: base_footprint
|
||||||
odom_topic: /odom_combined
|
odom_topic: /odom_combined
|
||||||
bt_loop_duration: 50
|
bt_loop_duration: 50
|
||||||
@@ -70,16 +71,16 @@ bt_navigator_rclcpp_node:
|
|||||||
controller_server:
|
controller_server:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
use_sim_time: False
|
use_sim_time: False
|
||||||
controller_frequency: 20.0
|
controller_frequency: 15.0
|
||||||
FollowPath:
|
FollowPath:
|
||||||
plugin: "nav2_mppi_controller::MPPIController"
|
plugin: "nav2_mppi_controller::MPPIController"
|
||||||
time_steps: 36
|
time_steps: 40
|
||||||
model_dt: 0.05
|
model_dt: 0.06666666666666666
|
||||||
batch_size: 1000
|
batch_size: 900
|
||||||
vx_std: 0.75
|
vx_std: 0.25
|
||||||
vy_std: 0.0
|
vy_std: 0.0
|
||||||
wz_std: 0.4
|
wz_std: 0.45
|
||||||
vx_max: 1.75
|
vx_max: 1.00
|
||||||
vx_min: -0.75
|
vx_min: -0.75
|
||||||
vy_max: 0.0
|
vy_max: 0.0
|
||||||
wz_max: 1.5
|
wz_max: 1.5
|
||||||
@@ -101,7 +102,7 @@ controller_server:
|
|||||||
GoalCritic:
|
GoalCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 5.0
|
cost_weight: 6.0
|
||||||
threshold_to_consider: 1.4
|
threshold_to_consider: 1.4
|
||||||
GoalAngleCritic:
|
GoalAngleCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
@@ -111,21 +112,21 @@ controller_server:
|
|||||||
PreferForwardCritic:
|
PreferForwardCritic:
|
||||||
enabled: false
|
enabled: false
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 0.0
|
cost_weight: 7.0
|
||||||
threshold_to_consider: 0.5
|
threshold_to_consider: 0.5
|
||||||
CostCritic:
|
CostCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 3.81
|
cost_weight: 5.0
|
||||||
critical_cost: 300.0
|
critical_cost: 300.0
|
||||||
consider_footprint: true
|
consider_footprint: true
|
||||||
collision_cost: 1000000.0
|
collision_cost: 100000.0
|
||||||
near_goal_distance: 1.0
|
near_goal_distance: 1.0
|
||||||
trajectory_point_step: 2
|
trajectory_point_step: 2
|
||||||
PathAlignCritic:
|
PathAlignCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 14.0
|
cost_weight: 8.0
|
||||||
max_path_occupancy_ratio: 0.05
|
max_path_occupancy_ratio: 0.05
|
||||||
trajectory_point_step: 4
|
trajectory_point_step: 4
|
||||||
threshold_to_consider: 0.5
|
threshold_to_consider: 0.5
|
||||||
@@ -135,13 +136,13 @@ controller_server:
|
|||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 5.0
|
cost_weight: 5.0
|
||||||
offset_from_furthest: 5
|
offset_from_furthest: 10
|
||||||
threshold_to_consider: 1.4
|
threshold_to_consider: 1.4
|
||||||
PathAngleCritic:
|
PathAngleCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 2.0
|
cost_weight: 2.0
|
||||||
offset_from_furthest: 4
|
offset_from_furthest: 5
|
||||||
threshold_to_consider: 0.5
|
threshold_to_consider: 0.5
|
||||||
max_angle_to_furthest: 1.0
|
max_angle_to_furthest: 1.0
|
||||||
forward_preference: false
|
forward_preference: false
|
||||||
@@ -180,7 +181,7 @@ local_costmap:
|
|||||||
inflation_layer:
|
inflation_layer:
|
||||||
plugin: "nav2_costmap_2d::InflationLayer"
|
plugin: "nav2_costmap_2d::InflationLayer"
|
||||||
cost_scaling_factor: 3.0
|
cost_scaling_factor: 3.0
|
||||||
inflation_radius: 0.55
|
inflation_radius: 0.20
|
||||||
always_send_full_costmap: True
|
always_send_full_costmap: True
|
||||||
local_costmap_client:
|
local_costmap_client:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
@@ -195,17 +196,22 @@ global_costmap:
|
|||||||
update_frequency: 1.0
|
update_frequency: 1.0
|
||||||
publish_frequency: 1.0
|
publish_frequency: 1.0
|
||||||
transform_tolerance: 0.5
|
transform_tolerance: 0.5
|
||||||
global_frame: odom
|
global_frame: map
|
||||||
robot_base_frame: base_footprint
|
robot_base_frame: base_footprint
|
||||||
use_sim_time: False
|
use_sim_time: False
|
||||||
rolling_window: true
|
rolling_window: false
|
||||||
width: 10
|
width: 8
|
||||||
height: 10
|
height: 8
|
||||||
resolution: 0.05
|
resolution: 0.05
|
||||||
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
|
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
|
||||||
footprint_padding: 0.02
|
footprint_padding: 0.02
|
||||||
track_unknown_space: false
|
track_unknown_space: true
|
||||||
plugins: ["obstacle_array_layer", "inflation_layer"]
|
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:
|
obstacle_array_layer:
|
||||||
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
||||||
enabled: true
|
enabled: true
|
||||||
@@ -218,8 +224,8 @@ global_costmap:
|
|||||||
extra_inflation: 0.02
|
extra_inflation: 0.02
|
||||||
inflation_layer:
|
inflation_layer:
|
||||||
plugin: "nav2_costmap_2d::InflationLayer"
|
plugin: "nav2_costmap_2d::InflationLayer"
|
||||||
cost_scaling_factor: 3.0
|
cost_scaling_factor: 2.0
|
||||||
inflation_radius: 0.55
|
inflation_radius: 0.35
|
||||||
always_send_full_costmap: True
|
always_send_full_costmap: True
|
||||||
global_costmap_client:
|
global_costmap_client:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
@@ -236,20 +242,20 @@ planner_server:
|
|||||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||||
downsample_costmap: false
|
downsample_costmap: false
|
||||||
downsampling_factor: 1
|
downsampling_factor: 1
|
||||||
tolerance: 0.25
|
tolerance: 0.15
|
||||||
allow_unknown: false
|
allow_unknown: false
|
||||||
max_iterations: 1000000
|
max_iterations: 1000000
|
||||||
max_on_approach_iterations: 1000
|
max_on_approach_iterations: 1000
|
||||||
max_planning_time: 5.0
|
max_planning_time: 25.0
|
||||||
motion_model_for_search: "REEDS_SHEPP"
|
motion_model_for_search: "REEDS_SHEPP"
|
||||||
angle_quantization_bins: 72
|
angle_quantization_bins: 72
|
||||||
analytic_expansion_ratio: 3.5
|
analytic_expansion_ratio: 3.5
|
||||||
analytic_expansion_max_length: 3.0
|
analytic_expansion_max_length: 3.0
|
||||||
minimum_turning_radius: 0.40
|
minimum_turning_radius: 0.40
|
||||||
reverse_penalty: 1.0
|
reverse_penalty: 1.6
|
||||||
change_penalty: 0.0
|
change_penalty: 2.0
|
||||||
non_straight_penalty: 1.2
|
non_straight_penalty: 1.2
|
||||||
cost_penalty: 2.0
|
cost_penalty: 4.0
|
||||||
retrospective_penalty: 0.015
|
retrospective_penalty: 0.015
|
||||||
# 5 m covers the rolling planning horizon without the startup and memory
|
# 5 m covers the rolling planning horizon without the startup and memory
|
||||||
# cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
|
# cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
|
||||||
@@ -258,7 +264,7 @@ planner_server:
|
|||||||
viz_expansions: false
|
viz_expansions: false
|
||||||
smooth_path: True
|
smooth_path: True
|
||||||
smoother:
|
smoother:
|
||||||
max_iterations: 1000
|
max_iterations: 700
|
||||||
w_smooth: 0.3
|
w_smooth: 0.3
|
||||||
w_data: 0.2
|
w_data: 0.2
|
||||||
tolerance: 1.0e-10
|
tolerance: 1.0e-10
|
||||||
@@ -294,7 +300,7 @@ behavior_server:
|
|||||||
wait:
|
wait:
|
||||||
plugin: "nav2_behaviors/Wait"
|
plugin: "nav2_behaviors/Wait"
|
||||||
wait_duration: 0.5
|
wait_duration: 0.5
|
||||||
global_frame: odom
|
global_frame: map
|
||||||
robot_base_frame: base_footprint
|
robot_base_frame: base_footprint
|
||||||
transform_tolerance: 0.5
|
transform_tolerance: 0.5
|
||||||
use_sim_time: False
|
use_sim_time: False
|
||||||
@@ -324,10 +330,10 @@ velocity_smoother:
|
|||||||
smoothing_frequency: 20.0
|
smoothing_frequency: 20.0
|
||||||
scale_velocities: False
|
scale_velocities: False
|
||||||
feedback: "OPEN_LOOP"
|
feedback: "OPEN_LOOP"
|
||||||
max_velocity: [1.75, 0.0, 1.5]
|
max_velocity: [1.00, 0.0, 2.0]
|
||||||
min_velocity: [-0.75, 0.0, -1.5]
|
min_velocity: [-0.75, 0.0, -2.0]
|
||||||
max_accel: [2.5, 0.0, 3.2]
|
max_accel: [3.73, 0.0, 3.2]
|
||||||
max_decel: [-2.5, 0.0, -3.2]
|
max_decel: [-1.1, 0.0, -4.5]
|
||||||
odom_topic: /odom_combined
|
odom_topic: /odom_combined
|
||||||
odom_duration: 0.1
|
odom_duration: 0.1
|
||||||
deadband_velocity: [0.03, 0.0, 0.03]
|
deadband_velocity: [0.03, 0.0, 0.03]
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -12,7 +12,7 @@
|
|||||||
bt_navigator:
|
bt_navigator:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
use_sim_time: False
|
use_sim_time: False
|
||||||
global_frame: odom
|
global_frame: map
|
||||||
robot_base_frame: base_footprint
|
robot_base_frame: base_footprint
|
||||||
odom_topic: /odom_combined
|
odom_topic: /odom_combined
|
||||||
bt_loop_duration: 50
|
bt_loop_duration: 50
|
||||||
@@ -213,17 +213,22 @@ global_costmap:
|
|||||||
update_frequency: 5.0
|
update_frequency: 5.0
|
||||||
publish_frequency: 3.0
|
publish_frequency: 3.0
|
||||||
transform_tolerance: 0.5
|
transform_tolerance: 0.5
|
||||||
global_frame: odom
|
global_frame: map
|
||||||
robot_base_frame: base_footprint
|
robot_base_frame: base_footprint
|
||||||
use_sim_time: False
|
use_sim_time: False
|
||||||
rolling_window: true
|
rolling_window: false
|
||||||
width: 10
|
width: 8
|
||||||
height: 10
|
height: 8
|
||||||
resolution: 0.05
|
resolution: 0.05
|
||||||
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
|
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
|
||||||
footprint_padding: 0.02
|
footprint_padding: 0.02
|
||||||
track_unknown_space: false
|
track_unknown_space: true
|
||||||
plugins: ["obstacle_array_layer", "inflation_layer"]
|
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:
|
obstacle_array_layer:
|
||||||
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
||||||
enabled: true
|
enabled: true
|
||||||
@@ -311,7 +316,7 @@ behavior_server:
|
|||||||
wait:
|
wait:
|
||||||
plugin: "nav2_behaviors/Wait"
|
plugin: "nav2_behaviors/Wait"
|
||||||
wait_duration: 0.5
|
wait_duration: 0.5
|
||||||
global_frame: odom
|
global_frame: map
|
||||||
robot_base_frame: base_footprint
|
robot_base_frame: base_footprint
|
||||||
transform_tolerance: 0.5
|
transform_tolerance: 0.5
|
||||||
use_sim_time: False
|
use_sim_time: False
|
||||||
|
|||||||
@@ -22,14 +22,15 @@ from ament_index_python.packages import get_package_share_directory
|
|||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import (
|
from launch.actions import (
|
||||||
DeclareLaunchArgument,
|
DeclareLaunchArgument,
|
||||||
|
ExecuteProcess,
|
||||||
GroupAction,
|
GroupAction,
|
||||||
IncludeLaunchDescription,
|
IncludeLaunchDescription,
|
||||||
SetEnvironmentVariable,
|
SetEnvironmentVariable,
|
||||||
)
|
)
|
||||||
from launch.conditions import IfCondition
|
from launch.conditions import IfCondition
|
||||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
from launch.substitutions import LaunchConfiguration, PythonExpression
|
from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression
|
||||||
from launch_ros.actions import SetRemap
|
from launch_ros.actions import Node, SetRemap
|
||||||
from nav2_common.launch import RewrittenYaml
|
from nav2_common.launch import RewrittenYaml
|
||||||
|
|
||||||
|
|
||||||
@@ -48,17 +49,20 @@ def generate_launch_description():
|
|||||||
start_base = LaunchConfiguration('start_base', default='true')
|
start_base = LaunchConfiguration('start_base', default='true')
|
||||||
start_lidar = LaunchConfiguration('start_lidar', default='true')
|
start_lidar = LaunchConfiguration('start_lidar', default='true')
|
||||||
start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', 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')
|
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(
|
nav_to_pose_bt_path = os.path.join(
|
||||||
pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
|
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(
|
configured_params = RewrittenYaml(
|
||||||
source_file=nav2_param_path,
|
source_file=nav2_param_path,
|
||||||
root_key='',
|
root_key='',
|
||||||
param_rewrites={
|
param_rewrites={
|
||||||
'global_frame': global_frame,
|
|
||||||
'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
|
'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,
|
convert_types=True,
|
||||||
)
|
)
|
||||||
@@ -95,6 +99,37 @@ def generate_launch_description():
|
|||||||
condition=IfCondition(start_obstacle_scanner),
|
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 ===============================
|
# ========================== Nav2 Navigation ===============================
|
||||||
navigation_launch = IncludeLaunchDescription(
|
navigation_launch = IncludeLaunchDescription(
|
||||||
PythonLaunchDescriptionSource(
|
PythonLaunchDescriptionSource(
|
||||||
@@ -140,13 +175,20 @@ def generate_launch_description():
|
|||||||
'start_obstacle_scanner',
|
'start_obstacle_scanner',
|
||||||
default_value='true',
|
default_value='true',
|
||||||
description='Also start the obstacle_scanner node'),
|
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,
|
safe_base_bringup,
|
||||||
lslidar_launch,
|
lslidar_launch,
|
||||||
obstacle_scanner_launch,
|
obstacle_scanner_launch,
|
||||||
|
static_map_publisher,
|
||||||
|
initial_pose_to_tf,
|
||||||
navigation_launch,
|
navigation_launch,
|
||||||
])
|
])
|
||||||
|
|||||||
@@ -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,
|
||||||
|
])
|
||||||
116
src/navigation/obstacle_nav2/scripts/initial_pose_to_tf.py
Normal file
116
src/navigation/obstacle_nav2/scripts/initial_pose_to_tf.py
Normal file
@@ -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()
|
||||||
92
src/navigation/obstacle_nav2/scripts/static_map_publisher.py
Normal file
92
src/navigation/obstacle_nav2/scripts/static_map_publisher.py
Normal file
@@ -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()
|
||||||
@@ -25,6 +25,8 @@ public:
|
|||||||
bool finishFrame(uint64_t frame_id, TimePoint now = Clock::now());
|
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 cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now = Clock::now());
|
||||||
bool makeSummaryLineIfDue(TimePoint now, std::string *line);
|
bool makeSummaryLineIfDue(TimePoint now, std::string *line);
|
||||||
|
void setPrintToTerminal(bool enabled);
|
||||||
|
bool printToTerminal() const;
|
||||||
|
|
||||||
const std::string &logPath() const;
|
const std::string &logPath() const;
|
||||||
bool isOpen() const;
|
bool isOpen() const;
|
||||||
@@ -50,6 +52,7 @@ private:
|
|||||||
TimePoint last_summary_time_;
|
TimePoint last_summary_time_;
|
||||||
std::string log_path_;
|
std::string log_path_;
|
||||||
std::ofstream log_file_;
|
std::ofstream log_file_;
|
||||||
|
bool print_to_terminal_{false};
|
||||||
};
|
};
|
||||||
} // namespace origincar_base_logging
|
} // namespace origincar_base_logging
|
||||||
|
|
||||||
|
|||||||
@@ -237,6 +237,7 @@ private:
|
|||||||
string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
|
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_;
|
string scan_topic_, wall_config_path_, combined_odom_topic_;
|
||||||
bool publish_tf_;
|
bool publish_tf_;
|
||||||
|
bool scan_odom_timing_to_terminal_;
|
||||||
double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_;
|
double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_;
|
||||||
int wall_scan_stride_;
|
int wall_scan_stride_;
|
||||||
std::string cmd_vel;
|
std::string cmd_vel;
|
||||||
|
|||||||
@@ -13,6 +13,23 @@ struct WallKalmanNoise
|
|||||||
double theta;
|
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
|
class WallKalmanFilter
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
@@ -31,6 +48,11 @@ private:
|
|||||||
WallKalmanNoise process_noise_per_second_;
|
WallKalmanNoise process_noise_per_second_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose,
|
||||||
|
const Pose2D &scan_pose,
|
||||||
|
const LocalizationQuality &quality,
|
||||||
|
bool stationary);
|
||||||
|
|
||||||
} // namespace origincar_wall
|
} // namespace origincar_wall
|
||||||
|
|
||||||
#endif // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
|
#endif // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
|
||||||
|
|||||||
@@ -8,7 +8,7 @@ def generate_launch_description():
|
|||||||
'serial_baud_rate': 921600,
|
'serial_baud_rate': 921600,
|
||||||
'serial_read_timeout_ms': 20,
|
'serial_read_timeout_ms': 20,
|
||||||
'tx_period_ms': 20,
|
'tx_period_ms': 20,
|
||||||
'cmd_watchdog_timeout_ms': 150,
|
'cmd_watchdog_timeout_ms': 500,
|
||||||
'control_period_ms': 50,
|
'control_period_ms': 50,
|
||||||
'robot_frame_id': 'base_footprint',
|
'robot_frame_id': 'base_footprint',
|
||||||
'odom_frame_id': 'odom',
|
'odom_frame_id': 'odom',
|
||||||
|
|||||||
@@ -164,6 +164,16 @@ bool ScanOdomTimingLogger::makeSummaryLineIfDue(TimePoint now, std::string *line
|
|||||||
return true;
|
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
|
const std::string &ScanOdomTimingLogger::logPath() const
|
||||||
{
|
{
|
||||||
return log_path_;
|
return log_path_;
|
||||||
|
|||||||
@@ -307,6 +307,10 @@ void origincar_base::Apply_Wall_Update()
|
|||||||
|
|
||||||
void origincar_base::Print_Timing_Log_If_Due()
|
void origincar_base::Print_Timing_Log_If_Due()
|
||||||
{
|
{
|
||||||
|
if (!scan_odom_timing_to_terminal_)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
std::string timing_summary;
|
std::string timing_summary;
|
||||||
if (scan_odom_timing_logger_.makeSummaryLineIfDue(std::chrono::steady_clock::now(), &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<std::string>("scan_topic", "/scan");
|
this->declare_parameter<std::string>("scan_topic", "/scan");
|
||||||
this->declare_parameter<std::string>("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
|
this->declare_parameter<std::string>("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
|
||||||
this->declare_parameter<bool>("publish_tf", true);
|
this->declare_parameter<bool>("publish_tf", true);
|
||||||
|
this->declare_parameter<bool>("scan_odom_timing_to_terminal", false);
|
||||||
this->declare_parameter<int>("wall_scan_stride", 2);
|
this->declare_parameter<int>("wall_scan_stride", 2);
|
||||||
this->declare_parameter<double>("laser_x", 0.0);
|
this->declare_parameter<double>("laser_x", 0.0);
|
||||||
this->declare_parameter<double>("laser_y", 0.0);
|
this->declare_parameter<double>("laser_y", 0.0);
|
||||||
@@ -665,6 +670,7 @@ origincar_base::origincar_base()
|
|||||||
this->get_parameter("scan_topic", scan_topic_);
|
this->get_parameter("scan_topic", scan_topic_);
|
||||||
this->get_parameter("wall_config_path", wall_config_path_);
|
this->get_parameter("wall_config_path", wall_config_path_);
|
||||||
this->get_parameter("publish_tf", publish_tf_);
|
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_);
|
this->get_parameter("wall_scan_stride", wall_scan_stride_);
|
||||||
laser_pose_ = {
|
laser_pose_ = {
|
||||||
this->get_parameter("laser_x").as_double(),
|
this->get_parameter("laser_x").as_double(),
|
||||||
@@ -702,6 +708,7 @@ origincar_base::origincar_base()
|
|||||||
0.0,
|
0.0,
|
||||||
};
|
};
|
||||||
wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose);
|
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<float>(initial_pose.x);
|
Robot_Pos.X = static_cast<float>(initial_pose.x);
|
||||||
Robot_Pos.Y = static_cast<float>(initial_pose.y);
|
Robot_Pos.Y = static_cast<float>(initial_pose.y);
|
||||||
Robot_Pos.Z = static_cast<float>(initial_pose.theta);
|
Robot_Pos.Z = static_cast<float>(initial_pose.theta);
|
||||||
|
|||||||
@@ -6,6 +6,24 @@
|
|||||||
namespace origincar_wall
|
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)
|
WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose)
|
||||||
: pose_(initial_pose),
|
: pose_(initial_pose),
|
||||||
covariance_{0.10, 0.10, 0.10},
|
covariance_{0.10, 0.10, 0.10},
|
||||||
@@ -63,4 +81,75 @@ WallKalmanNoise WallKalmanFilter::covariance() const
|
|||||||
return covariance_;
|
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
|
} // namespace origincar_wall
|
||||||
|
|||||||
@@ -85,6 +85,15 @@ void testSummaryPrintIsThrottledToOneHz()
|
|||||||
require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1500), &summary),
|
require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1500), &summary),
|
||||||
"summary should be throttled after printing");
|
"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
|
} // namespace
|
||||||
|
|
||||||
int main()
|
int main()
|
||||||
@@ -93,6 +102,7 @@ int main()
|
|||||||
{
|
{
|
||||||
testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate();
|
testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate();
|
||||||
testSummaryPrintIsThrottledToOneHz();
|
testSummaryPrintIsThrottledToOneHz();
|
||||||
|
testTerminalOutputDefaultsToOff();
|
||||||
}
|
}
|
||||||
catch (const std::exception &error)
|
catch (const std::exception &error)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -60,6 +60,69 @@ void testYawCorrectionUsesWrappedInnovation()
|
|||||||
|
|
||||||
require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary");
|
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
|
} // namespace
|
||||||
|
|
||||||
int main()
|
int main()
|
||||||
@@ -69,6 +132,9 @@ int main()
|
|||||||
testPredictUsesWheelVelocityAndImuYawRate();
|
testPredictUsesWheelVelocityAndImuYawRate();
|
||||||
testScanUpdateTrustsLowNoiseMoreThanHighNoise();
|
testScanUpdateTrustsLowNoiseMoreThanHighNoise();
|
||||||
testYawCorrectionUsesWrappedInnovation();
|
testYawCorrectionUsesWrappedInnovation();
|
||||||
|
testScanPolicyGradesByWallCountAndMeanError();
|
||||||
|
testScanPolicyRejectsPoorOrJumpingMeasurements();
|
||||||
|
testScanPolicyDowngradesWhileStationary();
|
||||||
}
|
}
|
||||||
catch (const std::exception &error)
|
catch (const std::exception &error)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -3,7 +3,7 @@
|
|||||||
## 项目用途
|
## 项目用途
|
||||||
|
|
||||||
- 这是 RDKx5 赛车机器人的 ROS2 Humble 工作区。
|
- 这是 RDKx5 赛车机器人的 ROS2 Humble 工作区。
|
||||||
- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和轨迹保护节点，不在包内重新实现这些功能。
|
- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和备用轨迹保护节点，不在包内重新实现这些功能。
|
||||||
|
|
||||||
## 环境准备
|
## 环境准备
|
||||||
|
|
||||||
@@ -25,8 +25,9 @@
|
|||||||
- `/sign4return` 是共享的 `std_msgs/msg/Int32` 控制话题：
|
- `/sign4return` 是共享的 `std_msgs/msg/Int32` 控制话题：
|
||||||
`0` 开启二维码检测，`5` 关闭二维码检测，`9` 触发 VLM 拍照识别，`10` 切换普通 Nav2 参数，`11` 切换任务二 Nav2 参数。
|
`0` 开启二维码检测，`5` 关闭二维码检测，`9` 触发 VLM 拍照识别，`10` 切换普通 Nav2 参数，`11` 切换任务二 Nav2 参数。
|
||||||
- `obstacle_nav2/nav2_profile_tuner` 监听 `/sign4return`，并根据 10 或 11 应用对应的 Nav2 profile。
|
- `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`。
|
- `obstacle_nav2/trajectory_guard_node` 订阅 `/trajectory_guard/input_path`，属于备用链路；默认比赛流程不依赖它。
|
||||||
- 比赛总控使用 Nav2 actions（`/navigate_to_pose`、`/compute_path_through_poses`、`/follow_path`），比赛点位应保持为可通过 ROS 参数配置。
|
- 比赛总控使用 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`。
|
||||||
|
|
||||||
## 外部系统
|
## 外部系统
|
||||||
|
|
||||||
|
|||||||
@@ -3,8 +3,10 @@ racing_control:
|
|||||||
# Startup
|
# Startup
|
||||||
auto_start: false
|
auto_start: false
|
||||||
frame_id: odom
|
frame_id: odom
|
||||||
use_post_qr_pose: true
|
use_post_qr_pose: false
|
||||||
enable_vlm_image_relay: false
|
enable_vlm_image_relay: false
|
||||||
|
enable_dynamic_replanning: true
|
||||||
|
enable_recovery: true
|
||||||
vlm_image_input_topic: /image
|
vlm_image_input_topic: /image
|
||||||
vlm_image_output_topic: /vlm_image
|
vlm_image_output_topic: /vlm_image
|
||||||
|
|
||||||
@@ -13,6 +15,7 @@ racing_control:
|
|||||||
qr_result_topic: /qr_results
|
qr_result_topic: /qr_results
|
||||||
vlm_result_topic: /vlm_result
|
vlm_result_topic: /vlm_result
|
||||||
odom_topic: /odom_combined
|
odom_topic: /odom_combined
|
||||||
|
recovery_cmd_vel_topic: /cmd_vel
|
||||||
trajectory_guard_input_topic: /trajectory_guard/input_path
|
trajectory_guard_input_topic: /trajectory_guard/input_path
|
||||||
|
|
||||||
# Nav2 actions and plugin IDs
|
# Nav2 actions and plugin IDs
|
||||||
@@ -22,16 +25,23 @@ racing_control:
|
|||||||
planner_id: GridBased
|
planner_id: GridBased
|
||||||
controller_id: FollowPath
|
controller_id: FollowPath
|
||||||
goal_checker_id: ""
|
goal_checker_id: ""
|
||||||
use_trajectory_guard: true
|
use_trajectory_guard: false
|
||||||
|
|
||||||
# Timeouts and settling waits, in seconds
|
# Timeouts and settling waits, in seconds
|
||||||
navigation_timeout_sec: 120.0
|
navigation_timeout_sec: 120.0
|
||||||
path_planning_timeout_sec: 30.0
|
path_planning_timeout_sec: 30.0
|
||||||
circle_timeout_sec: 120.0
|
circle_timeout_sec: 120.0
|
||||||
qr_result_timeout_sec: 8.0
|
qr_result_timeout_sec: 20.0
|
||||||
profile_switch_wait_sec: 1.0
|
profile_switch_wait_sec: 1.0
|
||||||
post_qr_wait_sec: 1.0
|
post_qr_wait_sec: 1.0
|
||||||
vlm_capture_wait_sec: 0.5
|
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
|
circle_goal_tolerance: 0.30
|
||||||
|
|
||||||
# VLM capture:
|
# VLM capture:
|
||||||
@@ -48,61 +58,36 @@ racing_control:
|
|||||||
sign_profile_task2: 11
|
sign_profile_task2: 11
|
||||||
|
|
||||||
# Pose parameters are flat x/y/yaw-radians triples in frame_id.
|
# Pose parameters are flat x/y/yaw-radians triples in frame_id.
|
||||||
qr_pose: [4.3860322643582883, 1.2934897914487595, 0.94658891138326606]
|
qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392]
|
||||||
post_qr_pose: [4.4004663023808863, 0.46113350031559491, 1.7514152174101529]
|
post_qr_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
|
||||||
entry_pose: [2.499999919243812, 2.1932039710724882, 1.5174118916273021]
|
entry_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
|
||||||
|
|
||||||
# The route's Nth waypoint is used as the VLM capture point.
|
# 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
|
# For the current point set, the 2nd waypoint is the VLM point.
|
||||||
# and goal_004 for main_2.
|
vlm_waypoint_number: 2
|
||||||
vlm_waypoint_number: 4
|
|
||||||
|
|
||||||
# main_1.json: clockwise route, excluding home.
|
# main_1.json: clockwise route, excluding home.
|
||||||
clockwise_waypoints:
|
clockwise_waypoints:
|
||||||
- 1.542550031688728
|
- 0.7534956931109804
|
||||||
- 3.1698990676859604
|
- 3.766501234923627
|
||||||
- 3.1415926535897918
|
- 1.5592359900104606
|
||||||
- 0.71981664792046363
|
- 3.8567885105264077
|
||||||
- 3.7665012349236271
|
- 4.329424195458402
|
||||||
- 1.5495230091246759
|
- 0.03997856199797794
|
||||||
- 1.3164186536457545
|
- 2.5144339572664096
|
||||||
- 4.3294241954584018
|
- 2.4337692660037766
|
||||||
- -0.016392199787162172
|
- -1.6101462328054222
|
||||||
- 3.659525047016253
|
clockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
|
||||||
- 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]
|
|
||||||
|
|
||||||
# main_2.json: counterclockwise route, excluding home.
|
# main_2.json: counterclockwise route, excluding home.
|
||||||
counterclockwise_waypoints:
|
counterclockwise_waypoints:
|
||||||
- 3.3612239633974195
|
- 4.2128251001861265
|
||||||
- 3.1747105213684104
|
- 3.7376334819031842
|
||||||
- 0.010869470292991894
|
- 1.5450283923222128
|
||||||
- 4.2272591382087246
|
- 1.1720794040064113
|
||||||
- 3.7280108975630366
|
- 4.319801449605877
|
||||||
- 1.5707963267948966
|
- 3.1112984779040144
|
||||||
- 3.4237709231207547
|
- 2.4951887885861144
|
||||||
- 4.3486693641386971
|
- 2.318297930897253
|
||||||
- 3.123412893984987
|
- -1.539556591616333
|
||||||
- 1.2731168626027138
|
counterclockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
|
||||||
- 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]
|
|
||||||
|
|||||||
@@ -86,6 +86,60 @@ inline bool shouldPublishVlmImageFrame(
|
|||||||
return enable_vlm_image_relay && has_latest_image;
|
return enable_vlm_image_relay && has_latest_image;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
template<typename WaitForActionServerFn>
|
||||||
|
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(
|
inline geometry_msgs::msg::PoseStamped poseFromXYYaw(
|
||||||
const double x, const double y, const double yaw, const std::string & frame_id)
|
const double x, const double y, const double yaw, const std::string & frame_id)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -203,7 +203,7 @@ private:
|
|||||||
|
|
||||||
auto_start_ = declare_parameter<bool>("auto_start", false);
|
auto_start_ = declare_parameter<bool>("auto_start", false);
|
||||||
use_trajectory_guard_ = declare_parameter<bool>("use_trajectory_guard", true);
|
use_trajectory_guard_ = declare_parameter<bool>("use_trajectory_guard", true);
|
||||||
use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", true);
|
use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", false);
|
||||||
enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
|
enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
|
||||||
vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
|
vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
|
||||||
vlm_image_output_topic_ =
|
vlm_image_output_topic_ =
|
||||||
@@ -912,8 +912,8 @@ private:
|
|||||||
Stage stage_{Stage::Idle};
|
Stage stage_{Stage::Idle};
|
||||||
bool race_started_{false};
|
bool race_started_{false};
|
||||||
bool auto_start_{false};
|
bool auto_start_{false};
|
||||||
bool use_trajectory_guard_{true};
|
bool use_trajectory_guard_{false};
|
||||||
bool use_post_qr_pose_{true};
|
bool use_post_qr_pose_{false};
|
||||||
bool enable_vlm_image_relay_{false};
|
bool enable_vlm_image_relay_{false};
|
||||||
bool qr_detection_disabled_{false};
|
bool qr_detection_disabled_{false};
|
||||||
rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
|
rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
|
||||||
@@ -950,7 +950,7 @@ private:
|
|||||||
int sign_vlm_trigger_{9};
|
int sign_vlm_trigger_{9};
|
||||||
int sign_profile_normal_{10};
|
int sign_profile_normal_{10};
|
||||||
int sign_profile_task2_{11};
|
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 qr_pose_;
|
||||||
geometry_msgs::msg::PoseStamped post_qr_pose_;
|
geometry_msgs::msg::PoseStamped post_qr_pose_;
|
||||||
|
|||||||
@@ -2,6 +2,7 @@
|
|||||||
|
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
|
#include <cstdint>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
@@ -17,6 +18,7 @@
|
|||||||
#include <termios.h>
|
#include <termios.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
|
||||||
|
#include "geometry_msgs/msg/twist.hpp"
|
||||||
#include "nav2_msgs/action/compute_path_through_poses.hpp"
|
#include "nav2_msgs/action/compute_path_through_poses.hpp"
|
||||||
#include "nav2_msgs/action/follow_path.hpp"
|
#include "nav2_msgs/action/follow_path.hpp"
|
||||||
#include "nav2_msgs/action/navigate_to_pose.hpp"
|
#include "nav2_msgs/action/navigate_to_pose.hpp"
|
||||||
@@ -135,6 +137,9 @@ public:
|
|||||||
loadParameters();
|
loadParameters();
|
||||||
|
|
||||||
sign_pub_ = create_publisher<std_msgs::msg::Int32>(sign_topic_, 10);
|
sign_pub_ = create_publisher<std_msgs::msg::Int32>(sign_topic_, 10);
|
||||||
|
startStartupQrEnablePublisher();
|
||||||
|
recovery_cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
|
||||||
|
recovery_cmd_vel_topic_, 10);
|
||||||
guard_path_pub_ = create_publisher<nav_msgs::msg::Path>(guard_input_topic_, 1);
|
guard_path_pub_ = create_publisher<nav_msgs::msg::Path>(guard_input_topic_, 1);
|
||||||
if (enable_vlm_image_relay_) {
|
if (enable_vlm_image_relay_) {
|
||||||
vlm_image_pub_ =
|
vlm_image_pub_ =
|
||||||
@@ -189,6 +194,8 @@ private:
|
|||||||
qr_result_topic_ = declare_parameter<std::string>("qr_result_topic", "/qr_results");
|
qr_result_topic_ = declare_parameter<std::string>("qr_result_topic", "/qr_results");
|
||||||
vlm_result_topic_ = declare_parameter<std::string>("vlm_result_topic", "/vlm_result");
|
vlm_result_topic_ = declare_parameter<std::string>("vlm_result_topic", "/vlm_result");
|
||||||
odom_topic_ = declare_parameter<std::string>("odom_topic", "/odom_combined");
|
odom_topic_ = declare_parameter<std::string>("odom_topic", "/odom_combined");
|
||||||
|
recovery_cmd_vel_topic_ =
|
||||||
|
declare_parameter<std::string>("recovery_cmd_vel_topic", "/cmd_vel");
|
||||||
navigate_action_ = declare_parameter<std::string>("navigate_action", "/navigate_to_pose");
|
navigate_action_ = declare_parameter<std::string>("navigate_action", "/navigate_to_pose");
|
||||||
compute_path_action_ =
|
compute_path_action_ =
|
||||||
declare_parameter<std::string>("compute_path_action", "/compute_path_through_poses");
|
declare_parameter<std::string>("compute_path_action", "/compute_path_through_poses");
|
||||||
@@ -202,9 +209,12 @@ private:
|
|||||||
goal_checker_id_ = declare_parameter<std::string>("goal_checker_id", "");
|
goal_checker_id_ = declare_parameter<std::string>("goal_checker_id", "");
|
||||||
|
|
||||||
auto_start_ = declare_parameter<bool>("auto_start", false);
|
auto_start_ = declare_parameter<bool>("auto_start", false);
|
||||||
use_trajectory_guard_ = declare_parameter<bool>("use_trajectory_guard", true);
|
use_trajectory_guard_ = declare_parameter<bool>(
|
||||||
use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", true);
|
"use_trajectory_guard", defaultUseTrajectoryGuard());
|
||||||
|
use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", false);
|
||||||
enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
|
enable_vlm_image_relay_ = declare_parameter<bool>("enable_vlm_image_relay", false);
|
||||||
|
enable_dynamic_replanning_ = declare_parameter<bool>("enable_dynamic_replanning", true);
|
||||||
|
enable_recovery_ = declare_parameter<bool>("enable_recovery", true);
|
||||||
vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
|
vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
|
||||||
vlm_image_output_topic_ =
|
vlm_image_output_topic_ =
|
||||||
declare_parameter<std::string>("vlm_image_output_topic", "/vlm_image");
|
declare_parameter<std::string>("vlm_image_output_topic", "/vlm_image");
|
||||||
@@ -215,9 +225,20 @@ private:
|
|||||||
profile_switch_wait_sec_ = declare_parameter<double>("profile_switch_wait_sec", 1.0);
|
profile_switch_wait_sec_ = declare_parameter<double>("profile_switch_wait_sec", 1.0);
|
||||||
post_qr_wait_sec_ = declare_parameter<double>("post_qr_wait_sec", 1.0);
|
post_qr_wait_sec_ = declare_parameter<double>("post_qr_wait_sec", 1.0);
|
||||||
vlm_capture_wait_sec_ = declare_parameter<double>("vlm_capture_wait_sec", 0.5);
|
vlm_capture_wait_sec_ = declare_parameter<double>("vlm_capture_wait_sec", 0.5);
|
||||||
|
dynamic_replan_interval_sec_ =
|
||||||
|
declare_parameter<double>("dynamic_replan_interval_sec", 1.0);
|
||||||
|
dynamic_replan_stop_distance_ =
|
||||||
|
declare_parameter<double>("dynamic_replan_stop_distance", 0.5);
|
||||||
|
recovery_backup_speed_ = declare_parameter<double>("recovery_backup_speed", -0.2);
|
||||||
|
recovery_backup_distance_ = declare_parameter<double>("recovery_backup_distance", 0.04);
|
||||||
|
recovery_backup_timeout_sec_ =
|
||||||
|
declare_parameter<double>("recovery_backup_timeout_sec", 0.2);
|
||||||
pass_through_vlm_trigger_radius_ =
|
pass_through_vlm_trigger_radius_ =
|
||||||
declare_parameter<double>("pass_through_vlm_trigger_radius", 0.35);
|
declare_parameter<double>("pass_through_vlm_trigger_radius", 0.35);
|
||||||
circle_goal_tolerance_ = declare_parameter<double>("circle_goal_tolerance", 0.30);
|
circle_goal_tolerance_ = declare_parameter<double>("circle_goal_tolerance", 0.30);
|
||||||
|
dynamic_replan_max_consecutive_failures_ =
|
||||||
|
declare_parameter<int>("dynamic_replan_max_consecutive_failures", 3);
|
||||||
|
max_recovery_attempts_ = declare_parameter<int>("max_recovery_attempts", 2);
|
||||||
|
|
||||||
const auto vlm_capture_mode =
|
const auto vlm_capture_mode =
|
||||||
declare_parameter<std::string>("vlm_capture_mode", "stop");
|
declare_parameter<std::string>("vlm_capture_mode", "stop");
|
||||||
@@ -232,29 +253,30 @@ private:
|
|||||||
sign_profile_normal_ = declare_parameter<int>("sign_profile_normal", 10);
|
sign_profile_normal_ = declare_parameter<int>("sign_profile_normal", 10);
|
||||||
sign_profile_task2_ = declare_parameter<int>("sign_profile_task2", 11);
|
sign_profile_task2_ = declare_parameter<int>("sign_profile_task2", 11);
|
||||||
|
|
||||||
qr_pose_ = singlePoseFromParameter("qr_pose", {0.80, 0.20, 0.0});
|
qr_pose_ = singlePoseFromParameter(
|
||||||
entry_pose_ = singlePoseFromParameter("entry_pose", {1.20, 0.20, 0.0});
|
"qr_pose", {4.333107888975101, 1.028867429995691, 1.1383894869544392});
|
||||||
post_qr_pose_ = singlePoseFromParameter("post_qr_pose", {1.20, 0.20, 0.0});
|
entry_pose_ = singlePoseFromParameter(
|
||||||
vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 4);
|
"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<int>("vlm_waypoint_number", 2);
|
||||||
|
|
||||||
const auto clockwise_defaults = std::vector<double>{
|
const auto clockwise_defaults = std::vector<double>{
|
||||||
1.20, 0.80, 1.5708,
|
0.7534956931109804, 3.766501234923627, 1.5592359900104606,
|
||||||
2.20, 0.80, 0.0,
|
3.8567885105264077, 4.329424195458402, 0.03997856199797794,
|
||||||
2.20, 1.40, 1.5708,
|
2.5144339572664096, 2.4337692660037766, -1.6101462328054222};
|
||||||
1.20, 1.40, 3.1416,
|
|
||||||
1.20, 0.80, -1.5708};
|
|
||||||
const auto counterclockwise_defaults = std::vector<double>{
|
const auto counterclockwise_defaults = std::vector<double>{
|
||||||
1.20, 0.80, -1.5708,
|
4.2128251001861265, 3.7376334819031842, 1.5450283923222128,
|
||||||
1.20, 1.40, 3.1416,
|
1.1720794040064113, 4.319801449605877, 3.1112984779040144,
|
||||||
2.20, 1.40, 1.5708,
|
2.4951887885861144, 2.318297930897253, -1.539556591616333};
|
||||||
2.20, 0.80, 0.0,
|
|
||||||
1.20, 0.80, 1.5708};
|
|
||||||
|
|
||||||
clockwise_route_.label = "顺时针";
|
clockwise_route_.label = "顺时针";
|
||||||
clockwise_route_.waypoints = posesFromFlatDoubles(
|
clockwise_route_.waypoints = posesFromFlatDoubles(
|
||||||
declare_parameter<std::vector<double>>("clockwise_waypoints", clockwise_defaults), frame_id_);
|
declare_parameter<std::vector<double>>("clockwise_waypoints", clockwise_defaults), frame_id_);
|
||||||
clockwise_route_.home_pose =
|
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_.label = "逆时针";
|
||||||
counterclockwise_route_.waypoints = posesFromFlatDoubles(
|
counterclockwise_route_.waypoints = posesFromFlatDoubles(
|
||||||
@@ -263,7 +285,9 @@ private:
|
|||||||
counterclockwise_defaults),
|
counterclockwise_defaults),
|
||||||
frame_id_);
|
frame_id_);
|
||||||
counterclockwise_route_.home_pose =
|
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(
|
geometry_msgs::msg::PoseStamped singlePoseFromParameter(
|
||||||
@@ -326,6 +350,10 @@ private:
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (recovery_in_progress_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
const auto elapsed = (now() - stage_start_).seconds();
|
const auto elapsed = (now() - stage_start_).seconds();
|
||||||
if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) {
|
if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) {
|
||||||
if (stage_ == Stage::WaitForQr) {
|
if (stage_ == Stage::WaitForQr) {
|
||||||
@@ -371,6 +399,7 @@ private:
|
|||||||
|
|
||||||
if (stage_ == Stage::ExecuteCirclePath) {
|
if (stage_ == Stage::ExecuteCirclePath) {
|
||||||
maybeTriggerPassThroughVlmCapture();
|
maybeTriggerPassThroughVlmCapture();
|
||||||
|
maybeRunDynamicReplanning();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) {
|
if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) {
|
||||||
@@ -417,6 +446,12 @@ private:
|
|||||||
void failRace(const std::string & reason)
|
void failRace(const std::string & reason)
|
||||||
{
|
{
|
||||||
stage_ = Stage::Failed;
|
stage_ = Stage::Failed;
|
||||||
|
recovery_in_progress_ = false;
|
||||||
|
if (recovery_timer_) {
|
||||||
|
recovery_timer_->cancel();
|
||||||
|
}
|
||||||
|
cancelActiveFollowGoal();
|
||||||
|
publishRecoveryVelocity(0.0);
|
||||||
publishSign(sign_qr_disable_);
|
publishSign(sign_qr_disable_);
|
||||||
RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
|
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);
|
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()
|
void disableQrDetectionOnce()
|
||||||
{
|
{
|
||||||
if (qr_detection_disabled_) {
|
if (qr_detection_disabled_) {
|
||||||
@@ -545,6 +612,7 @@ private:
|
|||||||
const auto vlm_index = vlmWaypointIndex(route);
|
const auto vlm_index = vlmWaypointIndex(route);
|
||||||
latest_vlm_result_.clear();
|
latest_vlm_result_.clear();
|
||||||
vlm_capture_triggered_ = false;
|
vlm_capture_triggered_ = false;
|
||||||
|
recovery_attempts_ = 0;
|
||||||
if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) {
|
if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) {
|
||||||
active_segment_ = RouteSegment::FullRoute;
|
active_segment_ = RouteSegment::FullRoute;
|
||||||
active_segment_waypoints_ = route.waypoints;
|
active_segment_waypoints_ = route.waypoints;
|
||||||
@@ -567,6 +635,7 @@ private:
|
|||||||
runSwitchToNormalProfile();
|
runSwitchToNormalProfile();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
recovery_attempts_ = 0;
|
||||||
active_segment_ = RouteSegment::AfterVlm;
|
active_segment_ = RouteSegment::AfterVlm;
|
||||||
active_segment_waypoints_.assign(
|
active_segment_waypoints_.assign(
|
||||||
route.waypoints.begin() + vlm_index + 1,
|
route.waypoints.begin() + vlm_index + 1,
|
||||||
@@ -583,7 +652,7 @@ private:
|
|||||||
startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_);
|
startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_);
|
||||||
|
|
||||||
if (!compute_path_client_->wait_for_action_server(2s)) {
|
if (!compute_path_client_->wait_for_action_server(2s)) {
|
||||||
failRace("ComputePathThroughPoses action server is not available");
|
handleRouteExecutionFailure("ComputePathThroughPoses action server is not available");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -596,7 +665,7 @@ private:
|
|||||||
options.goal_response_callback =
|
options.goal_response_callback =
|
||||||
[this](ComputeGoalHandle::SharedPtr goal_handle) {
|
[this](ComputeGoalHandle::SharedPtr goal_handle) {
|
||||||
if (!goal_handle) {
|
if (!goal_handle) {
|
||||||
failRace("circle path planning goal was rejected");
|
handleRouteExecutionFailure("circle path planning goal was rejected");
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
options.result_callback =
|
options.result_callback =
|
||||||
@@ -608,7 +677,7 @@ private:
|
|||||||
result.result->path.poses.empty())
|
result.result->path.poses.empty())
|
||||||
{
|
{
|
||||||
finishStage("planning failed");
|
finishStage("planning failed");
|
||||||
failRace("circle path planning failed");
|
handleRouteExecutionFailure("circle path planning failed");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
active_path_ = result.result->path;
|
active_path_ = result.result->path;
|
||||||
@@ -624,6 +693,9 @@ private:
|
|||||||
void runRouteSegmentExecution()
|
void runRouteSegmentExecution()
|
||||||
{
|
{
|
||||||
startStage(Stage::ExecuteCirclePath, circle_timeout_sec_);
|
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_) {
|
if (use_trajectory_guard_) {
|
||||||
auto path = stampPath(active_path_);
|
auto path = stampPath(active_path_);
|
||||||
guard_path_pub_->publish(path);
|
guard_path_pub_->publish(path);
|
||||||
@@ -632,11 +704,16 @@ private:
|
|||||||
path.poses.size(), guard_input_topic_.c_str());
|
path.poses.size(), guard_input_topic_.c_str());
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
sendActiveRouteFollowPath();
|
||||||
|
}
|
||||||
|
|
||||||
|
void sendActiveRouteFollowPath()
|
||||||
|
{
|
||||||
sendFollowPath(
|
sendFollowPath(
|
||||||
active_path_, [this](const bool ok) {
|
active_path_, [this](const bool ok) {
|
||||||
finishStage(ok ? "FollowPath succeeded" : "FollowPath failed");
|
finishStage(ok ? "FollowPath succeeded" : "FollowPath failed");
|
||||||
if (!ok) {
|
if (!ok) {
|
||||||
failRace("route segment FollowPath failed");
|
handleRouteExecutionFailure("route segment FollowPath failed");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
if (active_segment_ == RouteSegment::ToVlm) {
|
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<ComputePathThroughPoses>::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<void()> 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()
|
void runReturnOrigin()
|
||||||
{
|
{
|
||||||
startStage(Stage::ReturnOrigin, navigation_timeout_sec_);
|
startStage(Stage::ReturnOrigin, navigation_timeout_sec_);
|
||||||
@@ -741,7 +983,18 @@ private:
|
|||||||
const geometry_msgs::msg::PoseStamped & pose,
|
const geometry_msgs::msg::PoseStamped & pose,
|
||||||
std::function<void(bool)> on_done)
|
std::function<void(bool)> 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<void(bool)> 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");
|
failRace("NavigateToPose action server is not available");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
@@ -751,19 +1004,55 @@ private:
|
|||||||
|
|
||||||
auto options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
|
auto options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
|
||||||
options.goal_response_callback =
|
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) {
|
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 =
|
options.result_callback =
|
||||||
[callback = std::move(on_done)](const NavigateGoalHandle::WrappedResult & result) {
|
[this, pose, on_done, retries_remaining, expected_stage](
|
||||||
callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
|
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);
|
navigate_client_->async_send_goal(goal, options);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void retryNavigateGoalOrFinish(
|
||||||
|
const geometry_msgs::msg::PoseStamped & pose,
|
||||||
|
std::function<void(bool)> 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<void(bool)> on_done)
|
void sendFollowPath(const nav_msgs::msg::Path & path, std::function<void(bool)> on_done)
|
||||||
{
|
{
|
||||||
if (!follow_path_client_->wait_for_action_server(2s)) {
|
if (!follow_path_client_->wait_for_action_server(2s)) {
|
||||||
@@ -771,20 +1060,35 @@ private:
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
cancelActiveFollowGoal();
|
||||||
|
const auto generation = follow_goal_generation_;
|
||||||
FollowPath::Goal goal;
|
FollowPath::Goal goal;
|
||||||
goal.path = stampPath(path);
|
goal.path = stampPath(path);
|
||||||
goal.controller_id = controller_id_;
|
goal.controller_id = controller_id_;
|
||||||
goal.goal_checker_id = goal_checker_id_;
|
goal.goal_checker_id = goal_checker_id_;
|
||||||
|
|
||||||
|
auto callback = std::move(on_done);
|
||||||
auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
|
auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
|
||||||
options.goal_response_callback =
|
options.goal_response_callback =
|
||||||
[this](FollowGoalHandle::SharedPtr goal_handle) {
|
[this, generation, callback](FollowGoalHandle::SharedPtr goal_handle) mutable {
|
||||||
if (!goal_handle) {
|
if (generation != follow_goal_generation_) {
|
||||||
failRace("FollowPath goal was rejected");
|
return;
|
||||||
}
|
}
|
||||||
|
if (!goal_handle) {
|
||||||
|
active_follow_goal_handle_.reset();
|
||||||
|
callback(false);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
active_follow_goal_handle_ = goal_handle;
|
||||||
};
|
};
|
||||||
options.result_callback =
|
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);
|
callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -822,6 +1126,18 @@ private:
|
|||||||
return path;
|
return path;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::optional<geometry_msgs::msg::PoseStamped> currentPoseFromOdom() const
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> 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
|
std::size_t vlmWaypointIndex(const RouteConfig & route) const
|
||||||
{
|
{
|
||||||
if (vlm_waypoint_number_ <= 0) {
|
if (vlm_waypoint_number_ <= 0) {
|
||||||
@@ -912,12 +1228,18 @@ private:
|
|||||||
Stage stage_{Stage::Idle};
|
Stage stage_{Stage::Idle};
|
||||||
bool race_started_{false};
|
bool race_started_{false};
|
||||||
bool auto_start_{false};
|
bool auto_start_{false};
|
||||||
bool use_trajectory_guard_{true};
|
bool use_trajectory_guard_{false};
|
||||||
bool use_post_qr_pose_{true};
|
bool use_post_qr_pose_{false};
|
||||||
bool enable_vlm_image_relay_{false};
|
bool enable_vlm_image_relay_{false};
|
||||||
|
bool enable_dynamic_replanning_{true};
|
||||||
|
bool enable_recovery_{true};
|
||||||
bool qr_detection_disabled_{false};
|
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 race_start_{0, 0, RCL_ROS_TIME};
|
||||||
rclcpp::Time stage_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};
|
double stage_timeout_sec_{0.0};
|
||||||
|
|
||||||
std::string frame_id_;
|
std::string frame_id_;
|
||||||
@@ -925,6 +1247,7 @@ private:
|
|||||||
std::string qr_result_topic_;
|
std::string qr_result_topic_;
|
||||||
std::string vlm_result_topic_;
|
std::string vlm_result_topic_;
|
||||||
std::string odom_topic_;
|
std::string odom_topic_;
|
||||||
|
std::string recovery_cmd_vel_topic_;
|
||||||
std::string vlm_image_input_topic_;
|
std::string vlm_image_input_topic_;
|
||||||
std::string vlm_image_output_topic_;
|
std::string vlm_image_output_topic_;
|
||||||
std::string navigate_action_;
|
std::string navigate_action_;
|
||||||
@@ -942,6 +1265,11 @@ private:
|
|||||||
double profile_switch_wait_sec_{1.0};
|
double profile_switch_wait_sec_{1.0};
|
||||||
double post_qr_wait_sec_{1.0};
|
double post_qr_wait_sec_{1.0};
|
||||||
double vlm_capture_wait_sec_{0.5};
|
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 pass_through_vlm_trigger_radius_{0.35};
|
||||||
double circle_goal_tolerance_{0.30};
|
double circle_goal_tolerance_{0.30};
|
||||||
|
|
||||||
@@ -950,7 +1278,11 @@ private:
|
|||||||
int sign_vlm_trigger_{9};
|
int sign_vlm_trigger_{9};
|
||||||
int sign_profile_normal_{10};
|
int sign_profile_normal_{10};
|
||||||
int sign_profile_task2_{11};
|
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 qr_pose_;
|
||||||
geometry_msgs::msg::PoseStamped post_qr_pose_;
|
geometry_msgs::msg::PoseStamped post_qr_pose_;
|
||||||
@@ -963,6 +1295,8 @@ private:
|
|||||||
std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
|
std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
|
||||||
nav_msgs::msg::Path active_path_;
|
nav_msgs::msg::Path active_path_;
|
||||||
bool vlm_capture_triggered_{false};
|
bool vlm_capture_triggered_{false};
|
||||||
|
std::optional<geometry_msgs::msg::PoseStamped> recovery_start_pose_;
|
||||||
|
std::function<void()> recovery_done_callback_;
|
||||||
|
|
||||||
std::string latest_qr_result_;
|
std::string latest_qr_result_;
|
||||||
std::string latest_vlm_result_;
|
std::string latest_vlm_result_;
|
||||||
@@ -976,6 +1310,7 @@ private:
|
|||||||
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_pub_;
|
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_pub_;
|
||||||
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr guard_path_pub_;
|
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr guard_path_pub_;
|
||||||
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr vlm_image_pub_;
|
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr vlm_image_pub_;
|
||||||
|
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr recovery_cmd_vel_pub_;
|
||||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr qr_sub_;
|
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr qr_sub_;
|
||||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr vlm_sub_;
|
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr vlm_sub_;
|
||||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||||
@@ -983,8 +1318,13 @@ private:
|
|||||||
rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;
|
rclcpp_action::Client<NavigateToPose>::SharedPtr navigate_client_;
|
||||||
rclcpp_action::Client<ComputePathThroughPoses>::SharedPtr compute_path_client_;
|
rclcpp_action::Client<ComputePathThroughPoses>::SharedPtr compute_path_client_;
|
||||||
rclcpp_action::Client<FollowPath>::SharedPtr follow_path_client_;
|
rclcpp_action::Client<FollowPath>::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_;
|
rclcpp::TimerBase::SharedPtr tick_timer_;
|
||||||
|
|
||||||
|
int startup_qr_enable_publish_count_{0};
|
||||||
|
std::uint64_t follow_goal_generation_{0};
|
||||||
std::atomic<bool> start_requested_{false};
|
std::atomic<bool> start_requested_{false};
|
||||||
std::atomic<bool> stop_keyboard_{false};
|
std::atomic<bool> stop_keyboard_{false};
|
||||||
std::thread keyboard_thread_;
|
std::thread keyboard_thread_;
|
||||||
|
|||||||
@@ -91,6 +91,54 @@ TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable)
|
|||||||
EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false));
|
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)
|
TEST(RacingControlHelpers, ParsesVlmCaptureMode)
|
||||||
{
|
{
|
||||||
EXPECT_EQ(
|
EXPECT_EQ(
|
||||||
|
|||||||
@@ -6,13 +6,13 @@ tts_node:
|
|||||||
vlm_detect:
|
vlm_detect:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
crop_ratio: 0.45
|
crop_ratio: 0.45
|
||||||
image_max_dim: 96
|
image_max_dim: 128
|
||||||
image_topic: /image
|
image_topic: /image
|
||||||
max_tokens: 30
|
max_tokens: 30
|
||||||
prompt_text: 忽略白色边框。描述图中医院病房场景：一个人在医院病床上，盖着白色被子，画风为2D动漫插画。对人物外观特征高度抽象，称呼为「一个病人」。30字以内。
|
prompt_text: 图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。
|
||||||
result_topic: /vlm_result
|
result_topic: /vlm_result
|
||||||
temperature: 0.1
|
temperature: 0.1
|
||||||
trigger_sign: 9
|
trigger_sign: 9
|
||||||
trigger_topic: /sign4return
|
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
|
vlm_model: /home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf
|
||||||
|
|||||||
@@ -1,20 +1,7 @@
|
|||||||
#!/usr/bin/env python3
|
#!/usr/bin/env python3
|
||||||
# -*- coding: utf-8 -*-
|
# -*- 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
|
import os
|
||||||
from ament_index_python.packages import get_package_share_directory
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
|
||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import DeclareLaunchArgument, LogInfo
|
from launch.actions import DeclareLaunchArgument, LogInfo
|
||||||
from launch.conditions import IfCondition
|
from launch.conditions import IfCondition
|
||||||
@@ -23,15 +10,11 @@ from launch_ros.actions import Node
|
|||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
|
|
||||||
# ==================== Launch 参数 ====================
|
|
||||||
use_vlm = LaunchConfiguration('use_vlm')
|
use_vlm = LaunchConfiguration('use_vlm')
|
||||||
use_tts = LaunchConfiguration('use_tts')
|
use_tts = LaunchConfiguration('use_tts')
|
||||||
use_qr_tts = LaunchConfiguration('use_qr_tts')
|
use_qr_tts = LaunchConfiguration('use_qr_tts')
|
||||||
|
|
||||||
config_file = LaunchConfiguration('config_file')
|
config_file = LaunchConfiguration('config_file')
|
||||||
|
|
||||||
# vlm_node 可覆盖参数
|
|
||||||
vlm_host = LaunchConfiguration('vlm_host')
|
vlm_host = LaunchConfiguration('vlm_host')
|
||||||
vlm_model = LaunchConfiguration('vlm_model')
|
vlm_model = LaunchConfiguration('vlm_model')
|
||||||
image_topic = LaunchConfiguration('image_topic')
|
image_topic = LaunchConfiguration('image_topic')
|
||||||
@@ -40,68 +23,31 @@ def generate_launch_description():
|
|||||||
result_topic = LaunchConfiguration('result_topic')
|
result_topic = LaunchConfiguration('result_topic')
|
||||||
prompt_text = LaunchConfiguration('prompt_text')
|
prompt_text = LaunchConfiguration('prompt_text')
|
||||||
max_tokens = LaunchConfiguration('max_tokens')
|
max_tokens = LaunchConfiguration('max_tokens')
|
||||||
|
image_max_dim = LaunchConfiguration('image_max_dim')
|
||||||
|
|
||||||
# tts_server 可覆盖参数
|
|
||||||
audio_sink = LaunchConfiguration('audio_sink')
|
audio_sink = LaunchConfiguration('audio_sink')
|
||||||
tts_speed = LaunchConfiguration('tts_speed')
|
tts_speed = LaunchConfiguration('tts_speed')
|
||||||
|
|
||||||
# ==================== 参数声明 ====================
|
declare_use_vlm = DeclareLaunchArgument('use_vlm', default_value='true')
|
||||||
declare_use_vlm = DeclareLaunchArgument(
|
declare_use_tts = DeclareLaunchArgument('use_tts', default_value='true')
|
||||||
'use_vlm', default_value='true',
|
declare_use_qr_tts = DeclareLaunchArgument('use_qr_tts', default_value='false')
|
||||||
description='启动 VLM 图生文节点')
|
declare_config_file = DeclareLaunchArgument('config_file',
|
||||||
|
|
||||||
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',
|
|
||||||
default_value=PathJoinSubstitution([
|
default_value=PathJoinSubstitution([
|
||||||
get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'
|
get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml']))
|
||||||
]),
|
|
||||||
description='YAML 配置文件路径')
|
|
||||||
|
|
||||||
# vlm_node 参数
|
declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.111:8000')
|
||||||
declare_vlm_host = DeclareLaunchArgument(
|
declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf')
|
||||||
'vlm_host', default_value='http://192.168.10.189:8000',
|
declare_image_topic = DeclareLaunchArgument('image_topic', default_value='/image')
|
||||||
description='VLM 服务器地址')
|
declare_trigger_topic = DeclareLaunchArgument('trigger_topic', default_value='/sign4return')
|
||||||
declare_vlm_model = DeclareLaunchArgument(
|
declare_trigger_sign = DeclareLaunchArgument('trigger_sign', default_value='9')
|
||||||
'vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf',
|
declare_result_topic = DeclareLaunchArgument('result_topic', default_value='/vlm_result')
|
||||||
description='VLM 模型名称')
|
declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。')
|
||||||
declare_image_topic = DeclareLaunchArgument(
|
declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='100')
|
||||||
'image_topic', default_value='/image',
|
declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='128')
|
||||||
description='输入的压缩图像话题')
|
declare_audio_sink = DeclareLaunchArgument('audio_sink',
|
||||||
declare_trigger_topic = DeclareLaunchArgument(
|
default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
|
||||||
'trigger_topic', default_value='/sign4return',
|
declare_tts_speed = DeclareLaunchArgument('tts_speed', default_value='1.5')
|
||||||
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 数')
|
|
||||||
|
|
||||||
# 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(
|
vlm_node = Node(
|
||||||
package='vlm_detect',
|
package='vlm_detect',
|
||||||
executable='vlm_node',
|
executable='vlm_node',
|
||||||
@@ -116,11 +62,12 @@ def generate_launch_description():
|
|||||||
'trigger_topic': trigger_topic,
|
'trigger_topic': trigger_topic,
|
||||||
'trigger_sign': trigger_sign,
|
'trigger_sign': trigger_sign,
|
||||||
'result_topic': result_topic,
|
'result_topic': result_topic,
|
||||||
|
'prompt_text': prompt_text,
|
||||||
'max_tokens': max_tokens,
|
'max_tokens': max_tokens,
|
||||||
|
'image_max_dim': image_max_dim,
|
||||||
}],
|
}],
|
||||||
)
|
)
|
||||||
|
|
||||||
# TTS 语音播报服务端
|
|
||||||
tts_server = Node(
|
tts_server = Node(
|
||||||
package='vlm_detect',
|
package='vlm_detect',
|
||||||
executable='tts_server',
|
executable='tts_server',
|
||||||
@@ -134,7 +81,6 @@ def generate_launch_description():
|
|||||||
}],
|
}],
|
||||||
)
|
)
|
||||||
|
|
||||||
# 二维码 → TTS 桥接 (订阅 qr_results，调用 /tts/speak)
|
|
||||||
qr_tts_bridge = Node(
|
qr_tts_bridge = Node(
|
||||||
package='vlm_detect',
|
package='vlm_detect',
|
||||||
executable='qr_tts_bridge',
|
executable='qr_tts_bridge',
|
||||||
@@ -143,9 +89,7 @@ def generate_launch_description():
|
|||||||
condition=IfCondition(use_qr_tts),
|
condition=IfCondition(use_qr_tts),
|
||||||
)
|
)
|
||||||
|
|
||||||
# ==================== 组装 ====================
|
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
# 参数声明
|
|
||||||
declare_use_vlm,
|
declare_use_vlm,
|
||||||
declare_use_tts,
|
declare_use_tts,
|
||||||
declare_use_qr_tts,
|
declare_use_qr_tts,
|
||||||
@@ -158,13 +102,11 @@ def generate_launch_description():
|
|||||||
declare_result_topic,
|
declare_result_topic,
|
||||||
declare_prompt_text,
|
declare_prompt_text,
|
||||||
declare_max_tokens,
|
declare_max_tokens,
|
||||||
|
declare_image_max_dim,
|
||||||
declare_audio_sink,
|
declare_audio_sink,
|
||||||
declare_tts_speed,
|
declare_tts_speed,
|
||||||
# 节点
|
LogInfo(msg=['Config: ', config_file]),
|
||||||
LogInfo(msg=['配置文件: ', config_file]),
|
LogInfo(msg=['VLM Host: ', vlm_host]),
|
||||||
LogInfo(msg=['VLM 服务: ', vlm_host]),
|
|
||||||
LogInfo(msg=['TTS 服务: ', use_tts]),
|
|
||||||
LogInfo(msg=['QR-TTS 桥接: ', use_qr_tts]),
|
|
||||||
vlm_node,
|
vlm_node,
|
||||||
tts_server,
|
tts_server,
|
||||||
qr_tts_bridge,
|
qr_tts_bridge,
|
||||||
|
|||||||
BIN
src/vlm_detect/vlm_detect/__pycache__/tts_server.cpython-310.pyc
Normal file
BIN
src/vlm_detect/vlm_detect/__pycache__/tts_server.cpython-310.pyc
Normal file
Binary file not shown.
Binary file not shown.
@@ -75,7 +75,15 @@ class VLMProcessor(Node):
|
|||||||
self.get_logger().info(f"Trigger {msg.data}")
|
self.get_logger().info(f"Trigger {msg.data}")
|
||||||
with self.image_lock:
|
with self.image_lock:
|
||||||
if self.latest_image is None:
|
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
|
return
|
||||||
img = self.latest_image.copy()
|
img = self.latest_image.copy()
|
||||||
self._busy = True
|
self._busy = True
|
||||||
|
|||||||
222
test1.md
Normal file
222
test1.md
Normal file
@@ -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
|
||||||
|
<useBuiltinTransports>false</useBuiltinTransports>
|
||||||
|
<userTransports>
|
||||||
|
<transport_id>udp_transport</transport_id>
|
||||||
|
</userTransports>
|
||||||
|
```
|
||||||
|
|
||||||
|
这会关闭 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 权重。
|
||||||
Reference in New Issue
Block a user