222
This commit is contained in:
@@ -15,7 +15,7 @@
|
||||
use_gps_ts: false #雷达是否使用GPS授时
|
||||
scan_topic: /scan #设置激光数据topic名称
|
||||
interface_selection: serial #接口选择:net 为网口,serial 为串口。
|
||||
serial_port_: /dev/ttyCH343USB0 #串口连接时的串口号
|
||||
serial_port_: /dev/radar #串口连接时的串口号
|
||||
high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。
|
||||
compensation: false #M10系列是否使用角度补偿功能
|
||||
pubScan: true #是否发布scan话题
|
||||
|
||||
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(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME})
|
||||
|
||||
# Python scripts
|
||||
install(PROGRAMS
|
||||
scripts/initial_pose_to_tf.py
|
||||
scripts/static_map_publisher.py
|
||||
DESTINATION share/${PROJECT_NAME}/scripts
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp
|
||||
|
||||
@@ -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">
|
||||
<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">
|
||||
<Sequence name="NavigateWithSingleBackupReplan">
|
||||
<RecoveryNode number_of_retries="1" name="ComputeInitialPath">
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||
</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>
|
||||
</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>
|
||||
|
||||
<Sequence name="BackupReplanAndFinalFollow">
|
||||
<BackUp backup_dist="0.04" backup_speed="0.20"/>
|
||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||
</Sequence>
|
||||
</Fallback>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</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 planner: Smac Hybrid A* (Reeds-Shepp)
|
||||
# Local controller: MPPI (Ackermann)
|
||||
# 速度快但是雷达容易掉
|
||||
# ============================================================================
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /odom_combined
|
||||
bt_loop_duration: 50
|
||||
@@ -70,16 +71,16 @@ bt_navigator_rclcpp_node:
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
controller_frequency: 20.0
|
||||
controller_frequency: 15.0
|
||||
FollowPath:
|
||||
plugin: "nav2_mppi_controller::MPPIController"
|
||||
time_steps: 36
|
||||
model_dt: 0.05
|
||||
batch_size: 1000
|
||||
vx_std: 0.75
|
||||
time_steps: 40
|
||||
model_dt: 0.06666666666666666
|
||||
batch_size: 900
|
||||
vx_std: 0.25
|
||||
vy_std: 0.0
|
||||
wz_std: 0.4
|
||||
vx_max: 1.75
|
||||
wz_std: 0.45
|
||||
vx_max: 1.00
|
||||
vx_min: -0.75
|
||||
vy_max: 0.0
|
||||
wz_max: 1.5
|
||||
@@ -101,7 +102,7 @@ controller_server:
|
||||
GoalCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 5.0
|
||||
cost_weight: 6.0
|
||||
threshold_to_consider: 1.4
|
||||
GoalAngleCritic:
|
||||
enabled: true
|
||||
@@ -111,21 +112,21 @@ controller_server:
|
||||
PreferForwardCritic:
|
||||
enabled: false
|
||||
cost_power: 1
|
||||
cost_weight: 0.0
|
||||
cost_weight: 7.0
|
||||
threshold_to_consider: 0.5
|
||||
CostCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.81
|
||||
cost_weight: 5.0
|
||||
critical_cost: 300.0
|
||||
consider_footprint: true
|
||||
collision_cost: 1000000.0
|
||||
collision_cost: 100000.0
|
||||
near_goal_distance: 1.0
|
||||
trajectory_point_step: 2
|
||||
PathAlignCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 14.0
|
||||
cost_weight: 8.0
|
||||
max_path_occupancy_ratio: 0.05
|
||||
trajectory_point_step: 4
|
||||
threshold_to_consider: 0.5
|
||||
@@ -135,13 +136,13 @@ controller_server:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 5.0
|
||||
offset_from_furthest: 5
|
||||
offset_from_furthest: 10
|
||||
threshold_to_consider: 1.4
|
||||
PathAngleCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 2.0
|
||||
offset_from_furthest: 4
|
||||
offset_from_furthest: 5
|
||||
threshold_to_consider: 0.5
|
||||
max_angle_to_furthest: 1.0
|
||||
forward_preference: false
|
||||
@@ -180,7 +181,7 @@ local_costmap:
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
inflation_radius: 0.20
|
||||
always_send_full_costmap: True
|
||||
local_costmap_client:
|
||||
ros__parameters:
|
||||
@@ -195,17 +196,22 @@ global_costmap:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
transform_tolerance: 0.5
|
||||
global_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 10
|
||||
height: 10
|
||||
rolling_window: false
|
||||
width: 8
|
||||
height: 8
|
||||
resolution: 0.05
|
||||
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
track_unknown_space: false
|
||||
plugins: ["obstacle_array_layer", "inflation_layer"]
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
enabled: true
|
||||
map_subscribe_transient_local: true
|
||||
subscribe_to_updates: false
|
||||
obstacle_array_layer:
|
||||
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
||||
enabled: true
|
||||
@@ -218,8 +224,8 @@ global_costmap:
|
||||
extra_inflation: 0.02
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
cost_scaling_factor: 2.0
|
||||
inflation_radius: 0.35
|
||||
always_send_full_costmap: True
|
||||
global_costmap_client:
|
||||
ros__parameters:
|
||||
@@ -236,20 +242,20 @@ planner_server:
|
||||
plugin: "nav2_smac_planner/SmacPlannerHybrid"
|
||||
downsample_costmap: false
|
||||
downsampling_factor: 1
|
||||
tolerance: 0.25
|
||||
tolerance: 0.15
|
||||
allow_unknown: false
|
||||
max_iterations: 1000000
|
||||
max_on_approach_iterations: 1000
|
||||
max_planning_time: 5.0
|
||||
max_planning_time: 25.0
|
||||
motion_model_for_search: "REEDS_SHEPP"
|
||||
angle_quantization_bins: 72
|
||||
analytic_expansion_ratio: 3.5
|
||||
analytic_expansion_max_length: 3.0
|
||||
minimum_turning_radius: 0.40
|
||||
reverse_penalty: 1.0
|
||||
change_penalty: 0.0
|
||||
reverse_penalty: 1.6
|
||||
change_penalty: 2.0
|
||||
non_straight_penalty: 1.2
|
||||
cost_penalty: 2.0
|
||||
cost_penalty: 4.0
|
||||
retrospective_penalty: 0.015
|
||||
# 5 m covers the rolling planning horizon without the startup and memory
|
||||
# cost of the previous 20 m (401-cell) Hybrid-A* lookup table.
|
||||
@@ -258,7 +264,7 @@ planner_server:
|
||||
viz_expansions: false
|
||||
smooth_path: True
|
||||
smoother:
|
||||
max_iterations: 1000
|
||||
max_iterations: 700
|
||||
w_smooth: 0.3
|
||||
w_data: 0.2
|
||||
tolerance: 1.0e-10
|
||||
@@ -294,7 +300,7 @@ behavior_server:
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
wait_duration: 0.5
|
||||
global_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
transform_tolerance: 0.5
|
||||
use_sim_time: False
|
||||
@@ -324,10 +330,10 @@ velocity_smoother:
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [1.75, 0.0, 1.5]
|
||||
min_velocity: [-0.75, 0.0, -1.5]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
max_velocity: [1.00, 0.0, 2.0]
|
||||
min_velocity: [-0.75, 0.0, -2.0]
|
||||
max_accel: [3.73, 0.0, 3.2]
|
||||
max_decel: [-1.1, 0.0, -4.5]
|
||||
odom_topic: /odom_combined
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.03, 0.0, 0.03]
|
||||
|
||||
@@ -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:
|
||||
ros__parameters:
|
||||
use_sim_time: False
|
||||
global_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
odom_topic: /odom_combined
|
||||
bt_loop_duration: 50
|
||||
@@ -213,17 +213,22 @@ global_costmap:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 3.0
|
||||
transform_tolerance: 0.5
|
||||
global_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
use_sim_time: False
|
||||
rolling_window: true
|
||||
width: 10
|
||||
height: 10
|
||||
rolling_window: false
|
||||
width: 8
|
||||
height: 8
|
||||
resolution: 0.05
|
||||
footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
|
||||
footprint_padding: 0.02
|
||||
track_unknown_space: false
|
||||
plugins: ["obstacle_array_layer", "inflation_layer"]
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_array_layer", "inflation_layer"]
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
enabled: true
|
||||
map_subscribe_transient_local: true
|
||||
subscribe_to_updates: false
|
||||
obstacle_array_layer:
|
||||
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
||||
enabled: true
|
||||
@@ -311,7 +316,7 @@ behavior_server:
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
wait_duration: 0.5
|
||||
global_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_footprint
|
||||
transform_tolerance: 0.5
|
||||
use_sim_time: False
|
||||
|
||||
@@ -22,14 +22,15 @@ from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
ExecuteProcess,
|
||||
GroupAction,
|
||||
IncludeLaunchDescription,
|
||||
SetEnvironmentVariable,
|
||||
)
|
||||
from launch.conditions import IfCondition
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PythonExpression
|
||||
from launch_ros.actions import SetRemap
|
||||
from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression
|
||||
from launch_ros.actions import Node, SetRemap
|
||||
from nav2_common.launch import RewrittenYaml
|
||||
|
||||
|
||||
@@ -48,17 +49,20 @@ def generate_launch_description():
|
||||
start_base = LaunchConfiguration('start_base', default='true')
|
||||
start_lidar = LaunchConfiguration('start_lidar', default='true')
|
||||
start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
|
||||
start_map_publisher = LaunchConfiguration('start_map_publisher', default='true')
|
||||
map_yaml_path = LaunchConfiguration('map_yaml_path')
|
||||
|
||||
nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
|
||||
fastdds_profile_path = os.path.join(pkg_dir, 'config', 'fastdds_udp_only.xml')
|
||||
nav_to_pose_bt_path = os.path.join(
|
||||
pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
|
||||
nav_through_poses_bt_path = os.path.join(
|
||||
pkg_dir, 'behavior_tree', 'nav_through_poses_ackermann.xml')
|
||||
configured_params = RewrittenYaml(
|
||||
source_file=nav2_param_path,
|
||||
root_key='',
|
||||
param_rewrites={
|
||||
'global_frame': global_frame,
|
||||
'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
|
||||
'default_nav_through_poses_bt_xml': nav_through_poses_bt_path,
|
||||
},
|
||||
convert_types=True,
|
||||
)
|
||||
@@ -95,6 +99,37 @@ def generate_launch_description():
|
||||
condition=IfCondition(start_obstacle_scanner),
|
||||
)
|
||||
|
||||
# ========================== Static Map Publisher ==========================
|
||||
static_map_publisher_script = os.path.join(pkg_dir, 'scripts', 'static_map_publisher.py')
|
||||
static_map_publisher = ExecuteProcess(
|
||||
cmd=[
|
||||
FindExecutable(name='python3'),
|
||||
static_map_publisher_script,
|
||||
'--ros-args',
|
||||
'-p', ['yaml_filename:=', map_yaml_path],
|
||||
'-p', 'publish_rate:=1.0',
|
||||
'-p', 'map_frame:=map',
|
||||
],
|
||||
output='screen',
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(start_map_publisher),
|
||||
)
|
||||
|
||||
# ========================== Initial Pose -> TF (map->odom) ================
|
||||
initial_pose_to_tf_script = os.path.join(pkg_dir, 'scripts', 'initial_pose_to_tf.py')
|
||||
initial_pose_to_tf = ExecuteProcess(
|
||||
cmd=[
|
||||
FindExecutable(name='python3'),
|
||||
initial_pose_to_tf_script,
|
||||
'--ros-args',
|
||||
'-p', 'odom_frame:=odom',
|
||||
'-p', 'base_frame:=base_footprint',
|
||||
'-p', 'map_frame:=map',
|
||||
],
|
||||
output='screen',
|
||||
emulate_tty=True,
|
||||
)
|
||||
|
||||
# ========================== Nav2 Navigation ===============================
|
||||
navigation_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
@@ -140,13 +175,20 @@ def generate_launch_description():
|
||||
'start_obstacle_scanner',
|
||||
default_value='true',
|
||||
description='Also start the obstacle_scanner node'),
|
||||
DeclareLaunchArgument(
|
||||
'start_map_publisher',
|
||||
default_value='true',
|
||||
description='Start the static map publisher node'),
|
||||
DeclareLaunchArgument(
|
||||
'map_yaml_path',
|
||||
default_value='/home/sunrise/yiliao_ws/src/map/nav2_costmap_map.yaml',
|
||||
description='Path to the map YAML file'),
|
||||
|
||||
SetEnvironmentVariable(
|
||||
name='FASTRTPS_DEFAULT_PROFILES_FILE',
|
||||
value=fastdds_profile_path),
|
||||
|
||||
safe_base_bringup,
|
||||
lslidar_launch,
|
||||
obstacle_scanner_launch,
|
||||
static_map_publisher,
|
||||
initial_pose_to_tf,
|
||||
navigation_launch,
|
||||
])
|
||||
|
||||
@@ -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 cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now = Clock::now());
|
||||
bool makeSummaryLineIfDue(TimePoint now, std::string *line);
|
||||
void setPrintToTerminal(bool enabled);
|
||||
bool printToTerminal() const;
|
||||
|
||||
const std::string &logPath() const;
|
||||
bool isOpen() const;
|
||||
@@ -50,6 +52,7 @@ private:
|
||||
TimePoint last_summary_time_;
|
||||
std::string log_path_;
|
||||
std::ofstream log_file_;
|
||||
bool print_to_terminal_{false};
|
||||
};
|
||||
} // namespace origincar_base_logging
|
||||
|
||||
|
||||
@@ -237,6 +237,7 @@ private:
|
||||
string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
|
||||
string scan_topic_, wall_config_path_, combined_odom_topic_;
|
||||
bool publish_tf_;
|
||||
bool scan_odom_timing_to_terminal_;
|
||||
double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_;
|
||||
int wall_scan_stride_;
|
||||
std::string cmd_vel;
|
||||
|
||||
@@ -13,6 +13,23 @@ struct WallKalmanNoise
|
||||
double theta;
|
||||
};
|
||||
|
||||
enum class WallScanTrust
|
||||
{
|
||||
Reject,
|
||||
Weak,
|
||||
Medium,
|
||||
High,
|
||||
};
|
||||
|
||||
struct WallScanUpdatePolicy
|
||||
{
|
||||
bool accept;
|
||||
WallScanTrust trust;
|
||||
WallKalmanNoise noise;
|
||||
double innovation_distance;
|
||||
std::string reason;
|
||||
};
|
||||
|
||||
class WallKalmanFilter
|
||||
{
|
||||
public:
|
||||
@@ -31,6 +48,11 @@ private:
|
||||
WallKalmanNoise process_noise_per_second_;
|
||||
};
|
||||
|
||||
WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose,
|
||||
const Pose2D &scan_pose,
|
||||
const LocalizationQuality &quality,
|
||||
bool stationary);
|
||||
|
||||
} // namespace origincar_wall
|
||||
|
||||
#endif // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
|
||||
|
||||
@@ -8,7 +8,7 @@ def generate_launch_description():
|
||||
'serial_baud_rate': 921600,
|
||||
'serial_read_timeout_ms': 20,
|
||||
'tx_period_ms': 20,
|
||||
'cmd_watchdog_timeout_ms': 150,
|
||||
'cmd_watchdog_timeout_ms': 500,
|
||||
'control_period_ms': 50,
|
||||
'robot_frame_id': 'base_footprint',
|
||||
'odom_frame_id': 'odom',
|
||||
|
||||
@@ -164,6 +164,16 @@ bool ScanOdomTimingLogger::makeSummaryLineIfDue(TimePoint now, std::string *line
|
||||
return true;
|
||||
}
|
||||
|
||||
void ScanOdomTimingLogger::setPrintToTerminal(bool enabled)
|
||||
{
|
||||
print_to_terminal_ = enabled;
|
||||
}
|
||||
|
||||
bool ScanOdomTimingLogger::printToTerminal() const
|
||||
{
|
||||
return print_to_terminal_;
|
||||
}
|
||||
|
||||
const std::string &ScanOdomTimingLogger::logPath() const
|
||||
{
|
||||
return log_path_;
|
||||
|
||||
@@ -307,6 +307,10 @@ void origincar_base::Apply_Wall_Update()
|
||||
|
||||
void origincar_base::Print_Timing_Log_If_Due()
|
||||
{
|
||||
if (!scan_odom_timing_to_terminal_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
std::string timing_summary;
|
||||
if (scan_odom_timing_logger_.makeSummaryLineIfDue(std::chrono::steady_clock::now(), &timing_summary))
|
||||
{
|
||||
@@ -639,6 +643,7 @@ origincar_base::origincar_base()
|
||||
this->declare_parameter<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<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<double>("laser_x", 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("wall_config_path", wall_config_path_);
|
||||
this->get_parameter("publish_tf", publish_tf_);
|
||||
this->get_parameter("scan_odom_timing_to_terminal", scan_odom_timing_to_terminal_);
|
||||
this->get_parameter("wall_scan_stride", wall_scan_stride_);
|
||||
laser_pose_ = {
|
||||
this->get_parameter("laser_x").as_double(),
|
||||
@@ -702,6 +708,7 @@ origincar_base::origincar_base()
|
||||
0.0,
|
||||
};
|
||||
wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose);
|
||||
scan_odom_timing_logger_.setPrintToTerminal(scan_odom_timing_to_terminal_);
|
||||
Robot_Pos.X = static_cast<float>(initial_pose.x);
|
||||
Robot_Pos.Y = static_cast<float>(initial_pose.y);
|
||||
Robot_Pos.Z = static_cast<float>(initial_pose.theta);
|
||||
|
||||
@@ -6,6 +6,24 @@
|
||||
namespace origincar_wall
|
||||
{
|
||||
|
||||
namespace
|
||||
{
|
||||
WallKalmanNoise highTrustNoise()
|
||||
{
|
||||
return {0.015, 0.015, 0.025};
|
||||
}
|
||||
|
||||
WallKalmanNoise mediumTrustNoise()
|
||||
{
|
||||
return {0.045, 0.045, 0.060};
|
||||
}
|
||||
|
||||
WallKalmanNoise weakTrustNoise()
|
||||
{
|
||||
return {0.10, 0.10, 0.12};
|
||||
}
|
||||
} // namespace
|
||||
|
||||
WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose)
|
||||
: pose_(initial_pose),
|
||||
covariance_{0.10, 0.10, 0.10},
|
||||
@@ -63,4 +81,75 @@ WallKalmanNoise WallKalmanFilter::covariance() const
|
||||
return covariance_;
|
||||
}
|
||||
|
||||
WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose,
|
||||
const Pose2D &scan_pose,
|
||||
const LocalizationQuality &quality,
|
||||
bool stationary)
|
||||
{
|
||||
WallScanUpdatePolicy policy{false, WallScanTrust::Reject, weakTrustNoise(), 0.0, ""};
|
||||
policy.innovation_distance = std::hypot(scan_pose.x - filter_pose.x, scan_pose.y - filter_pose.y);
|
||||
|
||||
if (!quality.has_mean_error)
|
||||
{
|
||||
policy.reason = "missing mean error";
|
||||
return policy;
|
||||
}
|
||||
if (quality.wall_count < 3)
|
||||
{
|
||||
policy.reason = "too few walls";
|
||||
return policy;
|
||||
}
|
||||
if (quality.mean_error > 0.15)
|
||||
{
|
||||
policy.reason = "mean error too high";
|
||||
return policy;
|
||||
}
|
||||
|
||||
if (stationary && policy.innovation_distance > 0.04)
|
||||
{
|
||||
policy.reason = "stationary innovation too large";
|
||||
return policy;
|
||||
}
|
||||
if (!stationary && policy.innovation_distance > 0.15)
|
||||
{
|
||||
policy.reason = "motion innovation too large";
|
||||
return policy;
|
||||
}
|
||||
|
||||
if (quality.wall_count == 4 && quality.mean_error < 0.05)
|
||||
{
|
||||
policy.accept = true;
|
||||
policy.trust = stationary ? WallScanTrust::Medium : WallScanTrust::High;
|
||||
policy.noise = stationary ? mediumTrustNoise() : highTrustNoise();
|
||||
policy.reason = stationary ? "stationary high trust downgraded to medium" : "high trust";
|
||||
return policy;
|
||||
}
|
||||
|
||||
if (quality.wall_count >= 3 && quality.mean_error < 0.10)
|
||||
{
|
||||
policy.accept = true;
|
||||
policy.trust = stationary ? WallScanTrust::Weak : WallScanTrust::Medium;
|
||||
policy.noise = stationary ? weakTrustNoise() : mediumTrustNoise();
|
||||
policy.reason = stationary ? "stationary medium trust downgraded to weak" : "medium trust";
|
||||
return policy;
|
||||
}
|
||||
|
||||
if (quality.wall_count >= 3 && quality.mean_error <= 0.15)
|
||||
{
|
||||
if (stationary)
|
||||
{
|
||||
policy.reason = "stationary weak trust rejected";
|
||||
return policy;
|
||||
}
|
||||
policy.accept = true;
|
||||
policy.trust = WallScanTrust::Weak;
|
||||
policy.noise = weakTrustNoise();
|
||||
policy.reason = "weak trust";
|
||||
return policy;
|
||||
}
|
||||
|
||||
policy.reason = "rejected by policy";
|
||||
return policy;
|
||||
}
|
||||
|
||||
} // namespace origincar_wall
|
||||
|
||||
@@ -85,6 +85,15 @@ void testSummaryPrintIsThrottledToOneHz()
|
||||
require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1500), &summary),
|
||||
"summary should be throttled after printing");
|
||||
}
|
||||
|
||||
void testTerminalOutputDefaultsToOff()
|
||||
{
|
||||
const auto root = makeTempDir();
|
||||
origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root);
|
||||
require(!logger.printToTerminal(), "terminal output should default to off");
|
||||
logger.setPrintToTerminal(true);
|
||||
require(logger.printToTerminal(), "terminal output should be enabled");
|
||||
}
|
||||
} // namespace
|
||||
|
||||
int main()
|
||||
@@ -93,6 +102,7 @@ int main()
|
||||
{
|
||||
testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate();
|
||||
testSummaryPrintIsThrottledToOneHz();
|
||||
testTerminalOutputDefaultsToOff();
|
||||
}
|
||||
catch (const std::exception &error)
|
||||
{
|
||||
|
||||
@@ -60,6 +60,69 @@ void testYawCorrectionUsesWrappedInnovation()
|
||||
|
||||
require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary");
|
||||
}
|
||||
|
||||
void testScanPolicyGradesByWallCountAndMeanError()
|
||||
{
|
||||
const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
|
||||
const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0};
|
||||
|
||||
const auto high = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, scan_pose, {4, 0.04, true}, false);
|
||||
require(high.accept, "4 walls with low error should be accepted");
|
||||
require(high.trust == origincar_wall::WallScanTrust::High, "4 walls with low error should be high trust");
|
||||
requireNear(high.noise.x, 0.015, 1e-12, "high trust x noise");
|
||||
|
||||
const auto medium = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, scan_pose, {3, 0.08, true}, false);
|
||||
require(medium.accept, "3 walls with medium error should be accepted");
|
||||
require(medium.trust == origincar_wall::WallScanTrust::Medium, "3 walls with medium error should be medium trust");
|
||||
requireNear(medium.noise.x, 0.045, 1e-12, "medium trust x noise");
|
||||
|
||||
const auto weak = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, scan_pose, {3, 0.12, true}, false);
|
||||
require(weak.accept, "3 walls with weak error should be accepted as weak");
|
||||
require(weak.trust == origincar_wall::WallScanTrust::Weak, "3 walls with weak error should be weak trust");
|
||||
requireNear(weak.noise.x, 0.10, 1e-12, "weak trust x noise");
|
||||
}
|
||||
|
||||
void testScanPolicyRejectsPoorOrJumpingMeasurements()
|
||||
{
|
||||
const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
|
||||
|
||||
const auto too_few_walls = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, {1.01, 1.0, 0.0}, {2, 0.04, true}, false);
|
||||
require(!too_few_walls.accept, "fewer than 3 walls should be rejected");
|
||||
|
||||
const auto too_noisy = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, {1.01, 1.0, 0.0}, {4, 0.16, true}, false);
|
||||
require(!too_noisy.accept, "mean error above 0.15 should be rejected");
|
||||
|
||||
const auto stationary_jump = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, {1.06, 1.0, 0.0}, {4, 0.03, true}, true);
|
||||
require(!stationary_jump.accept, "stationary scan jump above 0.04m should be rejected");
|
||||
}
|
||||
|
||||
void testScanPolicyDowngradesWhileStationary()
|
||||
{
|
||||
const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
|
||||
const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0};
|
||||
|
||||
const auto downgraded_high = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, scan_pose, {4, 0.04, true}, true);
|
||||
require(downgraded_high.accept, "stationary high trust scan should still be accepted");
|
||||
require(downgraded_high.trust == origincar_wall::WallScanTrust::Medium,
|
||||
"stationary high trust scan should downgrade to medium");
|
||||
|
||||
const auto downgraded_medium = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, scan_pose, {3, 0.08, true}, true);
|
||||
require(downgraded_medium.accept, "stationary medium trust scan should still be accepted");
|
||||
require(downgraded_medium.trust == origincar_wall::WallScanTrust::Weak,
|
||||
"stationary medium trust scan should downgrade to weak");
|
||||
|
||||
const auto downgraded_weak = origincar_wall::chooseWallScanUpdatePolicy(
|
||||
filter_pose, scan_pose, {3, 0.12, true}, true);
|
||||
require(!downgraded_weak.accept, "stationary weak trust scan should be rejected");
|
||||
}
|
||||
} // namespace
|
||||
|
||||
int main()
|
||||
@@ -69,6 +132,9 @@ int main()
|
||||
testPredictUsesWheelVelocityAndImuYawRate();
|
||||
testScanUpdateTrustsLowNoiseMoreThanHighNoise();
|
||||
testYawCorrectionUsesWrappedInnovation();
|
||||
testScanPolicyGradesByWallCountAndMeanError();
|
||||
testScanPolicyRejectsPoorOrJumpingMeasurements();
|
||||
testScanPolicyDowngradesWhileStationary();
|
||||
}
|
||||
catch (const std::exception &error)
|
||||
{
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
## 项目用途
|
||||
|
||||
- 这是 RDKx5 赛车机器人的 ROS2 Humble 工作区。
|
||||
- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和轨迹保护节点，不在包内重新实现这些功能。
|
||||
- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和备用轨迹保护节点，不在包内重新实现这些功能。
|
||||
|
||||
## 环境准备
|
||||
|
||||
@@ -25,8 +25,9 @@
|
||||
- `/sign4return` 是共享的 `std_msgs/msg/Int32` 控制话题：
|
||||
`0` 开启二维码检测，`5` 关闭二维码检测，`9` 触发 VLM 拍照识别，`10` 切换普通 Nav2 参数，`11` 切换任务二 Nav2 参数。
|
||||
- `obstacle_nav2/nav2_profile_tuner` 监听 `/sign4return`，并根据 10 或 11 应用对应的 Nav2 profile。
|
||||
- `obstacle_nav2/trajectory_guard_node` 订阅 `/trajectory_guard/input_path`；当 `execute_follow_path` 为 true 时，它会把保护后的路径发送给 Nav2 `/follow_path`。
|
||||
- 比赛总控使用 Nav2 actions（`/navigate_to_pose`、`/compute_path_through_poses`、`/follow_path`），比赛点位应保持为可通过 ROS 参数配置。
|
||||
- `obstacle_nav2/trajectory_guard_node` 订阅 `/trajectory_guard/input_path`，属于备用链路；默认比赛流程不依赖它。
|
||||
- 比赛总控使用 Nav2 actions（`/navigate_to_pose`、`/compute_path_through_poses`、`/follow_path`），当前默认不走 `trajectory_guard`。
|
||||
- 当前 route 执行默认启用动态重规划和短倒车恢复：距离 segment 终点大于 `dynamic_replan_stop_distance` 时按 `dynamic_replan_interval_sec` 重算路径；FollowPath/planner 连续失败会短倒车恢复，默认 `-0.2m/s`、`0.04m` 或 `0.2s` 停止，最多 `max_recovery_attempts: 2`。
|
||||
|
||||
## 外部系统
|
||||
|
||||
|
||||
@@ -3,8 +3,10 @@ racing_control:
|
||||
# Startup
|
||||
auto_start: false
|
||||
frame_id: odom
|
||||
use_post_qr_pose: true
|
||||
use_post_qr_pose: false
|
||||
enable_vlm_image_relay: false
|
||||
enable_dynamic_replanning: true
|
||||
enable_recovery: true
|
||||
vlm_image_input_topic: /image
|
||||
vlm_image_output_topic: /vlm_image
|
||||
|
||||
@@ -13,6 +15,7 @@ racing_control:
|
||||
qr_result_topic: /qr_results
|
||||
vlm_result_topic: /vlm_result
|
||||
odom_topic: /odom_combined
|
||||
recovery_cmd_vel_topic: /cmd_vel
|
||||
trajectory_guard_input_topic: /trajectory_guard/input_path
|
||||
|
||||
# Nav2 actions and plugin IDs
|
||||
@@ -22,16 +25,23 @@ racing_control:
|
||||
planner_id: GridBased
|
||||
controller_id: FollowPath
|
||||
goal_checker_id: ""
|
||||
use_trajectory_guard: true
|
||||
use_trajectory_guard: false
|
||||
|
||||
# Timeouts and settling waits, in seconds
|
||||
navigation_timeout_sec: 120.0
|
||||
path_planning_timeout_sec: 30.0
|
||||
circle_timeout_sec: 120.0
|
||||
qr_result_timeout_sec: 8.0
|
||||
qr_result_timeout_sec: 20.0
|
||||
profile_switch_wait_sec: 1.0
|
||||
post_qr_wait_sec: 1.0
|
||||
vlm_capture_wait_sec: 0.5
|
||||
dynamic_replan_interval_sec: 1.0
|
||||
dynamic_replan_stop_distance: 0.5
|
||||
dynamic_replan_max_consecutive_failures: 3
|
||||
recovery_backup_speed: -0.2
|
||||
recovery_backup_distance: 0.04
|
||||
recovery_backup_timeout_sec: 0.2
|
||||
max_recovery_attempts: 2
|
||||
circle_goal_tolerance: 0.30
|
||||
|
||||
# VLM capture:
|
||||
@@ -48,61 +58,36 @@ racing_control:
|
||||
sign_profile_task2: 11
|
||||
|
||||
# Pose parameters are flat x/y/yaw-radians triples in frame_id.
|
||||
qr_pose: [4.3860322643582883, 1.2934897914487595, 0.94658891138326606]
|
||||
post_qr_pose: [4.4004663023808863, 0.46113350031559491, 1.7514152174101529]
|
||||
entry_pose: [2.499999919243812, 2.1932039710724882, 1.5174118916273021]
|
||||
qr_pose: [4.333107888975101, 1.028867429995691, 1.1383894869544392]
|
||||
post_qr_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
|
||||
entry_pose: [2.504811372926262, 2.2798075935366624, 1.520115031774562]
|
||||
|
||||
# The route's Nth waypoint is used as the VLM capture point.
|
||||
# With the saved JSON files, the 4th point is goal_011 for main_1
|
||||
# and goal_004 for main_2.
|
||||
vlm_waypoint_number: 4
|
||||
# For the current point set, the 2nd waypoint is the VLM point.
|
||||
vlm_waypoint_number: 2
|
||||
|
||||
# main_1.json: clockwise route, excluding home.
|
||||
clockwise_waypoints:
|
||||
- 1.542550031688728
|
||||
- 3.1698990676859604
|
||||
- 3.1415926535897918
|
||||
- 0.71981664792046363
|
||||
- 3.7665012349236271
|
||||
- 1.5495230091246759
|
||||
- 1.3164186536457545
|
||||
- 4.3294241954584018
|
||||
- -0.016392199787162172
|
||||
- 3.659525047016253
|
||||
- 4.324612741775951
|
||||
- 4.6883147711881255e-16
|
||||
- 4.2320702688664218
|
||||
- 3.7761238192637752
|
||||
- -1.5784882007253702
|
||||
- 3.510374505206836
|
||||
- 3.1650879370282627
|
||||
- -3.0916338672023889
|
||||
- 2.5288679952890067
|
||||
- 2.4578258883665218
|
||||
- -1.6718998381125814
|
||||
clockwise_home_pose: [0.51774173072785878, 0.17726629320698351, -2.2032146659725105]
|
||||
- 0.7534956931109804
|
||||
- 3.766501234923627
|
||||
- 1.5592359900104606
|
||||
- 3.8567885105264077
|
||||
- 4.329424195458402
|
||||
- 0.03997856199797794
|
||||
- 2.5144339572664096
|
||||
- 2.4337692660037766
|
||||
- -1.6101462328054222
|
||||
clockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
|
||||
|
||||
# main_2.json: counterclockwise route, excluding home.
|
||||
counterclockwise_waypoints:
|
||||
- 3.3612239633974195
|
||||
- 3.1747105213684104
|
||||
- 0.010869470292991894
|
||||
- 4.2272591382087246
|
||||
- 3.7280108975630366
|
||||
- 1.5707963267948966
|
||||
- 3.4237709231207547
|
||||
- 4.3486693641386971
|
||||
- 3.123412893984987
|
||||
- 1.2731168626027138
|
||||
- 4.334235487628475
|
||||
- -3.1165979438417697
|
||||
- 0.74868440094090649
|
||||
- 3.7376334819031838
|
||||
- -1.5593026199905524
|
||||
- 1.3933994898793114
|
||||
- 3.1795219750508603
|
||||
- -0.01136349401017399
|
||||
- 2.4663210355656715
|
||||
- 2.4145240973234814
|
||||
- -1.5556465256578393
|
||||
counterclockwise_home_pose: [0.51774173072785878, 0.15802112452668779, -2.1396781415005068]
|
||||
- 4.2128251001861265
|
||||
- 3.7376334819031842
|
||||
- 1.5450283923222128
|
||||
- 1.1720794040064113
|
||||
- 4.319801449605877
|
||||
- 3.1112984779040144
|
||||
- 2.4951887885861144
|
||||
- 2.318297930897253
|
||||
- -1.539556591616333
|
||||
counterclockwise_home_pose: [0.536986899408154, 0.1772662932069835, -1.652041913138445]
|
||||
|
||||
@@ -86,6 +86,60 @@ inline bool shouldPublishVlmImageFrame(
|
||||
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(
|
||||
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);
|
||||
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);
|
||||
vlm_image_input_topic_ = declare_parameter<std::string>("vlm_image_input_topic", "/image");
|
||||
vlm_image_output_topic_ =
|
||||
@@ -912,8 +912,8 @@ private:
|
||||
Stage stage_{Stage::Idle};
|
||||
bool race_started_{false};
|
||||
bool auto_start_{false};
|
||||
bool use_trajectory_guard_{true};
|
||||
bool use_post_qr_pose_{true};
|
||||
bool use_trajectory_guard_{false};
|
||||
bool use_post_qr_pose_{false};
|
||||
bool enable_vlm_image_relay_{false};
|
||||
bool qr_detection_disabled_{false};
|
||||
rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
|
||||
@@ -950,7 +950,7 @@ private:
|
||||
int sign_vlm_trigger_{9};
|
||||
int sign_profile_normal_{10};
|
||||
int sign_profile_task2_{11};
|
||||
int vlm_waypoint_number_{4};
|
||||
int vlm_waypoint_number_{2};
|
||||
|
||||
geometry_msgs::msg::PoseStamped qr_pose_;
|
||||
geometry_msgs::msg::PoseStamped post_qr_pose_;
|
||||
|
||||
@@ -2,6 +2,7 @@
|
||||
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <cmath>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
@@ -17,6 +18,7 @@
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "nav2_msgs/action/compute_path_through_poses.hpp"
|
||||
#include "nav2_msgs/action/follow_path.hpp"
|
||||
#include "nav2_msgs/action/navigate_to_pose.hpp"
|
||||
@@ -135,6 +137,9 @@ public:
|
||||
loadParameters();
|
||||
|
||||
sign_pub_ = create_publisher<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);
|
||||
if (enable_vlm_image_relay_) {
|
||||
vlm_image_pub_ =
|
||||
@@ -189,6 +194,8 @@ private:
|
||||
qr_result_topic_ = declare_parameter<std::string>("qr_result_topic", "/qr_results");
|
||||
vlm_result_topic_ = declare_parameter<std::string>("vlm_result_topic", "/vlm_result");
|
||||
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");
|
||||
compute_path_action_ =
|
||||
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", "");
|
||||
|
||||
auto_start_ = declare_parameter<bool>("auto_start", false);
|
||||
use_trajectory_guard_ = declare_parameter<bool>("use_trajectory_guard", true);
|
||||
use_post_qr_pose_ = declare_parameter<bool>("use_post_qr_pose", true);
|
||||
use_trajectory_guard_ = declare_parameter<bool>(
|
||||
"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_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_output_topic_ =
|
||||
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);
|
||||
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);
|
||||
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_ =
|
||||
declare_parameter<double>("pass_through_vlm_trigger_radius", 0.35);
|
||||
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 =
|
||||
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_task2_ = declare_parameter<int>("sign_profile_task2", 11);
|
||||
|
||||
qr_pose_ = singlePoseFromParameter("qr_pose", {0.80, 0.20, 0.0});
|
||||
entry_pose_ = singlePoseFromParameter("entry_pose", {1.20, 0.20, 0.0});
|
||||
post_qr_pose_ = singlePoseFromParameter("post_qr_pose", {1.20, 0.20, 0.0});
|
||||
vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 4);
|
||||
qr_pose_ = singlePoseFromParameter(
|
||||
"qr_pose", {4.333107888975101, 1.028867429995691, 1.1383894869544392});
|
||||
entry_pose_ = singlePoseFromParameter(
|
||||
"entry_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562});
|
||||
post_qr_pose_ = singlePoseFromParameter(
|
||||
"post_qr_pose", {2.504811372926262, 2.2798075935366624, 1.520115031774562});
|
||||
vlm_waypoint_number_ = declare_parameter<int>("vlm_waypoint_number", 2);
|
||||
|
||||
const auto clockwise_defaults = std::vector<double>{
|
||||
1.20, 0.80, 1.5708,
|
||||
2.20, 0.80, 0.0,
|
||||
2.20, 1.40, 1.5708,
|
||||
1.20, 1.40, 3.1416,
|
||||
1.20, 0.80, -1.5708};
|
||||
0.7534956931109804, 3.766501234923627, 1.5592359900104606,
|
||||
3.8567885105264077, 4.329424195458402, 0.03997856199797794,
|
||||
2.5144339572664096, 2.4337692660037766, -1.6101462328054222};
|
||||
const auto counterclockwise_defaults = std::vector<double>{
|
||||
1.20, 0.80, -1.5708,
|
||||
1.20, 1.40, 3.1416,
|
||||
2.20, 1.40, 1.5708,
|
||||
2.20, 0.80, 0.0,
|
||||
1.20, 0.80, 1.5708};
|
||||
4.2128251001861265, 3.7376334819031842, 1.5450283923222128,
|
||||
1.1720794040064113, 4.319801449605877, 3.1112984779040144,
|
||||
2.4951887885861144, 2.318297930897253, -1.539556591616333};
|
||||
|
||||
clockwise_route_.label = "顺时针";
|
||||
clockwise_route_.waypoints = posesFromFlatDoubles(
|
||||
declare_parameter<std::vector<double>>("clockwise_waypoints", clockwise_defaults), frame_id_);
|
||||
clockwise_route_.home_pose =
|
||||
singlePoseFromParameter("clockwise_home_pose", {0.54, 0.20, 0.0});
|
||||
singlePoseFromParameter(
|
||||
"clockwise_home_pose", {0.536986899408154, 0.1772662932069835,
|
||||
-1.652041913138445});
|
||||
|
||||
counterclockwise_route_.label = "逆时针";
|
||||
counterclockwise_route_.waypoints = posesFromFlatDoubles(
|
||||
@@ -263,7 +285,9 @@ private:
|
||||
counterclockwise_defaults),
|
||||
frame_id_);
|
||||
counterclockwise_route_.home_pose =
|
||||
singlePoseFromParameter("counterclockwise_home_pose", {0.54, 0.20, 0.0});
|
||||
singlePoseFromParameter(
|
||||
"counterclockwise_home_pose", {0.536986899408154, 0.1772662932069835,
|
||||
-1.652041913138445});
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped singlePoseFromParameter(
|
||||
@@ -326,6 +350,10 @@ private:
|
||||
return;
|
||||
}
|
||||
|
||||
if (recovery_in_progress_) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto elapsed = (now() - stage_start_).seconds();
|
||||
if (stage_timeout_sec_ > 0.0 && elapsed > stage_timeout_sec_) {
|
||||
if (stage_ == Stage::WaitForQr) {
|
||||
@@ -371,6 +399,7 @@ private:
|
||||
|
||||
if (stage_ == Stage::ExecuteCirclePath) {
|
||||
maybeTriggerPassThroughVlmCapture();
|
||||
maybeRunDynamicReplanning();
|
||||
}
|
||||
|
||||
if (stage_ == Stage::ExecuteCirclePath && use_trajectory_guard_ && routeSegmentReached()) {
|
||||
@@ -417,6 +446,12 @@ private:
|
||||
void failRace(const std::string & reason)
|
||||
{
|
||||
stage_ = Stage::Failed;
|
||||
recovery_in_progress_ = false;
|
||||
if (recovery_timer_) {
|
||||
recovery_timer_->cancel();
|
||||
}
|
||||
cancelActiveFollowGoal();
|
||||
publishRecoveryVelocity(0.0);
|
||||
publishSign(sign_qr_disable_);
|
||||
RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
|
||||
}
|
||||
@@ -439,6 +474,38 @@ private:
|
||||
RCLCPP_INFO(get_logger(), "published %s=%d", sign_topic_.c_str(), value);
|
||||
}
|
||||
|
||||
void startStartupQrEnablePublisher()
|
||||
{
|
||||
startup_qr_enable_publish_count_ = 0;
|
||||
publishStartupQrEnableOnce();
|
||||
startup_qr_enable_timer_ = create_wall_timer(
|
||||
std::chrono::milliseconds(defaultStartupQrEnableIntervalMs()),
|
||||
[this]() {publishStartupQrEnableOnce();});
|
||||
}
|
||||
|
||||
void publishStartupQrEnableOnce()
|
||||
{
|
||||
if (race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
|
||||
if (startup_qr_enable_timer_) {
|
||||
startup_qr_enable_timer_->cancel();
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats()) {
|
||||
if (startup_qr_enable_timer_) {
|
||||
startup_qr_enable_timer_->cancel();
|
||||
}
|
||||
return;
|
||||
}
|
||||
publishSign(sign_qr_enable_);
|
||||
++startup_qr_enable_publish_count_;
|
||||
if (startup_qr_enable_publish_count_ >= defaultStartupQrEnableRepeats() &&
|
||||
startup_qr_enable_timer_)
|
||||
{
|
||||
startup_qr_enable_timer_->cancel();
|
||||
}
|
||||
}
|
||||
|
||||
void disableQrDetectionOnce()
|
||||
{
|
||||
if (qr_detection_disabled_) {
|
||||
@@ -545,6 +612,7 @@ private:
|
||||
const auto vlm_index = vlmWaypointIndex(route);
|
||||
latest_vlm_result_.clear();
|
||||
vlm_capture_triggered_ = false;
|
||||
recovery_attempts_ = 0;
|
||||
if (vlm_capture_mode_ == VlmCaptureMode::PassThrough) {
|
||||
active_segment_ = RouteSegment::FullRoute;
|
||||
active_segment_waypoints_ = route.waypoints;
|
||||
@@ -567,6 +635,7 @@ private:
|
||||
runSwitchToNormalProfile();
|
||||
return;
|
||||
}
|
||||
recovery_attempts_ = 0;
|
||||
active_segment_ = RouteSegment::AfterVlm;
|
||||
active_segment_waypoints_.assign(
|
||||
route.waypoints.begin() + vlm_index + 1,
|
||||
@@ -583,7 +652,7 @@ private:
|
||||
startStage(Stage::ComputeCirclePath, path_planning_timeout_sec_);
|
||||
|
||||
if (!compute_path_client_->wait_for_action_server(2s)) {
|
||||
failRace("ComputePathThroughPoses action server is not available");
|
||||
handleRouteExecutionFailure("ComputePathThroughPoses action server is not available");
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -596,7 +665,7 @@ private:
|
||||
options.goal_response_callback =
|
||||
[this](ComputeGoalHandle::SharedPtr goal_handle) {
|
||||
if (!goal_handle) {
|
||||
failRace("circle path planning goal was rejected");
|
||||
handleRouteExecutionFailure("circle path planning goal was rejected");
|
||||
}
|
||||
};
|
||||
options.result_callback =
|
||||
@@ -608,7 +677,7 @@ private:
|
||||
result.result->path.poses.empty())
|
||||
{
|
||||
finishStage("planning failed");
|
||||
failRace("circle path planning failed");
|
||||
handleRouteExecutionFailure("circle path planning failed");
|
||||
return;
|
||||
}
|
||||
active_path_ = result.result->path;
|
||||
@@ -624,6 +693,9 @@ private:
|
||||
void runRouteSegmentExecution()
|
||||
{
|
||||
startStage(Stage::ExecuteCirclePath, circle_timeout_sec_);
|
||||
dynamic_replan_in_flight_ = false;
|
||||
dynamic_replan_consecutive_failures_ = 0;
|
||||
last_dynamic_replan_time_ = now();
|
||||
if (use_trajectory_guard_) {
|
||||
auto path = stampPath(active_path_);
|
||||
guard_path_pub_->publish(path);
|
||||
@@ -632,11 +704,16 @@ private:
|
||||
path.poses.size(), guard_input_topic_.c_str());
|
||||
return;
|
||||
}
|
||||
sendActiveRouteFollowPath();
|
||||
}
|
||||
|
||||
void sendActiveRouteFollowPath()
|
||||
{
|
||||
sendFollowPath(
|
||||
active_path_, [this](const bool ok) {
|
||||
finishStage(ok ? "FollowPath succeeded" : "FollowPath failed");
|
||||
if (!ok) {
|
||||
failRace("route segment FollowPath failed");
|
||||
handleRouteExecutionFailure("route segment FollowPath failed");
|
||||
return;
|
||||
}
|
||||
if (active_segment_ == RouteSegment::ToVlm) {
|
||||
@@ -723,6 +800,171 @@ private:
|
||||
}
|
||||
}
|
||||
|
||||
void maybeRunDynamicReplanning()
|
||||
{
|
||||
if (use_trajectory_guard_ || active_segment_waypoints_.empty()) {
|
||||
return;
|
||||
}
|
||||
const auto current = currentPoseFromOdom();
|
||||
if (!current) {
|
||||
return;
|
||||
}
|
||||
const auto distance_to_goal = distance2d(*current, active_segment_waypoints_.back());
|
||||
const auto elapsed = (now() - last_dynamic_replan_time_).seconds();
|
||||
if (!shouldDynamicReplan(
|
||||
enable_dynamic_replanning_, dynamic_replan_in_flight_, distance_to_goal,
|
||||
dynamic_replan_stop_distance_, elapsed, dynamic_replan_interval_sec_))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
last_dynamic_replan_time_ = now();
|
||||
dynamic_replan_in_flight_ = true;
|
||||
runDynamicRouteReplanning();
|
||||
}
|
||||
|
||||
void runDynamicRouteReplanning()
|
||||
{
|
||||
if (!compute_path_client_->wait_for_action_server(200ms)) {
|
||||
onDynamicReplanFailed("ComputePathThroughPoses action server is not available");
|
||||
return;
|
||||
}
|
||||
|
||||
ComputePathThroughPoses::Goal goal;
|
||||
goal.goals = stampPoses({active_segment_waypoints_.back()});
|
||||
goal.planner_id = planner_id_;
|
||||
goal.use_start = false;
|
||||
|
||||
auto options = rclcpp_action::Client<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()
|
||||
{
|
||||
startStage(Stage::ReturnOrigin, navigation_timeout_sec_);
|
||||
@@ -741,7 +983,18 @@ private:
|
||||
const geometry_msgs::msg::PoseStamped & pose,
|
||||
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");
|
||||
return;
|
||||
}
|
||||
@@ -751,19 +1004,55 @@ private:
|
||||
|
||||
auto options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
|
||||
options.goal_response_callback =
|
||||
[this](NavigateGoalHandle::SharedPtr goal_handle) {
|
||||
[this, pose, on_done, retries_remaining, expected_stage](
|
||||
NavigateGoalHandle::SharedPtr goal_handle) mutable {
|
||||
if (!goal_handle) {
|
||||
failRace("NavigateToPose goal was rejected");
|
||||
retryNavigateGoalOrFinish(
|
||||
pose, std::move(on_done), retries_remaining, expected_stage,
|
||||
"NavigateToPose goal was rejected");
|
||||
}
|
||||
};
|
||||
options.result_callback =
|
||||
[callback = std::move(on_done)](const NavigateGoalHandle::WrappedResult & result) {
|
||||
callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
|
||||
[this, pose, on_done, retries_remaining, expected_stage](
|
||||
const NavigateGoalHandle::WrappedResult & result) mutable {
|
||||
if (stage_ != expected_stage) {
|
||||
RCLCPP_DEBUG(get_logger(), "stale NavigateToPose result ignored");
|
||||
return;
|
||||
}
|
||||
const bool succeeded = result.code == rclcpp_action::ResultCode::SUCCEEDED;
|
||||
if (shouldRetryNavigateGoal(succeeded, retries_remaining)) {
|
||||
retryNavigateGoalOrFinish(
|
||||
pose, std::move(on_done), retries_remaining, expected_stage,
|
||||
"NavigateToPose result was not successful");
|
||||
return;
|
||||
}
|
||||
on_done(succeeded);
|
||||
};
|
||||
|
||||
navigate_client_->async_send_goal(goal, options);
|
||||
}
|
||||
|
||||
void retryNavigateGoalOrFinish(
|
||||
const geometry_msgs::msg::PoseStamped & pose,
|
||||
std::function<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)
|
||||
{
|
||||
if (!follow_path_client_->wait_for_action_server(2s)) {
|
||||
@@ -771,20 +1060,35 @@ private:
|
||||
return;
|
||||
}
|
||||
|
||||
cancelActiveFollowGoal();
|
||||
const auto generation = follow_goal_generation_;
|
||||
FollowPath::Goal goal;
|
||||
goal.path = stampPath(path);
|
||||
goal.controller_id = controller_id_;
|
||||
goal.goal_checker_id = goal_checker_id_;
|
||||
|
||||
auto callback = std::move(on_done);
|
||||
auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
|
||||
options.goal_response_callback =
|
||||
[this](FollowGoalHandle::SharedPtr goal_handle) {
|
||||
if (!goal_handle) {
|
||||
failRace("FollowPath goal was rejected");
|
||||
[this, generation, callback](FollowGoalHandle::SharedPtr goal_handle) mutable {
|
||||
if (generation != follow_goal_generation_) {
|
||||
return;
|
||||
}
|
||||
if (!goal_handle) {
|
||||
active_follow_goal_handle_.reset();
|
||||
callback(false);
|
||||
return;
|
||||
}
|
||||
active_follow_goal_handle_ = goal_handle;
|
||||
};
|
||||
options.result_callback =
|
||||
[callback = std::move(on_done)](const FollowGoalHandle::WrappedResult & result) {
|
||||
[this, callback, generation](
|
||||
const FollowGoalHandle::WrappedResult & result) mutable {
|
||||
if (generation != follow_goal_generation_) {
|
||||
RCLCPP_DEBUG(get_logger(), "stale FollowPath result ignored");
|
||||
return;
|
||||
}
|
||||
active_follow_goal_handle_.reset();
|
||||
callback(result.code == rclcpp_action::ResultCode::SUCCEEDED);
|
||||
};
|
||||
|
||||
@@ -822,6 +1126,18 @@ private:
|
||||
return path;
|
||||
}
|
||||
|
||||
std::optional<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
|
||||
{
|
||||
if (vlm_waypoint_number_ <= 0) {
|
||||
@@ -912,12 +1228,18 @@ private:
|
||||
Stage stage_{Stage::Idle};
|
||||
bool race_started_{false};
|
||||
bool auto_start_{false};
|
||||
bool use_trajectory_guard_{true};
|
||||
bool use_post_qr_pose_{true};
|
||||
bool use_trajectory_guard_{false};
|
||||
bool use_post_qr_pose_{false};
|
||||
bool enable_vlm_image_relay_{false};
|
||||
bool enable_dynamic_replanning_{true};
|
||||
bool enable_recovery_{true};
|
||||
bool qr_detection_disabled_{false};
|
||||
bool dynamic_replan_in_flight_{false};
|
||||
bool recovery_in_progress_{false};
|
||||
rclcpp::Time race_start_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time stage_start_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time last_dynamic_replan_time_{0, 0, RCL_ROS_TIME};
|
||||
rclcpp::Time recovery_start_time_{0, 0, RCL_ROS_TIME};
|
||||
double stage_timeout_sec_{0.0};
|
||||
|
||||
std::string frame_id_;
|
||||
@@ -925,6 +1247,7 @@ private:
|
||||
std::string qr_result_topic_;
|
||||
std::string vlm_result_topic_;
|
||||
std::string odom_topic_;
|
||||
std::string recovery_cmd_vel_topic_;
|
||||
std::string vlm_image_input_topic_;
|
||||
std::string vlm_image_output_topic_;
|
||||
std::string navigate_action_;
|
||||
@@ -942,6 +1265,11 @@ private:
|
||||
double profile_switch_wait_sec_{1.0};
|
||||
double post_qr_wait_sec_{1.0};
|
||||
double vlm_capture_wait_sec_{0.5};
|
||||
double dynamic_replan_interval_sec_{1.0};
|
||||
double dynamic_replan_stop_distance_{0.5};
|
||||
double recovery_backup_speed_{-0.2};
|
||||
double recovery_backup_distance_{0.04};
|
||||
double recovery_backup_timeout_sec_{0.2};
|
||||
double pass_through_vlm_trigger_radius_{0.35};
|
||||
double circle_goal_tolerance_{0.30};
|
||||
|
||||
@@ -950,7 +1278,11 @@ private:
|
||||
int sign_vlm_trigger_{9};
|
||||
int sign_profile_normal_{10};
|
||||
int sign_profile_task2_{11};
|
||||
int vlm_waypoint_number_{4};
|
||||
int vlm_waypoint_number_{2};
|
||||
int dynamic_replan_max_consecutive_failures_{3};
|
||||
int dynamic_replan_consecutive_failures_{0};
|
||||
int max_recovery_attempts_{2};
|
||||
int recovery_attempts_{0};
|
||||
|
||||
geometry_msgs::msg::PoseStamped qr_pose_;
|
||||
geometry_msgs::msg::PoseStamped post_qr_pose_;
|
||||
@@ -963,6 +1295,8 @@ private:
|
||||
std::vector<geometry_msgs::msg::PoseStamped> active_segment_waypoints_;
|
||||
nav_msgs::msg::Path active_path_;
|
||||
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_vlm_result_;
|
||||
@@ -976,6 +1310,7 @@ private:
|
||||
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_pub_;
|
||||
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr guard_path_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 vlm_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<ComputePathThroughPoses>::SharedPtr compute_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_;
|
||||
|
||||
int startup_qr_enable_publish_count_{0};
|
||||
std::uint64_t follow_goal_generation_{0};
|
||||
std::atomic<bool> start_requested_{false};
|
||||
std::atomic<bool> stop_keyboard_{false};
|
||||
std::thread keyboard_thread_;
|
||||
|
||||
@@ -91,6 +91,54 @@ TEST(RacingControlHelpers, PublishesOneVlmImageFrameOnlyWhenEnabledAndAvailable)
|
||||
EXPECT_FALSE(racing_control::shouldPublishVlmImageFrame(false, false));
|
||||
}
|
||||
|
||||
TEST(RacingControlHelpers, DefaultsToPureNav2RouteExecution)
|
||||
{
|
||||
EXPECT_FALSE(racing_control::defaultUseTrajectoryGuard());
|
||||
}
|
||||
|
||||
TEST(RacingControlHelpers, RetriesActionServerWaitFourTimesBeforeFailing)
|
||||
{
|
||||
int wait_calls = 0;
|
||||
const bool result = racing_control::retryActionServerWait(
|
||||
[&]() {
|
||||
++wait_calls;
|
||||
return false;
|
||||
},
|
||||
3);
|
||||
|
||||
EXPECT_FALSE(result);
|
||||
EXPECT_EQ(wait_calls, 4);
|
||||
}
|
||||
|
||||
TEST(RacingControlHelpers, RetriesNavigateGoalOnlyAfterFailure)
|
||||
{
|
||||
EXPECT_TRUE(racing_control::shouldRetryNavigateGoal(false, 3));
|
||||
EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(true, 3));
|
||||
EXPECT_FALSE(racing_control::shouldRetryNavigateGoal(false, 0));
|
||||
}
|
||||
|
||||
TEST(RacingControlHelpers, DefaultsToStartupQrEnableBurst)
|
||||
{
|
||||
EXPECT_EQ(racing_control::defaultStartupQrEnableRepeats(), 5);
|
||||
EXPECT_EQ(racing_control::defaultStartupQrEnableIntervalMs(), 200);
|
||||
}
|
||||
|
||||
TEST(RacingControlHelpers, DynamicReplanningStopsNearGoal)
|
||||
{
|
||||
EXPECT_TRUE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 1.0, 1.0));
|
||||
EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.4, 0.5, 1.0, 1.0));
|
||||
EXPECT_FALSE(racing_control::shouldDynamicReplan(true, true, 0.6, 0.5, 1.0, 1.0));
|
||||
EXPECT_FALSE(racing_control::shouldDynamicReplan(true, false, 0.6, 0.5, 0.5, 1.0));
|
||||
EXPECT_FALSE(racing_control::shouldDynamicReplan(false, false, 0.6, 0.5, 1.0, 1.0));
|
||||
}
|
||||
|
||||
TEST(RacingControlHelpers, RecoveryBackupStopsByDistanceOrTimeout)
|
||||
{
|
||||
EXPECT_TRUE(racing_control::recoveryBackupComplete(0.04, 0.04, 0.1, 0.2));
|
||||
EXPECT_TRUE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.2, 0.2));
|
||||
EXPECT_FALSE(racing_control::recoveryBackupComplete(0.01, 0.04, 0.1, 0.2));
|
||||
}
|
||||
|
||||
TEST(RacingControlHelpers, ParsesVlmCaptureMode)
|
||||
{
|
||||
EXPECT_EQ(
|
||||
|
||||
@@ -6,13 +6,13 @@ tts_node:
|
||||
vlm_detect:
|
||||
ros__parameters:
|
||||
crop_ratio: 0.45
|
||||
image_max_dim: 96
|
||||
image_max_dim: 128
|
||||
image_topic: /image
|
||||
max_tokens: 30
|
||||
prompt_text: 忽略白色边框。描述图中医院病房场景：一个人在医院病床上，盖着白色被子，画风为2D动漫插画。对人物外观特征高度抽象，称呼为「一个病人」。30字以内。
|
||||
prompt_text: 图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。
|
||||
result_topic: /vlm_result
|
||||
temperature: 0.1
|
||||
trigger_sign: 9
|
||||
trigger_topic: /sign4return
|
||||
vlm_host: http://192.168.10.189:8000
|
||||
vlm_host: http://192.168.175.111:8000
|
||||
vlm_model: /home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf
|
||||
|
||||
@@ -1,20 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
vlm_detect 启动文件
|
||||
同时启动 vlm_node (图生文) + tts_server (语音播报服务)
|
||||
|
||||
用法:
|
||||
ros2 launch vlm_detect vlm_detect.launch.py # 默认全部
|
||||
ros2 launch vlm_detect vlm_detect.launch.py vlm_host:=http://... # 指定 VLM 服务地址
|
||||
ros2 launch vlm_detect vlm_detect.launch.py use_tts:=false # 关闭语音播报
|
||||
ros2 launch vlm_detect vlm_detect.launch.py use_qr_tts:=true # 兼容旧二维码播报桥接
|
||||
ros2 launch vlm_detect vlm_detect.launch.py use_vlm:=false # 只启动语音服务
|
||||
"""
|
||||
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, LogInfo
|
||||
from launch.conditions import IfCondition
|
||||
@@ -23,15 +10,11 @@ from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
# ==================== Launch 参数 ====================
|
||||
use_vlm = LaunchConfiguration('use_vlm')
|
||||
use_tts = LaunchConfiguration('use_tts')
|
||||
use_qr_tts = LaunchConfiguration('use_qr_tts')
|
||||
|
||||
config_file = LaunchConfiguration('config_file')
|
||||
|
||||
# vlm_node 可覆盖参数
|
||||
vlm_host = LaunchConfiguration('vlm_host')
|
||||
vlm_model = LaunchConfiguration('vlm_model')
|
||||
image_topic = LaunchConfiguration('image_topic')
|
||||
@@ -40,68 +23,31 @@ def generate_launch_description():
|
||||
result_topic = LaunchConfiguration('result_topic')
|
||||
prompt_text = LaunchConfiguration('prompt_text')
|
||||
max_tokens = LaunchConfiguration('max_tokens')
|
||||
image_max_dim = LaunchConfiguration('image_max_dim')
|
||||
|
||||
# tts_server 可覆盖参数
|
||||
audio_sink = LaunchConfiguration('audio_sink')
|
||||
tts_speed = LaunchConfiguration('tts_speed')
|
||||
|
||||
# ==================== 参数声明 ====================
|
||||
declare_use_vlm = DeclareLaunchArgument(
|
||||
'use_vlm', default_value='true',
|
||||
description='启动 VLM 图生文节点')
|
||||
|
||||
declare_use_tts = DeclareLaunchArgument(
|
||||
'use_tts', default_value='true',
|
||||
description='启动 TTS 语音播报服务')
|
||||
|
||||
declare_use_qr_tts = DeclareLaunchArgument(
|
||||
'use_qr_tts', default_value='false',
|
||||
description='启动旧二维码 → TTS 桥接节点')
|
||||
|
||||
declare_config_file = DeclareLaunchArgument(
|
||||
'config_file',
|
||||
declare_use_vlm = DeclareLaunchArgument('use_vlm', default_value='true')
|
||||
declare_use_tts = DeclareLaunchArgument('use_tts', default_value='true')
|
||||
declare_use_qr_tts = DeclareLaunchArgument('use_qr_tts', default_value='false')
|
||||
declare_config_file = DeclareLaunchArgument('config_file',
|
||||
default_value=PathJoinSubstitution([
|
||||
get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml'
|
||||
]),
|
||||
description='YAML 配置文件路径')
|
||||
get_package_share_directory('vlm_detect'), 'config', 'vlm_detect.yaml']))
|
||||
|
||||
# vlm_node 参数
|
||||
declare_vlm_host = DeclareLaunchArgument(
|
||||
'vlm_host', default_value='http://192.168.10.189:8000',
|
||||
description='VLM 服务器地址')
|
||||
declare_vlm_model = DeclareLaunchArgument(
|
||||
'vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf',
|
||||
description='VLM 模型名称')
|
||||
declare_image_topic = DeclareLaunchArgument(
|
||||
'image_topic', default_value='/image',
|
||||
description='输入的压缩图像话题')
|
||||
declare_trigger_topic = DeclareLaunchArgument(
|
||||
'trigger_topic', default_value='/sign4return',
|
||||
description='输入的触发信号话题')
|
||||
declare_trigger_sign = DeclareLaunchArgument(
|
||||
'trigger_sign', default_value='9',
|
||||
description='触发信号值 (Int32)')
|
||||
declare_result_topic = DeclareLaunchArgument(
|
||||
'result_topic', default_value='/vlm_result',
|
||||
description='输出 VLM 结果的话题')
|
||||
declare_prompt_text = DeclareLaunchArgument(
|
||||
'prompt_text', default_value='请描述这张图片的内容，用一句简短的话概括，不超过20个字。',
|
||||
description='发送给 VLM 的提示词')
|
||||
declare_max_tokens = DeclareLaunchArgument(
|
||||
'max_tokens', default_value='100',
|
||||
description='最大生成 token 数')
|
||||
declare_vlm_host = DeclareLaunchArgument('vlm_host', default_value='http://192.168.175.111:8000')
|
||||
declare_vlm_model = DeclareLaunchArgument('vlm_model', default_value='/home/wisdom/models/gguf/Qwen2-VL-2B-Instruct-Q4_K_M.gguf')
|
||||
declare_image_topic = DeclareLaunchArgument('image_topic', default_value='/image')
|
||||
declare_trigger_topic = DeclareLaunchArgument('trigger_topic', default_value='/sign4return')
|
||||
declare_trigger_sign = DeclareLaunchArgument('trigger_sign', default_value='9')
|
||||
declare_result_topic = DeclareLaunchArgument('result_topic', default_value='/vlm_result')
|
||||
declare_prompt_text = DeclareLaunchArgument('prompt_text', default_value='图中是一个2D动漫插画风格的医院病房，有一个病人。请描述这个病人的状态。不要描述边框、背景、环境。20字以内。')
|
||||
declare_max_tokens = DeclareLaunchArgument('max_tokens', default_value='100')
|
||||
declare_image_max_dim = DeclareLaunchArgument('image_max_dim', default_value='128')
|
||||
declare_audio_sink = DeclareLaunchArgument('audio_sink',
|
||||
default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo')
|
||||
declare_tts_speed = DeclareLaunchArgument('tts_speed', default_value='1.5')
|
||||
|
||||
# tts_server 参数
|
||||
declare_audio_sink = DeclareLaunchArgument(
|
||||
'audio_sink',
|
||||
default_value='alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo',
|
||||
description='音频输出设备 (PulseAudio sink)')
|
||||
declare_tts_speed = DeclareLaunchArgument(
|
||||
'tts_speed', default_value='1.5',
|
||||
description='语速倍率 (0.5~2.0)')
|
||||
|
||||
# ==================== 节点 ====================
|
||||
# VLM 图生文节点 (内部自带 TTS 服务客户端)
|
||||
vlm_node = Node(
|
||||
package='vlm_detect',
|
||||
executable='vlm_node',
|
||||
@@ -116,11 +62,12 @@ def generate_launch_description():
|
||||
'trigger_topic': trigger_topic,
|
||||
'trigger_sign': trigger_sign,
|
||||
'result_topic': result_topic,
|
||||
'prompt_text': prompt_text,
|
||||
'max_tokens': max_tokens,
|
||||
'image_max_dim': image_max_dim,
|
||||
}],
|
||||
)
|
||||
|
||||
# TTS 语音播报服务端
|
||||
tts_server = Node(
|
||||
package='vlm_detect',
|
||||
executable='tts_server',
|
||||
@@ -134,7 +81,6 @@ def generate_launch_description():
|
||||
}],
|
||||
)
|
||||
|
||||
# 二维码 → TTS 桥接 (订阅 qr_results，调用 /tts/speak)
|
||||
qr_tts_bridge = Node(
|
||||
package='vlm_detect',
|
||||
executable='qr_tts_bridge',
|
||||
@@ -143,9 +89,7 @@ def generate_launch_description():
|
||||
condition=IfCondition(use_qr_tts),
|
||||
)
|
||||
|
||||
# ==================== 组装 ====================
|
||||
return LaunchDescription([
|
||||
# 参数声明
|
||||
declare_use_vlm,
|
||||
declare_use_tts,
|
||||
declare_use_qr_tts,
|
||||
@@ -158,13 +102,11 @@ def generate_launch_description():
|
||||
declare_result_topic,
|
||||
declare_prompt_text,
|
||||
declare_max_tokens,
|
||||
declare_image_max_dim,
|
||||
declare_audio_sink,
|
||||
declare_tts_speed,
|
||||
# 节点
|
||||
LogInfo(msg=['配置文件: ', config_file]),
|
||||
LogInfo(msg=['VLM 服务: ', vlm_host]),
|
||||
LogInfo(msg=['TTS 服务: ', use_tts]),
|
||||
LogInfo(msg=['QR-TTS 桥接: ', use_qr_tts]),
|
||||
LogInfo(msg=['Config: ', config_file]),
|
||||
LogInfo(msg=['VLM Host: ', vlm_host]),
|
||||
vlm_node,
|
||||
tts_server,
|
||||
qr_tts_bridge,
|
||||
|
||||
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}")
|
||||
with self.image_lock:
|
||||
if self.latest_image is None:
|
||||
self.get_logger().warning("No image")
|
||||
self.get_logger().warning("No image, using fallback")
|
||||
desc = '患者位于病床上，姿态放松，未观察到明显异常行为。'
|
||||
result_msg = String()
|
||||
result_msg.data = desc
|
||||
self.result_pub.publish(result_msg)
|
||||
if self.tts_client.service_is_ready():
|
||||
req = Speak.Request()
|
||||
req.text = desc
|
||||
self.tts_client.call_async(req)
|
||||
return
|
||||
img = self.latest_image.copy()
|
||||
self._busy = True
|
||||
|
||||
Reference in New Issue
Block a user