Compare commits
14 Commits
b53f203f2d
...
bdddcc63cc
| Author | SHA1 | Date | |
|---|---|---|---|
| bdddcc63cc | |||
| c30a810b12 | |||
| 94609d938d | |||
| b3d06e1ba8 | |||
| 07598f4b11 | |||
| 173670e194 | |||
| 8991e5208e | |||
| 71bae6c68c | |||
| ce49a0c5d3 | |||
| b0e52bdb6f | |||
| 3880d0265c | |||
| 66acea5e87 | |||
| b4689b940f | |||
| 1219a40077 |
2
.gitignore
vendored
2
.gitignore
vendored
@@ -11,3 +11,5 @@ log
|
|||||||
.vscode
|
.vscode
|
||||||
|
|
||||||
datas
|
datas
|
||||||
|
|
||||||
|
running_logs
|
||||||
|
|||||||
237
AGENTS.md
Normal file
237
AGENTS.md
Normal file
@@ -0,0 +1,237 @@
|
|||||||
|
# yiliao_ws 项目说明
|
||||||
|
|
||||||
|
本文件是 `/home/sunrise/yiliao_ws` 工作区的项目级说明，主要记录当前比赛会用到的模块、接口、启动方式和调试注意事项。后续如果某个子目录内还有自己的 `AGENTS.md`，进入该子目录工作时以更近的说明为准。
|
||||||
|
|
||||||
|
## 项目定位
|
||||||
|
|
||||||
|
- 这是 RDKx5 机器人上的 ROS 2 Humble 工作区，当前主要服务于医疗赛道/竞速赛道任务。
|
||||||
|
- 当前比赛主链路是调度型架构：底盘、雷达、相机、二维码、Nav2、VLM、TTS 等功能分别由独立模块完成，`src/racing_control` 只负责总调度。
|
||||||
|
- 当前默认只走 Nav2 路径规划与跟随，`trajectory_guard` 只作为可选备用链路，不作为默认路径。
|
||||||
|
- 比赛点位、路线和行为开关应优先放在 YAML/launch 参数中，不要把新采集的场地点位直接写死进 C++。
|
||||||
|
- 修改比赛逻辑时优先保证流程稳定、有序、可调试；除非明确需要，不要在总调度节点里另起感知、规划或控制功能。
|
||||||
|
|
||||||
|
## 环境与访问
|
||||||
|
|
||||||
|
- 机器人 SSH：
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210
|
||||||
|
```
|
||||||
|
- 工作区根目录：
|
||||||
|
```bash
|
||||||
|
/home/sunrise/yiliao_ws
|
||||||
|
```
|
||||||
|
- 运行 ROS 2 命令前通常需要：
|
||||||
|
```bash
|
||||||
|
cd /home/sunrise/yiliao_ws
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
```
|
||||||
|
- 部分相机/Hobot 相关流程可能还需要：
|
||||||
|
```bash
|
||||||
|
source /opt/tros/humble/setup.bash
|
||||||
|
```
|
||||||
|
- 远端仓库可能已有无关改动或运行日志。不要随意 `git reset`、`git checkout --` 或删除未确认文件。
|
||||||
|
|
||||||
|
## 编译与测试
|
||||||
|
|
||||||
|
- 编译单个包：
|
||||||
|
```bash
|
||||||
|
cd /home/sunrise/yiliao_ws
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
colcon build --packages-select <package_name> --cmake-args -DBUILD_TESTING=ON
|
||||||
|
```
|
||||||
|
- 测试 `racing_control`：
|
||||||
|
```bash
|
||||||
|
cd /home/sunrise/yiliao_ws
|
||||||
|
source /opt/ros/humble/setup.bash
|
||||||
|
source install/setup.bash
|
||||||
|
colcon test --packages-select racing_control
|
||||||
|
colcon test-result --verbose --test-result-base build/racing_control
|
||||||
|
```
|
||||||
|
- 不要在同一个工作区里同时启动多个 `colcon build`。
|
||||||
|
- `ament_xmllint` 可能依赖远端 ROS schema。如果只有 xmllint 因网络/schema 临时失败，先重跑验证，再考虑改 XML。
|
||||||
|
|
||||||
|
## 比赛模块总览
|
||||||
|
|
||||||
|
### 总调度
|
||||||
|
|
||||||
|
- 包路径：`src/racing_control`
|
||||||
|
- 主节点：`src/racing_control/src/racing_control.cpp`
|
||||||
|
- 配置：`src/racing_control/config/racing_control.yaml`
|
||||||
|
- 启动：`src/racing_control/launch/racing_control.launch.py`
|
||||||
|
- 点位转换说明：`src/racing_control/点位格式转换.md`
|
||||||
|
- 设计原则：
|
||||||
|
- 只做比赛流程调度，不重复实现二维码、VLM、Nav2、底盘控制等子功能。
|
||||||
|
- 通过 `/sign4return` 控制二维码、VLM 和 Nav2 参数档位。
|
||||||
|
- 通过 Nav2 action 完成到点、路径规划和路径跟随。
|
||||||
|
- 当前比赛流程：
|
||||||
|
- 导航到 `qr_pose` 附近。
|
||||||
|
- 一旦收到可解析二维码结果，立即选择顺/逆时针路线。
|
||||||
|
- 识别二维码后不再等待 QR 目标，按配置前往 `post_qr_pose` 和 `entry_pose`。
|
||||||
|
- 进入对应方向路线，按 `clockwise_waypoints` 或 `counterclockwise_waypoints` 执行。
|
||||||
|
- 到第 `vlm_waypoint_number` 个路线点时触发 VLM。
|
||||||
|
- home 前一个点到达后发布 `/sign4return=10` 调参，再发布 home goal。
|
||||||
|
- 最后返回方向对应的 `clockwise_home_pose` 或 `counterclockwise_home_pose`。
|
||||||
|
- 使用的 Nav2 actions：
|
||||||
|
- `/navigate_to_pose`
|
||||||
|
- `/compute_path_through_poses`
|
||||||
|
- `/follow_path`
|
||||||
|
- 常用启动参数：
|
||||||
|
```bash
|
||||||
|
ros2 launch racing_control racing_control.launch.py auto_start:=false
|
||||||
|
ros2 launch racing_control racing_control.launch.py auto_start:=true
|
||||||
|
ros2 launch racing_control racing_control.launch.py enable_vlm_image_relay:=true
|
||||||
|
```
|
||||||
|
|
||||||
|
### 点位与路线参数
|
||||||
|
|
||||||
|
- 点位数组使用 `[x, y, yaw_radians]`，坐标系由 `frame_id` 指定。
|
||||||
|
- `racing_control.yaml` 中当前关键字段：
|
||||||
|
- `qr_pose`：二维码区域导航点。
|
||||||
|
- `post_qr_pose`：二维码识别后的后置过渡点，用于后续调参。
|
||||||
|
- `entry_pose`：进入正式路线前的入口点。
|
||||||
|
- `clockwise_waypoints`：顺时针路线点，不包含 home。
|
||||||
|
- `counterclockwise_waypoints`：逆时针路线点，不包含 home。
|
||||||
|
- `clockwise_home_pose`、`counterclockwise_home_pose`：最终 home 点。
|
||||||
|
- `use_post_qr_pose: true` 时启用 QR 后置点；设为 `false` 时二维码识别后直接去 `entry_pose`。
|
||||||
|
- `vlm_waypoint_number: 4` 表示正式路线第 4 个点是 VLM 拍摄点。不要依赖 `goal_004` 之类的点名。
|
||||||
|
- 二维码方向解析：
|
||||||
|
- 文本包含 `顺` 或奇数数字时选择顺时针。
|
||||||
|
- 文本包含 `逆` 或偶数数字时选择逆时针。
|
||||||
|
|
||||||
|
### Nav2 与轨迹保护
|
||||||
|
|
||||||
|
- 包路径：`src/navigation/obstacle_nav2`
|
||||||
|
- 主启动：`launch/obstacle_nav2.launch.py`
|
||||||
|
- 轨迹保护启动：`launch/trajectory_guard.launch.py`
|
||||||
|
- 运行时参数档位切换：`launch/nav2_profile_tuner.launch.py`
|
||||||
|
- 关键配置：
|
||||||
|
- `config/nav2_profile_10.yaml`：普通/默认导航参数。
|
||||||
|
- `config/nav2_profile_11.yaml`：任务二路线参数。
|
||||||
|
- `config/trajectory_guard.yaml`：轨迹保护参数。
|
||||||
|
- `trajectory_guard_node` 订阅 `/trajectory_guard/input_path`，属于备用链路；默认比赛流程不依赖它。
|
||||||
|
- `nav2_profile_tuner` 监听 `/sign4return`，收到 10 或 11 后调用 Nav2 参数服务切换参数档位。
|
||||||
|
- `obstacle_nav2.launch.py` 当前以里程计坐标为主，默认 global frame 是 `odom`；静态地图和 AMCL 相关配置留作备用。
|
||||||
|
|
||||||
|
### 相机与二维码
|
||||||
|
|
||||||
|
- 相机包：`src/car_usb_cam`
|
||||||
|
- 二维码包：`src/qr_detection`
|
||||||
|
- 常见相机话题：`/image`
|
||||||
|
- 当前 VLM/调度链路期望的图像类型：`sensor_msgs/msg/CompressedImage`
|
||||||
|
- 二维码结果话题：`/qr_results`，类型 `std_msgs/msg/String`
|
||||||
|
- 二维码检测受 `/sign4return` 控制：
|
||||||
|
- `0`：启用二维码检测。
|
||||||
|
- `5`：关闭二维码检测。
|
||||||
|
- 常用检查命令：
|
||||||
|
```bash
|
||||||
|
ros2 topic echo /qr_results
|
||||||
|
ros2 topic info /image
|
||||||
|
ros2 topic hz /image
|
||||||
|
```
|
||||||
|
- 如果 `racing_control` 到二维码区域后不继续走，优先检查：
|
||||||
|
- 日志里是否出现 `QR result received`。
|
||||||
|
- `/qr_results` 是否真的有输出。
|
||||||
|
- 输出文本是否包含 `顺`、`逆` 或可解析数字。
|
||||||
|
|
||||||
|
### VLM 与语音
|
||||||
|
|
||||||
|
- 包路径：`src/vlm_detect`
|
||||||
|
- 启动文件：
|
||||||
|
- `launch/vlm_detect.launch.py`：OpenAI 兼容 VLM 服务流程。
|
||||||
|
- `launch/local_vlm_adapter.launch.py`：本地 `hobot_llamacpp` 适配流程。
|
||||||
|
- VLM 触发信号：`/sign4return=9`
|
||||||
|
- VLM 结果话题：`/vlm_result`
|
||||||
|
- TTS 服务：`/tts/speak`，类型 `origincar_msg/srv/Speak`
|
||||||
|
- `vlm_detect` 的图像输入可通过 launch 参数 `image_topic` 配置。
|
||||||
|
- `racing_control` 可选单帧 VLM 图像转发：
|
||||||
|
- `enable_vlm_image_relay: false` 默认关闭。
|
||||||
|
- 输入话题：`vlm_image_input_topic`，默认 `/image`。
|
||||||
|
- 输出话题：`vlm_image_output_topic`，默认 `/vlm_image`。
|
||||||
|
- 启用后，`racing_control` 在发布 `/sign4return=9` 前，把缓存到的一帧压缩图像发布到 `/vlm_image`。
|
||||||
|
- 若要让 VLM 使用该帧，VLM 需这样启动：
|
||||||
|
```bash
|
||||||
|
ros2 launch vlm_detect vlm_detect.launch.py image_topic:=/vlm_image
|
||||||
|
```
|
||||||
|
- 当前 VLM 拍摄模式：
|
||||||
|
- `vlm_capture_mode: stop`：到 VLM 点停住，发布图像/触发信号，等待 `vlm_capture_wait_sec` 后继续。
|
||||||
|
- `vlm_capture_mode: pass_through`：保留的经过拍照模式，按 `pass_through_vlm_trigger_radius` 在接近 VLM 点时触发。
|
||||||
|
|
||||||
|
### 底盘、雷达、里程计与消息
|
||||||
|
|
||||||
|
- 底盘包：`src/origincar_base`
|
||||||
|
- 常见启动：`origincar_bringup.launch.py`、`base_serial.launch.py`、`ekf.launch.py`
|
||||||
|
- 负责底盘串口、IMU、里程计、EKF 和 TF 等基础能力。
|
||||||
|
- 雷达驱动路径：`src/LSLIDAR_X_ROS2-20240228/src`
|
||||||
|
- 障碍物检测包：`src/obstacle_scanner`
|
||||||
|
- 消费雷达数据，发布导航需要的障碍物信息。
|
||||||
|
- 消息/服务包：`src/origincar_msg`
|
||||||
|
- 包含 `origincar_msg/srv/Speak`。
|
||||||
|
- 当前 `racing_control` 期望里程计话题是 `/odom_combined`。
|
||||||
|
|
||||||
|
### 启动编排
|
||||||
|
|
||||||
|
- `src/my_robot_bringup/launch/master_launch.py` 是较完整的分阶段总启动，涉及底盘、雷达、TTS、相机、二维码、障碍物、规划和 VLM。
|
||||||
|
- 当前 `racing_control` 流程会直接依赖 Nav2 actions，并结合 `obstacle_nav2`/轨迹保护相关节点。修改 `master_launch.py` 前，应确认现场实际是通过总 launch 还是多个终端分别启动。
|
||||||
|
- `planner` 包里还有较旧或备用的 Hybrid A*、Pure Pursuit 等流程。除非当前 launch 明确使用它，否则先把它当作历史/备用路径看待。
|
||||||
|
|
||||||
|
## 共享信号与话题
|
||||||
|
|
||||||
|
- `/sign4return` 类型是 `std_msgs/msg/Int32`，被多个模块共享：
|
||||||
|
- `0`：启用二维码检测。
|
||||||
|
- `5`：关闭二维码检测。
|
||||||
|
- `9`：触发 VLM 拍摄/推理。
|
||||||
|
- `10`：应用普通 Nav2 参数档。
|
||||||
|
- `11`：应用任务二 Nav2 参数档。
|
||||||
|
- 新增 `/sign4return` 数值前，必须检查所有订阅者，包括底盘、感知和导航参数切换节点。
|
||||||
|
- 其他关键接口：
|
||||||
|
- `/qr_results`：二维码文本结果。
|
||||||
|
- `/vlm_result`：VLM 文本结果。
|
||||||
|
- `/tts/speak`：TTS 服务。
|
||||||
|
- `/image`：压缩相机流。
|
||||||
|
- `/vlm_image`：可选单帧 VLM 图像转发输出。
|
||||||
|
- `/trajectory_guard/input_path`：轨迹保护输入路径。
|
||||||
|
- `/cmd_vel`：真实运动控制通道，避免多个节点同时发布。
|
||||||
|
|
||||||
|
## 调试注意事项
|
||||||
|
|
||||||
|
- 不要让多个节点同时发布 `/cmd_vel`，除非明确 remap 掉其中一个。
|
||||||
|
- 二维码区域后不继续走时：
|
||||||
|
- 看 `racing_control` 日志是否出现 `QR result received`。
|
||||||
|
- 直接 echo `/qr_results`。
|
||||||
|
- 确认二维码文本能被解析为顺/逆方向。
|
||||||
|
- VLM 报 `No image` 或没有结果时：
|
||||||
|
- 检查 `ros2 topic info /image`。
|
||||||
|
- 如果启用转发，确认 `enable_vlm_image_relay:=true`，并且 VLM 使用 `image_topic:=/vlm_image`。
|
||||||
|
- 确认相机实际发布的是 `sensor_msgs/msg/CompressedImage`。
|
||||||
|
- 路线段末端卡住时：
|
||||||
|
- 检查 `/odom_combined` 是否连续。
|
||||||
|
- 检查 `circle_goal_tolerance` 是否过小。
|
||||||
|
- 如果临时启用了 `trajectory_guard`，再检查它是否正常向 `/follow_path` 转发。
|
||||||
|
- Nav2 在任务二前后行为差异大时：
|
||||||
|
- 查看 `nav2_profile_10.yaml` 和 `nav2_profile_11.yaml`。
|
||||||
|
- 查看 `nav2_profile_tuner` 日志里是否收到并应用 `/sign4return` 10/11。
|
||||||
|
- launch 已启动但按空格不能开始比赛时，可能是 stdin 不是 TTY；可使用 `auto_start:=true`。
|
||||||
|
- 更新现场点位时，优先修改 `src/racing_control/config/racing_control.yaml`，然后重新 build/install `racing_control`，确保 launch 读到安装后的配置。
|
||||||
|
|
||||||
|
## 关键参数清单
|
||||||
|
|
||||||
|
- `frame_id`：比赛点位所属坐标系。
|
||||||
|
- `qr_pose`、`post_qr_pose`、`entry_pose`：二维码阶段到正式路线阶段的关键过渡点。
|
||||||
|
- `clockwise_waypoints`、`counterclockwise_waypoints`：顺/逆时针路线点。
|
||||||
|
- `clockwise_home_pose`、`counterclockwise_home_pose`：顺/逆方向 home 点。
|
||||||
|
- `use_post_qr_pose`：是否启用二维码后置点。
|
||||||
|
- `vlm_waypoint_number`：第几个正式路线点触发 VLM。
|
||||||
|
- `vlm_capture_mode`：`stop` 或 `pass_through`。
|
||||||
|
- `vlm_capture_wait_sec`：停住拍照模式下触发 VLM 后等待时间。
|
||||||
|
- `enable_vlm_image_relay`：是否由 `racing_control` 把 `/image` 的单帧图像转发到 `/vlm_image`。
|
||||||
|
- `vlm_image_input_topic`、`vlm_image_output_topic`：VLM 图像转发输入/输出话题。
|
||||||
|
|
||||||
|
## 待后续补齐
|
||||||
|
|
||||||
|
- 比赛日标准启动顺序：到底使用 `my_robot_bringup/master_launch.py`，还是分终端启动 `obstacle_nav2`、`vlm_detect`、`racing_control` 等节点。
|
||||||
|
- 当前最权威的 Nav2 参数归属：`obstacle_nav2` 的 profile 看起来是比赛主路径，但 `planner` 和 `my_robot_bringup` 仍保留旧流程。
|
||||||
|
- 不同部署模式下相机话题的最终类型。近期 `vlm_detect` 期望 `CompressedImage`，换相机驱动前要重新确认。
|
||||||
|
- `/sign4return` 除 0、5、9、10、11 外，在底盘板和感知节点里的完整含义。
|
||||||
|
- 现场最新点位来源、采集时间和对应 JSON/YAML 转换记录。
|
||||||
@@ -151,7 +151,7 @@ World 文件在 `src/origincar_description/world/`:
|
|||||||
|
|
||||||
## 实车底盘驱动包 (origincar_base)
|
## 实车底盘驱动包 (origincar_base)
|
||||||
|
|
||||||
- `origincar_base_node`（C++）: 串口读写（/dev/ttyACM0, 115200bps）、航迹推算、四元数姿态解算（Mahony AHRS）
|
- `origincar_base_node`（C++）: 串口读写（/dev/ttyACM0, 921600bps）、航迹推算、四元数姿态解算（Mahony AHRS）
|
||||||
- `cmd_vel_to_ackermann_drive.py`（Python）: Twist → AckermannDriveStamped 转换，wheelbase=0.143
|
- `cmd_vel_to_ackermann_drive.py`（Python）: Twist → AckermannDriveStamped 转换，wheelbase=0.143
|
||||||
- 帧协议: 24 字节收（帧头0x7B/帧尾0x7D）/ 11 字节发
|
- 帧协议: 24 字节收（帧头0x7B/帧尾0x7D）/ 11 字节发
|
||||||
- EKF 融合: `/odom` + IMU → `/odom_combined`，`two_d_mode=true`
|
- EKF 融合: `/odom` + IMU → `/odom_combined`，`two_d_mode=true`
|
||||||
|
|||||||
@@ -0,0 +1,631 @@
|
|||||||
|
# Racing Control 最终回家保护实现计划
|
||||||
|
|
||||||
|
> **给 agentic workers:** 必须使用 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans` 按任务逐步实现本计划。所有步骤用 checkbox（`- [ ]`）跟踪。
|
||||||
|
|
||||||
|
**目标:** 给 `racing_control` 增加最终兜底回家逻辑，让普通 Nav2/恢复全部失败后，小车仍然低速朝 home 前进并尝试完赛，而不是停在 `race failed`。
|
||||||
|
|
||||||
|
**架构:** 核心控制算法单独放到 `final_home_fallback.hpp/.cpp`，主程序不直接写算法，只负责状态机、参数、odom/home 输入和 `/cmd_vel` 发布。`racing_control.cpp` 新增 `FinalHomeFallback` 阶段，在该阶段每个控制周期调用算法生成速度，同时按固定间隔尝试 Nav2 回家；Nav2 成功则切回正常完成，Nav2 不成功则继续直控。
|
||||||
|
|
||||||
|
**技术栈:** ROS2 Humble、C++17、`rclcpp`、`geometry_msgs/msg/Twist`、`nav2_msgs/action/NavigateToPose`、gtest、现有 `racing_control` 包。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### 文件结构
|
||||||
|
|
||||||
|
- 新建: `/home/sunrise/yiliao_ws/src/racing_control/include/racing_control/final_home_fallback.hpp`
|
||||||
|
- 声明最终回家保护算法的数据结构和函数。
|
||||||
|
- 新建: `/home/sunrise/yiliao_ws/src/racing_control/src/final_home_fallback.cpp`
|
||||||
|
- 实现 yaw 提取、角度归一化、home 方向误差计算、直控速度生成。
|
||||||
|
- 修改: `/home/sunrise/yiliao_ws/src/racing_control/CMakeLists.txt`
|
||||||
|
- 把 `src/final_home_fallback.cpp` 加入 `racing_control_core`、`racing_control` 和 helper 测试目标。
|
||||||
|
- 修改: `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp`
|
||||||
|
- 只接入最终保护阶段、参数、Nav2 重试和 `/cmd_vel` 发布，不写核心算法。
|
||||||
|
- 修改: `/home/sunrise/yiliao_ws/src/racing_control/config/racing_control.yaml`
|
||||||
|
- 增加最终保护参数默认值。
|
||||||
|
- 修改: `/home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.cpp`
|
||||||
|
- 增加算法单测，验证角度归一化、到达判断、转弯方向、前进控制。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Task 1: 写最终保护算法的失败测试
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Modify: `/home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.cpp`
|
||||||
|
- Create later: `/home/sunrise/yiliao_ws/src/racing_control/include/racing_control/final_home_fallback.hpp`
|
||||||
|
- Create later: `/home/sunrise/yiliao_ws/src/racing_control/src/final_home_fallback.cpp`
|
||||||
|
|
||||||
|
- [ ] **Step 1: 引入新头文件**
|
||||||
|
|
||||||
|
在 `/home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.cpp` 的 include 区域加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
#include "racing_control/final_home_fallback.hpp"
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 2: 添加失败测试**
|
||||||
|
|
||||||
|
把下面测试追加到 `/home/sunrise/yiliao_ws/src/racing_control/test/test_racing_control_helpers.cpp`：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
TEST(FinalHomeFallback, NormalizesAnglesToShortestRotation)
|
||||||
|
{
|
||||||
|
EXPECT_NEAR(racing_control::normalizeAngle(3.5), -2.7831853071795862, 1e-6);
|
||||||
|
EXPECT_NEAR(racing_control::normalizeAngle(-3.5), 2.7831853071795862, 1e-6);
|
||||||
|
EXPECT_NEAR(racing_control::normalizeAngle(0.25), 0.25, 1e-6);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(FinalHomeFallback, StopsInsideDistanceTolerance)
|
||||||
|
{
|
||||||
|
const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
|
||||||
|
const auto home = racing_control::poseFromXYYaw(0.1, 0.1, 0.0, "odom");
|
||||||
|
|
||||||
|
const auto command = racing_control::computeFinalHomeFallbackCommand(
|
||||||
|
current, home, 0.2, 0.3, 0.12, 0.5, 0.7);
|
||||||
|
|
||||||
|
EXPECT_TRUE(command.reached);
|
||||||
|
EXPECT_DOUBLE_EQ(command.twist.linear.x, 0.0);
|
||||||
|
EXPECT_DOUBLE_EQ(command.twist.angular.z, 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(FinalHomeFallback, TurnsTowardHomeWhenYawErrorIsLarge)
|
||||||
|
{
|
||||||
|
const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
|
||||||
|
const auto home = racing_control::poseFromXYYaw(0.0, 1.0, 0.0, "odom");
|
||||||
|
|
||||||
|
const auto command = racing_control::computeFinalHomeFallbackCommand(
|
||||||
|
current, home, 0.2, 0.3, 0.12, 0.5, 0.7);
|
||||||
|
|
||||||
|
EXPECT_FALSE(command.reached);
|
||||||
|
EXPECT_NEAR(command.distance, 1.0, 1e-6);
|
||||||
|
EXPECT_NEAR(command.yaw_error, M_PI_2, 1e-6);
|
||||||
|
EXPECT_DOUBLE_EQ(command.twist.linear.x, 0.12);
|
||||||
|
EXPECT_DOUBLE_EQ(command.twist.angular.z, 0.7);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(FinalHomeFallback, DrivesForwardWhenYawErrorIsSmall)
|
||||||
|
{
|
||||||
|
const auto current = racing_control::poseFromXYYaw(0.0, 0.0, 0.0, "odom");
|
||||||
|
const auto home = racing_control::poseFromXYYaw(1.0, 0.1, 0.0, "odom");
|
||||||
|
|
||||||
|
const auto command = racing_control::computeFinalHomeFallbackCommand(
|
||||||
|
current, home, 0.2, 0.3, 0.12, 0.5, 0.7);
|
||||||
|
|
||||||
|
EXPECT_FALSE(command.reached);
|
||||||
|
EXPECT_GT(command.twist.linear.x, 0.0);
|
||||||
|
EXPECT_NEAR(command.twist.angular.z, command.yaw_error * 0.5, 1e-6);
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 3: 运行测试并确认失败**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon test --packages-select racing_control --ctest-args -R test_racing_control_helpers --output-on-failure"'
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：失败，原因是 `racing_control/final_home_fallback.hpp` 或 `computeFinalHomeFallbackCommand` 尚不存在。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Task 2: 新建最终保护算法文件
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Create: `/home/sunrise/yiliao_ws/src/racing_control/include/racing_control/final_home_fallback.hpp`
|
||||||
|
- Create: `/home/sunrise/yiliao_ws/src/racing_control/src/final_home_fallback.cpp`
|
||||||
|
- Modify: `/home/sunrise/yiliao_ws/src/racing_control/CMakeLists.txt`
|
||||||
|
|
||||||
|
- [ ] **Step 1: 创建算法头文件**
|
||||||
|
|
||||||
|
创建 `/home/sunrise/yiliao_ws/src/racing_control/include/racing_control/final_home_fallback.hpp`：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
#ifndef RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_
|
||||||
|
#define RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_
|
||||||
|
|
||||||
|
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||||
|
#include "geometry_msgs/msg/twist.hpp"
|
||||||
|
|
||||||
|
namespace racing_control
|
||||||
|
{
|
||||||
|
|
||||||
|
struct FinalHomeFallbackCommand
|
||||||
|
{
|
||||||
|
geometry_msgs::msg::Twist twist;
|
||||||
|
double distance{0.0};
|
||||||
|
double yaw_error{0.0};
|
||||||
|
bool reached{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
double normalizeAngle(double angle);
|
||||||
|
|
||||||
|
double yawFromPose(const geometry_msgs::msg::PoseStamped & pose);
|
||||||
|
|
||||||
|
FinalHomeFallbackCommand computeFinalHomeFallbackCommand(
|
||||||
|
const geometry_msgs::msg::PoseStamped & current,
|
||||||
|
const geometry_msgs::msg::PoseStamped & home,
|
||||||
|
double distance_tolerance,
|
||||||
|
double yaw_tolerance,
|
||||||
|
double forward_speed,
|
||||||
|
double yaw_gain,
|
||||||
|
double max_turn_speed);
|
||||||
|
|
||||||
|
} // namespace racing_control
|
||||||
|
|
||||||
|
#endif // RACING_CONTROL__FINAL_HOME_FALLBACK_HPP_
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 2: 创建算法实现文件**
|
||||||
|
|
||||||
|
创建 `/home/sunrise/yiliao_ws/src/racing_control/src/final_home_fallback.cpp`：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
#include "racing_control/final_home_fallback.hpp"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
namespace racing_control
|
||||||
|
{
|
||||||
|
|
||||||
|
double normalizeAngle(double angle)
|
||||||
|
{
|
||||||
|
while (angle > M_PI) {
|
||||||
|
angle -= 2.0 * M_PI;
|
||||||
|
}
|
||||||
|
while (angle < -M_PI) {
|
||||||
|
angle += 2.0 * M_PI;
|
||||||
|
}
|
||||||
|
return angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
double yawFromPose(const geometry_msgs::msg::PoseStamped & pose)
|
||||||
|
{
|
||||||
|
const auto & q = pose.pose.orientation;
|
||||||
|
return std::atan2(
|
||||||
|
2.0 * (q.w * q.z + q.x * q.y),
|
||||||
|
1.0 - 2.0 * (q.y * q.y + q.z * q.z));
|
||||||
|
}
|
||||||
|
|
||||||
|
FinalHomeFallbackCommand computeFinalHomeFallbackCommand(
|
||||||
|
const geometry_msgs::msg::PoseStamped & current,
|
||||||
|
const geometry_msgs::msg::PoseStamped & home,
|
||||||
|
const double distance_tolerance,
|
||||||
|
const double yaw_tolerance,
|
||||||
|
const double forward_speed,
|
||||||
|
const double yaw_gain,
|
||||||
|
const double max_turn_speed)
|
||||||
|
{
|
||||||
|
FinalHomeFallbackCommand command;
|
||||||
|
const auto dx = home.pose.position.x - current.pose.position.x;
|
||||||
|
const auto dy = home.pose.position.y - current.pose.position.y;
|
||||||
|
command.distance = std::hypot(dx, dy);
|
||||||
|
|
||||||
|
if (command.distance <= distance_tolerance) {
|
||||||
|
command.reached = true;
|
||||||
|
return command;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto target_yaw = std::atan2(dy, dx);
|
||||||
|
command.yaw_error = normalizeAngle(target_yaw - yawFromPose(current));
|
||||||
|
command.twist.linear.x = forward_speed;
|
||||||
|
|
||||||
|
if (std::abs(command.yaw_error) > yaw_tolerance) {
|
||||||
|
command.twist.angular.z = command.yaw_error > 0.0 ? max_turn_speed : -max_turn_speed;
|
||||||
|
} else {
|
||||||
|
const auto raw_turn = command.yaw_error * yaw_gain;
|
||||||
|
command.twist.angular.z = std::clamp(raw_turn, -max_turn_speed, max_turn_speed);
|
||||||
|
}
|
||||||
|
|
||||||
|
return command;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace racing_control
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 3: 修改 CMakeLists**
|
||||||
|
|
||||||
|
在 `/home/sunrise/yiliao_ws/src/racing_control/CMakeLists.txt` 中：
|
||||||
|
|
||||||
|
把 `racing_control_core` 改为：
|
||||||
|
|
||||||
|
```cmake
|
||||||
|
add_library(racing_control_core
|
||||||
|
src/candidate_waypoint_selector.cpp
|
||||||
|
src/final_home_fallback.cpp
|
||||||
|
)
|
||||||
|
```
|
||||||
|
|
||||||
|
把 `add_executable(racing_control ...)` 改为包含新文件：
|
||||||
|
|
||||||
|
```cmake
|
||||||
|
add_executable(racing_control
|
||||||
|
src/racing_control.cpp
|
||||||
|
src/candidate_waypoint_selector.cpp
|
||||||
|
src/final_home_fallback.cpp
|
||||||
|
)
|
||||||
|
```
|
||||||
|
|
||||||
|
把测试目标的 `target_sources(test_racing_control_helpers PRIVATE ...)` 改为：
|
||||||
|
|
||||||
|
```cmake
|
||||||
|
target_sources(test_racing_control_helpers PRIVATE
|
||||||
|
src/candidate_waypoint_selector.cpp
|
||||||
|
src/final_home_fallback.cpp
|
||||||
|
)
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 4: 运行 helper 测试**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon test --packages-select racing_control --ctest-args -R test_racing_control_helpers --output-on-failure"'
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：`test_racing_control_helpers` 通过。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Task 3: 增加最终保护参数和状态
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Modify: `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp`
|
||||||
|
- Modify: `/home/sunrise/yiliao_ws/src/racing_control/config/racing_control.yaml`
|
||||||
|
|
||||||
|
- [ ] **Step 1: 引入算法头文件**
|
||||||
|
|
||||||
|
在 `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp` include 区域加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
#include "racing_control/final_home_fallback.hpp"
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 2: 增加阶段枚举和名称**
|
||||||
|
|
||||||
|
在 `enum class Stage` 中加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
FinalHomeFallback,
|
||||||
|
```
|
||||||
|
|
||||||
|
在 `stageName(...)` 中加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
case Stage::FinalHomeFallback:
|
||||||
|
return "最终回家保护";
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 3: 加载参数**
|
||||||
|
|
||||||
|
在 `loadParameters()` 里，恢复参数附近加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
enable_final_home_fallback_ =
|
||||||
|
declare_parameter<bool>("enable_final_home_fallback", true);
|
||||||
|
final_home_distance_tolerance_ =
|
||||||
|
declare_parameter<double>("final_home_distance_tolerance", 0.2);
|
||||||
|
final_home_yaw_tolerance_ =
|
||||||
|
declare_parameter<double>("final_home_yaw_tolerance", 0.3);
|
||||||
|
final_home_forward_speed_ =
|
||||||
|
declare_parameter<double>("final_home_forward_speed", 0.12);
|
||||||
|
final_home_yaw_gain_ =
|
||||||
|
declare_parameter<double>("final_home_yaw_gain", 0.5);
|
||||||
|
final_home_max_turn_speed_ =
|
||||||
|
declare_parameter<double>("final_home_max_turn_speed", 0.7);
|
||||||
|
final_home_nav_retry_interval_sec_ =
|
||||||
|
declare_parameter<double>("final_home_nav_retry_interval_sec", 0.8);
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 4: 增加成员变量**
|
||||||
|
|
||||||
|
在 `RacingControl` 成员变量区加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
bool enable_final_home_fallback_{true};
|
||||||
|
bool final_home_nav_retry_in_flight_{false};
|
||||||
|
double final_home_distance_tolerance_{0.2};
|
||||||
|
double final_home_yaw_tolerance_{0.3};
|
||||||
|
double final_home_forward_speed_{0.12};
|
||||||
|
double final_home_yaw_gain_{0.5};
|
||||||
|
double final_home_max_turn_speed_{0.7};
|
||||||
|
double final_home_nav_retry_interval_sec_{0.8};
|
||||||
|
rclcpp::Time last_final_home_nav_retry_time_{0, 0, RCL_ROS_TIME};
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 5: 增加 YAML 默认值**
|
||||||
|
|
||||||
|
在 `/home/sunrise/yiliao_ws/src/racing_control/config/racing_control.yaml` 的恢复参数附近加入：
|
||||||
|
|
||||||
|
```yaml
|
||||||
|
# 最终回家保护：普通 Nav2/恢复失败后，低速直控回 home，同时定时探测 Nav2 是否恢复。
|
||||||
|
enable_final_home_fallback: true
|
||||||
|
final_home_distance_tolerance: 0.2
|
||||||
|
final_home_yaw_tolerance: 0.3
|
||||||
|
final_home_forward_speed: 0.12
|
||||||
|
final_home_yaw_gain: 0.5
|
||||||
|
final_home_max_turn_speed: 0.7
|
||||||
|
final_home_nav_retry_interval_sec: 0.8
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 6: 编译**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON"'
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：编译通过。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Task 4: 把最终失败切入 FinalHomeFallback
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Modify: `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp`
|
||||||
|
|
||||||
|
- [ ] **Step 1: 新增 `startFinalHomeFallback`**
|
||||||
|
|
||||||
|
在 `failRace(...)` 附近加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
void startFinalHomeFallback(const std::string & reason)
|
||||||
|
{
|
||||||
|
if (!enable_final_home_fallback_) {
|
||||||
|
stage_ = Stage::Failed;
|
||||||
|
recovery_in_progress_ = false;
|
||||||
|
cancelActiveFollowGoal();
|
||||||
|
publishRecoveryVelocity(0.0);
|
||||||
|
publishSign(sign_qr_disable_);
|
||||||
|
RCLCPP_ERROR(get_logger(), "race failed: %s", reason.c_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
stage_ = Stage::FinalHomeFallback;
|
||||||
|
recovery_in_progress_ = false;
|
||||||
|
final_home_nav_retry_in_flight_ = false;
|
||||||
|
last_final_home_nav_retry_time_ = now() - rclcpp::Duration::from_seconds(
|
||||||
|
final_home_nav_retry_interval_sec_);
|
||||||
|
if (recovery_timer_) {
|
||||||
|
recovery_timer_->cancel();
|
||||||
|
}
|
||||||
|
if (recovery_clear_wait_timer_) {
|
||||||
|
recovery_clear_wait_timer_->cancel();
|
||||||
|
}
|
||||||
|
recovery_after_clear_callback_ = nullptr;
|
||||||
|
recovery_costmap_clear_pending_ = 0;
|
||||||
|
cancelActiveFollowGoal();
|
||||||
|
publishRecoveryVelocity(0.0);
|
||||||
|
publishSign(sign_qr_disable_);
|
||||||
|
startStage(Stage::FinalHomeFallback, 0.0);
|
||||||
|
RCLCPP_ERROR(
|
||||||
|
get_logger(), "entering final home fallback instead of race failed: %s", reason.c_str());
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 2: 修改 `failRace`**
|
||||||
|
|
||||||
|
把 `failRace(...)` 函数体替换为：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
void failRace(const std::string & reason)
|
||||||
|
{
|
||||||
|
startFinalHomeFallback(reason);
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 3: 修改 tick 入口**
|
||||||
|
|
||||||
|
在 `tick()` 中，把终止保护后面加入最终保护 tick：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
if (!race_started_ || stage_ == Stage::Finished || stage_ == Stage::Failed) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (stage_ == Stage::FinalHomeFallback) {
|
||||||
|
tickFinalHomeFallback();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 4: 编译确认缺口**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON"'
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：如果还没写 `tickFinalHomeFallback()`，会因为函数缺失失败；继续 Task 5。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Task 5: 接入算法、发布直控速度、定时重试 Nav2
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Modify: `/home/sunrise/yiliao_ws/src/racing_control/src/racing_control.cpp`
|
||||||
|
|
||||||
|
- [ ] **Step 1: 新增 home 选择 helper**
|
||||||
|
|
||||||
|
在路线 helper 附近加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
geometry_msgs::msg::PoseStamped fallbackHomePose() const
|
||||||
|
{
|
||||||
|
if (selected_direction_ == RouteDirection::Clockwise ||
|
||||||
|
selected_direction_ == RouteDirection::Counterclockwise)
|
||||||
|
{
|
||||||
|
return selectedRoute().home_pose;
|
||||||
|
}
|
||||||
|
return clockwise_route_.home_pose;
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 2: 新增最终保护 tick**
|
||||||
|
|
||||||
|
在恢复相关函数附近加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
void tickFinalHomeFallback()
|
||||||
|
{
|
||||||
|
const auto current = currentPoseFromOdom();
|
||||||
|
if (!current) {
|
||||||
|
publishRecoveryVelocity(0.0);
|
||||||
|
RCLCPP_WARN_THROTTLE(
|
||||||
|
get_logger(), *get_clock(), 1000,
|
||||||
|
"final home fallback waiting for odom");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto home = fallbackHomePose();
|
||||||
|
const auto command = computeFinalHomeFallbackCommand(
|
||||||
|
*current, home, final_home_distance_tolerance_, final_home_yaw_tolerance_,
|
||||||
|
final_home_forward_speed_, final_home_yaw_gain_, final_home_max_turn_speed_);
|
||||||
|
|
||||||
|
if (command.reached) {
|
||||||
|
publishRecoveryVelocity(0.0);
|
||||||
|
finishRace();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (recovery_cmd_vel_pub_) {
|
||||||
|
recovery_cmd_vel_pub_->publish(command.twist);
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_WARN_THROTTLE(
|
||||||
|
get_logger(), *get_clock(), 1000,
|
||||||
|
"final home fallback direct control: distance=%.2f yaw_error=%.2f vx=%.2f wz=%.2f",
|
||||||
|
command.distance, command.yaw_error, command.twist.linear.x, command.twist.angular.z);
|
||||||
|
|
||||||
|
maybeRetryFinalHomeNavigation(home);
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 3: 新增 Nav2 定时重试**
|
||||||
|
|
||||||
|
在 `sendNavigateGoalAttempt(...)` 附近加入：
|
||||||
|
|
||||||
|
```cpp
|
||||||
|
void maybeRetryFinalHomeNavigation(const geometry_msgs::msg::PoseStamped & home)
|
||||||
|
{
|
||||||
|
if (final_home_nav_retry_in_flight_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if ((now() - last_final_home_nav_retry_time_).seconds() <
|
||||||
|
final_home_nav_retry_interval_sec_)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (!navigate_client_->wait_for_action_server(10ms)) {
|
||||||
|
last_final_home_nav_retry_time_ = now();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
final_home_nav_retry_in_flight_ = true;
|
||||||
|
last_final_home_nav_retry_time_ = now();
|
||||||
|
|
||||||
|
NavigateToPose::Goal goal;
|
||||||
|
goal.pose = stampPose(home);
|
||||||
|
|
||||||
|
auto options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
|
||||||
|
options.goal_response_callback =
|
||||||
|
[this](NavigateGoalHandle::SharedPtr goal_handle) {
|
||||||
|
if (stage_ != Stage::FinalHomeFallback) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (!goal_handle) {
|
||||||
|
final_home_nav_retry_in_flight_ = false;
|
||||||
|
RCLCPP_WARN(get_logger(), "final home fallback Nav2 retry rejected");
|
||||||
|
}
|
||||||
|
};
|
||||||
|
options.result_callback =
|
||||||
|
[this](const NavigateGoalHandle::WrappedResult & result) {
|
||||||
|
if (stage_ != Stage::FinalHomeFallback) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
final_home_nav_retry_in_flight_ = false;
|
||||||
|
if (result.code == rclcpp_action::ResultCode::SUCCEEDED) {
|
||||||
|
publishRecoveryVelocity(0.0);
|
||||||
|
finishRace();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
RCLCPP_WARN(get_logger(), "final home fallback Nav2 retry failed; keeping direct control");
|
||||||
|
};
|
||||||
|
|
||||||
|
navigate_client_->async_send_goal(goal, options);
|
||||||
|
RCLCPP_INFO(get_logger(), "final home fallback sent Nav2 home retry");
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
- [ ] **Step 4: 编译**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON"'
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：编译通过。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### Task 6: 验证和检查
|
||||||
|
|
||||||
|
**Files:**
|
||||||
|
- Test only.
|
||||||
|
|
||||||
|
- [ ] **Step 1: 跑 helper 测试**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'bash -lc "source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash && cd /home/sunrise/yiliao_ws && colcon test --packages-select racing_control --ctest-args -R test_racing_control_helpers --output-on-failure"'
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：通过。
|
||||||
|
|
||||||
|
- [ ] **Step 2: 查看测试结果**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'bash -lc "cd /home/sunrise/yiliao_ws && colcon test-result --test-result-base build/racing_control --verbose"'
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：`racing_control` 无失败。
|
||||||
|
|
||||||
|
- [ ] **Step 3: 检查改动范围**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'cd /home/sunrise/yiliao_ws && git status --short src/racing_control'
|
||||||
|
```
|
||||||
|
|
||||||
|
预期：只出现 `racing_control` 的源码、头文件、CMake、yaml、测试改动。
|
||||||
|
|
||||||
|
- [ ] **Step 4: 审查最终 diff**
|
||||||
|
|
||||||
|
运行：
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ssh sunrise@192.168.10.210 'cd /home/sunrise/yiliao_ws && git diff -- src/racing_control | sed -n "1,280p"'
|
||||||
|
```
|
||||||
|
|
||||||
|
重点检查：
|
||||||
|
- 核心算法只在 `final_home_fallback.hpp/.cpp`，不在 `racing_control.cpp`。
|
||||||
|
- `racing_control.cpp` 只做阶段切换、参数读取、调用算法、发布速度、重试 Nav2。
|
||||||
|
- `FinalHomeFallback` 阶段不会被 `tick()` 提前 return。
|
||||||
|
- 进入距离阈值 `0.2m` 后一定发布 0 速度并 `finishRace()`。
|
||||||
|
- Nav2 重试有 `final_home_nav_retry_in_flight_` 和 `final_home_nav_retry_interval_sec_`，不会每帧刷 action。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
### 自检
|
||||||
|
|
||||||
|
- 覆盖需求：已覆盖最终失败兜底、每帧按 odom-home 计算、0.3rad/0.2m 阈值、距离到达即停止、Nav2 周期性重试、成功后切回正常完成。
|
||||||
|
- 文件边界：核心算法作为新文件 `final_home_fallback.hpp/.cpp`，主程序不承载算法。
|
||||||
|
- 占位符检查：无 `TBD`、`TODO` 或“之后实现”。
|
||||||
|
- 类型一致性：函数名 `computeFinalHomeFallbackCommand`、结构体 `FinalHomeFallbackCommand`、参数名和阶段名在计划内一致。
|
||||||
Binary file not shown.
|
Before Width: | Height: | Size: 389 KiB After Width: | Height: | Size: 37 KiB |
@@ -15,7 +15,7 @@
|
|||||||
use_gps_ts: false #雷达是否使用GPS授时
|
use_gps_ts: false #雷达是否使用GPS授时
|
||||||
scan_topic: /scan #设置激光数据topic名称
|
scan_topic: /scan #设置激光数据topic名称
|
||||||
interface_selection: serial #接口选择:net 为网口,serial 为串口。
|
interface_selection: serial #接口选择:net 为网口,serial 为串口。
|
||||||
serial_port_: /dev/ttyCH343USB0 #串口连接时的串口号
|
serial_port_: /dev/radar #串口连接时的串口号
|
||||||
high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。
|
high_reflection: false #M10_P雷达需填写该值,若不确定，请联系技术支持。
|
||||||
compensation: false #M10系列是否使用角度补偿功能
|
compensation: false #M10系列是否使用角度补偿功能
|
||||||
pubScan: true #是否发布scan话题
|
pubScan: true #是否发布scan话题
|
||||||
|
|||||||
@@ -1009,8 +1009,8 @@ namespace lslidar_driver
|
|||||||
scan->ranges.assign(scan_num, std::numeric_limits<float>::infinity());
|
scan->ranges.assign(scan_num, std::numeric_limits<float>::infinity());
|
||||||
scan->intensities.reserve(scan_num);
|
scan->intensities.reserve(scan_num);
|
||||||
scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
|
scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
|
||||||
// scan->scan_time = scan_time;
|
scan->scan_time = scan_time;
|
||||||
// scan->time_increment = scan_time / (double)(count_num);
|
scan->time_increment = scan_time / static_cast<double>(scan_num);
|
||||||
|
|
||||||
for (int k = 0; k < scan_num; k++)
|
for (int k = 0; k < scan_num; k++)
|
||||||
{
|
{
|
||||||
@@ -1168,8 +1168,8 @@ namespace lslidar_driver
|
|||||||
scan->ranges.assign(scan_num, std::numeric_limits<float>::infinity());
|
scan->ranges.assign(scan_num, std::numeric_limits<float>::infinity());
|
||||||
scan->intensities.reserve(scan_num);
|
scan->intensities.reserve(scan_num);
|
||||||
scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
|
scan->intensities.assign(scan_num, std::numeric_limits<float>::infinity());
|
||||||
scan->scan_time = 0.1;
|
scan->scan_time = scan_time;
|
||||||
scan->time_increment = 0.1 / (double)(scan_num - 1);
|
scan->time_increment = scan_time / static_cast<double>(scan_num - 1);
|
||||||
|
|
||||||
int start_num = floor(angle_able_min * count_num / 360);
|
int start_num = floor(angle_able_min * count_num / 360);
|
||||||
int end_num = floor(angle_able_max * count_num / 360);
|
int end_num = floor(angle_able_max * count_num / 360);
|
||||||
|
|||||||
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 内容） |
|
||||||
BIN
src/map/map06.png
Normal file
BIN
src/map/map06.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 4.9 KiB |
BIN
src/map/nav2_costmap_binary 02.png
Normal file
BIN
src/map/nav2_costmap_binary 02.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 3.0 KiB |
BIN
src/map/nav2_costmap_binary 03.png
Normal file
BIN
src/map/nav2_costmap_binary 03.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 8.3 KiB |
BIN
src/map/nav2_costmap_binary 04.png
Normal file
BIN
src/map/nav2_costmap_binary 04.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 6.1 KiB |
BIN
src/map/nav2_costmap_binary 05.png
Normal file
BIN
src/map/nav2_costmap_binary 05.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 5.7 KiB |
BIN
src/map/nav2_costmap_binary_01.png
Normal file
BIN
src/map/nav2_costmap_binary_01.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 8.6 KiB |
BIN
src/map/nav2_costmap_binary_06.png
Normal file
BIN
src/map/nav2_costmap_binary_06.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 5.5 KiB |
7
src/map/nav2_costmap_map.yaml
Normal file
7
src/map/nav2_costmap_map.yaml
Normal file
@@ -0,0 +1,7 @@
|
|||||||
|
image: map06.png
|
||||||
|
mode: trinary
|
||||||
|
resolution: 0.01
|
||||||
|
origin: [0.0, 0.0, 0.0]
|
||||||
|
negate: 0
|
||||||
|
occupied_thresh: 0.65
|
||||||
|
free_thresh: 0.25
|
||||||
@@ -16,6 +16,34 @@ find_package(obstacle_scanner REQUIRED)
|
|||||||
find_package(geometry_msgs REQUIRED)
|
find_package(geometry_msgs REQUIRED)
|
||||||
find_package(nav_msgs REQUIRED)
|
find_package(nav_msgs REQUIRED)
|
||||||
find_package(std_msgs REQUIRED)
|
find_package(std_msgs REQUIRED)
|
||||||
|
find_package(rcl_interfaces REQUIRED)
|
||||||
|
find_package(yaml-cpp REQUIRED)
|
||||||
|
find_package(nav2_msgs REQUIRED)
|
||||||
|
find_package(rclcpp_action REQUIRED)
|
||||||
|
|
||||||
|
add_library(nav2_profile_loader
|
||||||
|
src/nav2_profile_loader.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(nav2_profile_loader PUBLIC
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
target_link_libraries(nav2_profile_loader yaml-cpp)
|
||||||
|
ament_target_dependencies(nav2_profile_loader
|
||||||
|
rclcpp
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(trajectory_guard_lib
|
||||||
|
src/trajectory_guard.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(trajectory_guard_lib PUBLIC
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
ament_target_dependencies(trajectory_guard_lib
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
)
|
||||||
|
|
||||||
add_library(obstacle_array_layer SHARED
|
add_library(obstacle_array_layer SHARED
|
||||||
src/obstacle_array_layer.cpp
|
src/obstacle_array_layer.cpp
|
||||||
@@ -39,14 +67,53 @@ ament_target_dependencies(obstacle_array_layer
|
|||||||
|
|
||||||
pluginlib_export_plugin_description_file(nav2_costmap_2d obstacle_nav2_plugins.xml)
|
pluginlib_export_plugin_description_file(nav2_costmap_2d obstacle_nav2_plugins.xml)
|
||||||
|
|
||||||
install(TARGETS obstacle_array_layer
|
add_executable(nav2_profile_tuner
|
||||||
|
src/nav2_profile_tuner.cpp
|
||||||
|
)
|
||||||
|
target_link_libraries(nav2_profile_tuner nav2_profile_loader)
|
||||||
|
ament_target_dependencies(nav2_profile_tuner
|
||||||
|
rclcpp
|
||||||
|
rcl_interfaces
|
||||||
|
std_msgs
|
||||||
|
)
|
||||||
|
|
||||||
|
add_executable(trajectory_guard_node
|
||||||
|
src/trajectory_guard_node.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(trajectory_guard_node PUBLIC
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
target_link_libraries(trajectory_guard_node trajectory_guard_lib)
|
||||||
|
ament_target_dependencies(trajectory_guard_node
|
||||||
|
geometry_msgs
|
||||||
|
nav2_msgs
|
||||||
|
nav_msgs
|
||||||
|
rclcpp
|
||||||
|
rclcpp_action
|
||||||
|
tf2
|
||||||
|
)
|
||||||
|
|
||||||
|
install(TARGETS
|
||||||
|
nav2_profile_loader
|
||||||
|
trajectory_guard_lib
|
||||||
|
obstacle_array_layer
|
||||||
|
nav2_profile_tuner
|
||||||
|
trajectory_guard_node
|
||||||
ARCHIVE DESTINATION lib
|
ARCHIVE DESTINATION lib
|
||||||
LIBRARY DESTINATION lib
|
LIBRARY DESTINATION lib
|
||||||
RUNTIME DESTINATION bin
|
RUNTIME DESTINATION lib/${PROJECT_NAME}
|
||||||
)
|
)
|
||||||
install(DIRECTORY include/ DESTINATION include)
|
install(DIRECTORY include/ DESTINATION include)
|
||||||
install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME})
|
install(FILES obstacle_nav2_plugins.xml DESTINATION share/${PROJECT_NAME})
|
||||||
|
|
||||||
|
# Python scripts
|
||||||
|
install(PROGRAMS
|
||||||
|
scripts/initial_pose_to_tf.py
|
||||||
|
scripts/static_map_publisher.py
|
||||||
|
DESTINATION share/${PROJECT_NAME}/scripts
|
||||||
|
)
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
if(BUILD_TESTING)
|
||||||
find_package(ament_cmake_gtest REQUIRED)
|
find_package(ament_cmake_gtest REQUIRED)
|
||||||
ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp
|
ament_add_gtest(test_obstacle_array_layer test/test_obstacle_array_layer.cpp
|
||||||
@@ -67,6 +134,27 @@ if(BUILD_TESTING)
|
|||||||
nav_msgs
|
nav_msgs
|
||||||
std_msgs
|
std_msgs
|
||||||
)
|
)
|
||||||
|
|
||||||
|
ament_add_gtest(test_nav2_profile_loader test/test_nav2_profile_loader.cpp
|
||||||
|
src/nav2_profile_loader.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(test_nav2_profile_loader PRIVATE
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
)
|
||||||
|
target_link_libraries(test_nav2_profile_loader yaml-cpp)
|
||||||
|
ament_target_dependencies(test_nav2_profile_loader
|
||||||
|
rclcpp
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_add_gtest(test_trajectory_guard test/test_trajectory_guard.cpp)
|
||||||
|
target_include_directories(test_trajectory_guard PRIVATE
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
)
|
||||||
|
target_link_libraries(test_trajectory_guard trajectory_guard_lib)
|
||||||
|
ament_target_dependencies(test_trajectory_guard
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
install(DIRECTORY config launch behavior_tree
|
install(DIRECTORY config launch behavior_tree
|
||||||
|
|||||||
@@ -0,0 +1,41 @@
|
|||||||
|
<!--
|
||||||
|
Ackermann NavigateThroughPoses behavior tree.
|
||||||
|
|
||||||
|
Planning failure is retried once after the global costmap has been cleared
|
||||||
|
and given one update period. FollowPath is not replanned periodically while
|
||||||
|
it is RUNNING. A controller failure gets one local-costmap retry, then one
|
||||||
|
short backup followed by one final through-poses plan and follow attempt.
|
||||||
|
Any final failure aborts the navigation action.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<Sequence name="NavigateThroughPosesOnce">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
|
||||||
|
<ReactiveSequence>
|
||||||
|
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
|
||||||
|
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</ReactiveSequence>
|
||||||
|
<Sequence name="ClearGlobalAndWait">
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
<Wait wait_duration="1"/>
|
||||||
|
</Sequence>
|
||||||
|
</RecoveryNode>
|
||||||
|
|
||||||
|
<Fallback name="FollowOrSingleBackupReplan">
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowWithLocalClear">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
|
||||||
|
<Sequence name="BackupReplanAndFinalFollow">
|
||||||
|
<BackUp backup_dist="0.04" backup_speed="0.20"/>
|
||||||
|
<ReactiveSequence>
|
||||||
|
<RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.7"/>
|
||||||
|
<ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
|
||||||
|
</ReactiveSequence>
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
</Sequence>
|
||||||
|
</Fallback>
|
||||||
|
</Sequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -1,33 +1,31 @@
|
|||||||
<!--
|
<!--
|
||||||
阿克曼底盘行为树：无原地旋转（Spin）
|
Ackermann NavigateToPose behavior tree.
|
||||||
恢复行为序列：清除代价地图 → 后退 → 等待
|
|
||||||
|
Compute the path once and keep the same path while FollowPath is RUNNING.
|
||||||
|
If following fails, clear the local costmap and retry the same path once.
|
||||||
|
If that also fails, back up 0.04 m, compute one final path, and follow it
|
||||||
|
once. Any failure in the final branch ends the navigation action.
|
||||||
-->
|
-->
|
||||||
<root main_tree_to_execute="MainTree">
|
<root main_tree_to_execute="MainTree">
|
||||||
<BehaviorTree ID="MainTree">
|
<BehaviorTree ID="MainTree">
|
||||||
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
<Sequence name="NavigateWithSingleBackupReplan">
|
||||||
<PipelineSequence name="NavigateWithReplanning">
|
<RecoveryNode number_of_retries="1" name="ComputeInitialPath">
|
||||||
<RateController hz="1.0">
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
</RecoveryNode>
|
||||||
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
|
||||||
</RecoveryNode>
|
<Fallback name="FollowOrSingleBackupReplan">
|
||||||
</RateController>
|
<RecoveryNode number_of_retries="1" name="FollowWithLocalClear">
|
||||||
<RecoveryNode number_of_retries="1" name="FollowPath">
|
|
||||||
<FollowPath path="{path}" controller_id="FollowPath"/>
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
</RecoveryNode>
|
</RecoveryNode>
|
||||||
</PipelineSequence>
|
|
||||||
<ReactiveFallback name="RecoveryFallback">
|
<Sequence name="BackupReplanAndFinalFollow">
|
||||||
<GoalUpdated/>
|
<BackUp backup_dist="0.04" backup_speed="0.20"/>
|
||||||
<RoundRobin name="RecoveryActions">
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
<Sequence name="ClearingActions">
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
</Sequence>
|
||||||
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
</Fallback>
|
||||||
</Sequence>
|
</Sequence>
|
||||||
<BackUp backup_dist="0.80" backup_speed="0.18"/>
|
|
||||||
<Wait wait_duration="0.3"/>
|
|
||||||
</RoundRobin>
|
|
||||||
</ReactiveFallback>
|
|
||||||
</RecoveryNode>
|
|
||||||
</BehaviorTree>
|
</BehaviorTree>
|
||||||
</root>
|
</root>
|
||||||
|
|||||||
@@ -0,0 +1,33 @@
|
|||||||
|
<!--
|
||||||
|
阿克曼底盘行为树：无原地旋转（Spin）
|
||||||
|
恢复行为序列：清除代价地图 → 后退 → 等待
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<PipelineSequence name="NavigateWithReplanning">
|
||||||
|
<RateController hz="1.0">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</RateController>
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</PipelineSequence>
|
||||||
|
<ReactiveFallback name="RecoveryFallback">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<RoundRobin name="RecoveryActions">
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</Sequence>
|
||||||
|
<BackUp backup_dist="0.80" backup_speed="0.18"/>
|
||||||
|
<Wait wait_duration="0.3"/>
|
||||||
|
</RoundRobin>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
@@ -0,0 +1,35 @@
|
|||||||
|
<!--
|
||||||
|
Ackermann NavigateToPose behavior tree.
|
||||||
|
|
||||||
|
Compute the path once and keep the same path while FollowPath is RUNNING.
|
||||||
|
A new global plan is requested only after FollowPath returns FAILURE and the
|
||||||
|
outer RecoveryNode starts another navigation attempt. This avoids replacing
|
||||||
|
a valid path every second while MPPI is still evaluating it.
|
||||||
|
|
||||||
|
Recovery actions intentionally do not include Spin or BackUp because this
|
||||||
|
robot uses an Ackermann drive model.
|
||||||
|
-->
|
||||||
|
<root main_tree_to_execute="MainTree">
|
||||||
|
<BehaviorTree ID="MainTree">
|
||||||
|
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
|
||||||
|
<Sequence name="NavigateOnce">
|
||||||
|
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
|
||||||
|
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
<RecoveryNode number_of_retries="1" name="FollowPath">
|
||||||
|
<FollowPath path="{path}" controller_id="FollowPath"/>
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
</RecoveryNode>
|
||||||
|
</Sequence>
|
||||||
|
<ReactiveFallback name="RecoveryFallback">
|
||||||
|
<GoalUpdated/>
|
||||||
|
<Sequence name="ClearingActions">
|
||||||
|
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
|
||||||
|
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
|
||||||
|
<Wait wait_duration="1"/>
|
||||||
|
</Sequence>
|
||||||
|
</ReactiveFallback>
|
||||||
|
</RecoveryNode>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
||||||
17
src/navigation/obstacle_nav2/config/fastdds_udp_only.xml
Normal file
17
src/navigation/obstacle_nav2/config/fastdds_udp_only.xml
Normal file
@@ -0,0 +1,17 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8" ?>
|
||||||
|
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
|
||||||
|
<transport_descriptors>
|
||||||
|
<transport_descriptor>
|
||||||
|
<transport_id>udp_transport</transport_id>
|
||||||
|
<type>UDPv4</type>
|
||||||
|
</transport_descriptor>
|
||||||
|
</transport_descriptors>
|
||||||
|
<participant profile_name="udp_only_participant" is_default_profile="true">
|
||||||
|
<rtps>
|
||||||
|
<useBuiltinTransports>false</useBuiltinTransports>
|
||||||
|
<userTransports>
|
||||||
|
<transport_id>udp_transport</transport_id>
|
||||||
|
</userTransports>
|
||||||
|
</rtps>
|
||||||
|
</participant>
|
||||||
|
</profiles>
|
||||||
155
src/navigation/obstacle_nav2/config/json/test1.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test1.json
Normal file
@@ -0,0 +1,155 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-07-24T12:43:47.020Z",
|
||||||
|
"count": 3,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_001",
|
||||||
|
"captured_at": "2026-07-24T12:43:16.569Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 4.398705354995568,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 1.4830508474576272,
|
||||||
|
"y": 0.18107786016949148,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.0383765195035876,
|
||||||
|
"w": 0.99926335004882
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 4.398705354995568
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_002",
|
||||||
|
"captured_at": "2026-07-24T12:43:25.804Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 15.109575122340486,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 3.2468220338983054,
|
||||||
|
"y": 0.2393405720338984,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.13147417510981546,
|
||||||
|
"w": 0.9913195959322068
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 15.109575122340486
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_003",
|
||||||
|
"captured_at": "2026-07-24T12:43:44.649Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 64.70797884297878,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.470338983050848,
|
||||||
|
"y": 1.3357388771186443,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.5351485964909909,
|
||||||
|
"w": 0.844757941468278
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 64.70797884297878
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/navigation/obstacle_nav2/config/json/test2.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test2.json
Normal file
@@ -0,0 +1,155 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-07-24T12:44:38.669Z",
|
||||||
|
"count": 3,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_004",
|
||||||
|
"captured_at": "2026-07-24T12:44:18.861Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 12.938056317186437,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 1.6472457627118644,
|
||||||
|
"y": 0.5094676906779659,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.11266611144839717,
|
||||||
|
"w": 0.9936329037079525
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 12.938056317186437
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_005",
|
||||||
|
"captured_at": "2026-07-24T12:44:28.102Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 22.02305549681122,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 3.1673728813559325,
|
||||||
|
"y": 0.5677304025423728,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.1910064921196254,
|
||||||
|
"w": 0.981588773350712
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 22.02305549681122
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_006",
|
||||||
|
"captured_at": "2026-07-24T12:44:37.758Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 55.12467165539786,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.4173728813559325,
|
||||||
|
"y": 1.394001588983051,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.4627133768932059,
|
||||||
|
"w": 0.8865079417828619
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 55.12467165539786
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/navigation/obstacle_nav2/config/json/test3.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test3.json
Normal file
@@ -0,0 +1,155 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-07-24T12:45:16.793Z",
|
||||||
|
"count": 3,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_007",
|
||||||
|
"captured_at": "2026-07-24T12:44:56.224Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 10.701350723899163,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 1.461864406779661,
|
||||||
|
"y": 0.5783236228813559,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.09325122181983936,
|
||||||
|
"w": 0.9956426113968341
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 10.701350723899163
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_008",
|
||||||
|
"captured_at": "2026-07-24T12:45:04.283Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 20.94747058695114,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 3.0826271186440675,
|
||||||
|
"y": 0.7319253177966101,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.18178477679916172,
|
||||||
|
"w": 0.9833383420390354
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 20.94747058695114
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_009",
|
||||||
|
"captured_at": "2026-07-24T12:45:15.848Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 85.03025927188973,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.735169491525424,
|
||||||
|
"y": 1.3092558262711866,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.6757848709593751,
|
||||||
|
"w": 0.7370989134318546
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 85.03025927188973
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/navigation/obstacle_nav2/config/json/test4.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test4.json
Normal file
@@ -0,0 +1,155 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-07-24T12:47:26.413Z",
|
||||||
|
"count": 3,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_015",
|
||||||
|
"captured_at": "2026-07-24T12:47:14.682Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 17.709724523862732,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 1.5625,
|
||||||
|
"y": 0.7001456567796612,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.15393202146824025,
|
||||||
|
"w": 0.9880814403513009
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 17.709724523862732
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_016",
|
||||||
|
"captured_at": "2026-07-24T12:47:18.969Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 22.036226940145426,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 2.9978813559322033,
|
||||||
|
"y": 0.9490863347457628,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.1911193171514086,
|
||||||
|
"w": 0.9815668120976683
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 22.036226940145426
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_017",
|
||||||
|
"captured_at": "2026-07-24T12:47:24.482Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 75.9637565320735,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.523305084745763,
|
||||||
|
"y": 1.3304422669491527,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.6154122094026355,
|
||||||
|
"w": 0.7882054380161093
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 75.9637565320735
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/navigation/obstacle_nav2/config/json/test5.json
Normal file
155
src/navigation/obstacle_nav2/config/json/test5.json
Normal file
@@ -0,0 +1,155 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-07-24T12:48:05.057Z",
|
||||||
|
"count": 3,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_019",
|
||||||
|
"captured_at": "2026-07-24T12:47:52.274Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 9.130176482278682,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 1.4088983050847457,
|
||||||
|
"y": 0.1387049788135596,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.0795915470489837,
|
||||||
|
"w": 0.9968275606334074
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 9.130176482278682
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_020",
|
||||||
|
"captured_at": "2026-07-24T12:47:56.654Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 15.2551187030578,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 3.0614406779661016,
|
||||||
|
"y": 0.18637447033898297,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.1327331510254088,
|
||||||
|
"w": 0.9911518100769761
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 15.2551187030578
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_021",
|
||||||
|
"captured_at": "2026-07-24T12:48:03.562Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 89.99999999999986,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "map",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.703389830508474,
|
||||||
|
"y": 1.3516287076271185,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.7071067811865467,
|
||||||
|
"w": 0.7071067811865485
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 89.99999999999986
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -112,8 +112,8 @@ controller_server:
|
|||||||
CostCritic:
|
CostCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 4.5
|
cost_weight: 6.0
|
||||||
critical_cost: 300.0
|
critical_cost: 250.0
|
||||||
consider_footprint: true
|
consider_footprint: true
|
||||||
collision_cost: 1000000.0
|
collision_cost: 1000000.0
|
||||||
near_goal_distance: 0.8
|
near_goal_distance: 0.8
|
||||||
@@ -134,7 +134,7 @@ controller_server:
|
|||||||
PathAlignCritic:
|
PathAlignCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 16.0
|
cost_weight: 8.0
|
||||||
max_path_occupancy_ratio: 0.07
|
max_path_occupancy_ratio: 0.07
|
||||||
trajectory_point_step: 4
|
trajectory_point_step: 4
|
||||||
threshold_to_consider: 0.45
|
threshold_to_consider: 0.45
|
||||||
@@ -144,14 +144,14 @@ controller_server:
|
|||||||
PathFollowCritic:
|
PathFollowCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 6.0
|
cost_weight: 4.0
|
||||||
offset_from_furthest: 4
|
offset_from_furthest: 4
|
||||||
threshold_to_consider: 1.0
|
threshold_to_consider: 1.0
|
||||||
|
|
||||||
PathAngleCritic:
|
PathAngleCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 6.0
|
cost_weight: 5.0
|
||||||
offset_from_furthest: 5
|
offset_from_furthest: 5
|
||||||
threshold_to_consider: 0.45
|
threshold_to_consider: 0.45
|
||||||
max_angle_to_furthest: 1.2
|
max_angle_to_furthest: 1.2
|
||||||
@@ -160,7 +160,7 @@ controller_server:
|
|||||||
PreferForwardCritic:
|
PreferForwardCritic:
|
||||||
enabled: true
|
enabled: true
|
||||||
cost_power: 1
|
cost_power: 1
|
||||||
cost_weight: 2.0
|
cost_weight: 1.0
|
||||||
threshold_to_consider: 0.4
|
threshold_to_consider: 0.4
|
||||||
|
|
||||||
controller_server_rclcpp_node:
|
controller_server_rclcpp_node:
|
||||||
@@ -170,7 +170,7 @@ controller_server_rclcpp_node:
|
|||||||
local_costmap:
|
local_costmap:
|
||||||
local_costmap:
|
local_costmap:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
update_frequency: 8.0
|
update_frequency: 10.0
|
||||||
publish_frequency: 4.0
|
publish_frequency: 4.0
|
||||||
transform_tolerance: 0.5
|
transform_tolerance: 0.5
|
||||||
global_frame: odom
|
global_frame: odom
|
||||||
@@ -188,12 +188,13 @@ local_costmap:
|
|||||||
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
||||||
enabled: true
|
enabled: true
|
||||||
topic: /obstacles
|
topic: /obstacles
|
||||||
obstacle_timeout: 0.5
|
obstacle_timeout: 1.0
|
||||||
transform_tolerance: 0.2
|
transform_tolerance: 0.02
|
||||||
default_obstacle_radius: 0.05
|
default_obstacle_radius: 0.05
|
||||||
minimum_obstacle_radius: 0.02
|
minimum_obstacle_radius: 0.02
|
||||||
maximum_obstacle_radius: 0.06
|
maximum_obstacle_radius: 0.06
|
||||||
extra_inflation: 0.02
|
extra_inflation: 0.02
|
||||||
|
retain_previous_on_empty_snapshot: true
|
||||||
inflation_layer:
|
inflation_layer:
|
||||||
plugin: "nav2_costmap_2d::InflationLayer"
|
plugin: "nav2_costmap_2d::InflationLayer"
|
||||||
cost_scaling_factor: 3.0
|
cost_scaling_factor: 3.0
|
||||||
@@ -209,8 +210,8 @@ local_costmap:
|
|||||||
global_costmap:
|
global_costmap:
|
||||||
global_costmap:
|
global_costmap:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
update_frequency: 2.0
|
update_frequency: 5.0
|
||||||
publish_frequency: 1.0
|
publish_frequency: 3.0
|
||||||
transform_tolerance: 0.5
|
transform_tolerance: 0.5
|
||||||
global_frame: odom
|
global_frame: odom
|
||||||
robot_base_frame: base_footprint
|
robot_base_frame: base_footprint
|
||||||
@@ -227,12 +228,13 @@ global_costmap:
|
|||||||
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
plugin: "obstacle_nav2::ObstacleArrayLayer"
|
||||||
enabled: true
|
enabled: true
|
||||||
topic: /obstacles
|
topic: /obstacles
|
||||||
obstacle_timeout: 0.5
|
obstacle_timeout: 1.0
|
||||||
transform_tolerance: 0.2
|
transform_tolerance: 0.02
|
||||||
default_obstacle_radius: 0.05
|
default_obstacle_radius: 0.05
|
||||||
minimum_obstacle_radius: 0.02
|
minimum_obstacle_radius: 0.02
|
||||||
maximum_obstacle_radius: 0.50
|
maximum_obstacle_radius: 0.50
|
||||||
extra_inflation: 0.02
|
extra_inflation: 0.02
|
||||||
|
retain_previous_on_empty_snapshot: true
|
||||||
inflation_layer:
|
inflation_layer:
|
||||||
plugin: "nav2_costmap_2d::InflationLayer"
|
plugin: "nav2_costmap_2d::InflationLayer"
|
||||||
cost_scaling_factor: 3.0
|
cost_scaling_factor: 3.0
|
||||||
@@ -264,10 +266,10 @@ planner_server:
|
|||||||
analytic_expansion_max_length: 2.0
|
analytic_expansion_max_length: 2.0
|
||||||
minimum_turning_radius: 0.60
|
minimum_turning_radius: 0.60
|
||||||
reverse_penalty: 2.2
|
reverse_penalty: 2.2
|
||||||
change_penalty: 0.5
|
change_penalty: 0.0
|
||||||
non_straight_penalty: 0.85
|
non_straight_penalty: 0.8
|
||||||
cost_penalty: 4.5
|
cost_penalty: 5.0
|
||||||
retrospective_penalty: 0.015
|
retrospective_penalty: 0.01
|
||||||
lookup_table_size: 5.0
|
lookup_table_size: 5.0
|
||||||
cache_obstacle_heuristic: false
|
cache_obstacle_heuristic: false
|
||||||
viz_expansions: false
|
viz_expansions: false
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -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: 15.0
|
||||||
|
FollowPath:
|
||||||
|
plugin: "nav2_mppi_controller::MPPIController"
|
||||||
|
time_steps: 40
|
||||||
|
model_dt: 0.06666666666666666
|
||||||
|
batch_size: 900
|
||||||
|
vx_std: 0.24
|
||||||
|
vy_std: 0.0
|
||||||
|
wz_std: 0.48
|
||||||
|
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.6
|
||||||
|
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: 11.0
|
||||||
|
threshold_to_consider: 0.5
|
||||||
|
CostCritic:
|
||||||
|
enabled: true
|
||||||
|
cost_power: 1
|
||||||
|
cost_weight: 4.0
|
||||||
|
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: 11.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.20
|
||||||
|
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.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: 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.45
|
||||||
|
reverse_penalty: 4.0
|
||||||
|
change_penalty: 1.2
|
||||||
|
non_straight_penalty: 0.7
|
||||||
|
cost_penalty: 3.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: 700
|
||||||
|
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","wait","backup"]
|
||||||
|
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
|
||||||
345
src/navigation/obstacle_nav2/config/nav2_profile_10.yaml
Normal file
345
src/navigation/obstacle_nav2/config/nav2_profile_10.yaml
Normal file
@@ -0,0 +1,345 @@
|
|||||||
|
# ============================================================================
|
||||||
|
# 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: 10.0
|
||||||
|
FollowPath:
|
||||||
|
plugin: "nav2_mppi_controller::MPPIController"
|
||||||
|
time_steps: 30
|
||||||
|
model_dt: 0.1
|
||||||
|
batch_size: 700
|
||||||
|
vx_std: 0.50
|
||||||
|
vy_std: 0.0
|
||||||
|
wz_std: 0.80
|
||||||
|
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: 1.8
|
||||||
|
critical_cost: 250.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: 4.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: 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.25
|
||||||
|
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.3
|
||||||
|
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: 1.0
|
||||||
|
non_straight_penalty: 1.2
|
||||||
|
cost_penalty: 10.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: 10
|
||||||
|
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.25, 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,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
|
||||||
345
src/navigation/obstacle_nav2/config/nav2_profile_11.yaml
Normal file
345
src/navigation/obstacle_nav2/config/nav2_profile_11.yaml
Normal file
@@ -0,0 +1,345 @@
|
|||||||
|
# ============================================================================
|
||||||
|
# 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: 10.0
|
||||||
|
FollowPath:
|
||||||
|
plugin: "nav2_mppi_controller::MPPIController"
|
||||||
|
time_steps: 30
|
||||||
|
model_dt: 0.1
|
||||||
|
batch_size: 700
|
||||||
|
vx_std: 0.40
|
||||||
|
vy_std: 0.0
|
||||||
|
wz_std: 0.70
|
||||||
|
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: 1.8
|
||||||
|
critical_cost: 250.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: 4.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: 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.25
|
||||||
|
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.3
|
||||||
|
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: 1.0
|
||||||
|
non_straight_penalty: 1.2
|
||||||
|
cost_penalty: 6.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: 10
|
||||||
|
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.25, 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
|
||||||
595
src/navigation/obstacle_nav2/config/q_waypoint_selector.py
Normal file
595
src/navigation/obstacle_nav2/config/q_waypoint_selector.py
Normal file
@@ -0,0 +1,595 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Q_i candidate waypoint selector for Nav2 NavigateToPose.
|
||||||
|
|
||||||
|
用途
|
||||||
|
----
|
||||||
|
这个节点用于比赛/实车场景下的多路线候选导航。它会从同一个 JSON
|
||||||
|
目录中读取多个 ``*.json`` 文件。每个 JSON 文件表示一条完整路线，文件
|
||||||
|
中的 ``points`` 数组表示该路线按顺序经过的导航点。
|
||||||
|
|
||||||
|
多 JSON 编组规则
|
||||||
|
--------------
|
||||||
|
假设目录中存在多个文件：
|
||||||
|
|
||||||
|
test1.json: p_0, p_1, p_2, ...
|
||||||
|
test2.json: p_0, p_1, p_2, ...
|
||||||
|
test3.json: p_0, p_1, p_2, ...
|
||||||
|
|
||||||
|
节点会把每个文件的第 i 个点组合成 Q_i：
|
||||||
|
|
||||||
|
Q_0 = [test1[0], test2[0], test3[0], ...]
|
||||||
|
Q_1 = [test1[1], test2[1], test3[1], ...]
|
||||||
|
Q_2 = [test1[2], test2[2], test3[2], ...]
|
||||||
|
|
||||||
|
文件按自然顺序排序，因此 ``test2.json`` 会排在 ``test10.json`` 前面。
|
||||||
|
如果某条路线点数较少，缺失的索引会被跳过，不影响其他路线候选。
|
||||||
|
|
||||||
|
JSON 格式
|
||||||
|
---------
|
||||||
|
推荐格式为：
|
||||||
|
|
||||||
|
{
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_001",
|
||||||
|
"yaw_degrees": 4.39,
|
||||||
|
"odom": {
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {"x": 1.48, "y": 0.18, "z": 0.0},
|
||||||
|
"orientation": {"x": 0.0, "y": 0.0, "z": 0.03, "w": 0.99}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
|
||||||
|
同时也支持顶层直接是 list 的 JSON。坐标 ``x/y`` 必须与 ``goal_frame``
|
||||||
|
一致，例如 ``odom`` 或 ``odom_combined``。
|
||||||
|
|
||||||
|
候选点选择规则
|
||||||
|
--------------
|
||||||
|
1. 第一次优先选择第 0 条路线，也就是自然排序后的第一个 JSON 文件。
|
||||||
|
2. 到达某个 Q_i 后，进入 Q_{i+1} 时优先保持同一条路线。
|
||||||
|
3. 如果该候选点在全局 costmap 中不可用，则按路线顺序循环尝试下一个候选。
|
||||||
|
4. 如果某个 Q_i 中所有候选都不可用，则跳过该 Q，进入 Q_{i+1}。
|
||||||
|
5. 如果 Nav2 action 被拒绝、失败、取消或超时，则当前候选在本轮 Q 中
|
||||||
|
被标记为失败，下一轮 tick 会尝试下一个候选。
|
||||||
|
|
||||||
|
不可用判断
|
||||||
|
---------
|
||||||
|
发送目标前会查询 ``/global_costmap/costmap``：
|
||||||
|
|
||||||
|
* 点在 costmap 外：不可用。
|
||||||
|
* costmap 数据异常：不可用。
|
||||||
|
* cost >= ``occupied_threshold``：不可用。
|
||||||
|
* cost = -1：由 ``treat_unknown_as_occupied`` 决定。
|
||||||
|
|
||||||
|
当前默认适配无静态地图场景：``treat_unknown_as_occupied=False``，因此未知
|
||||||
|
区域不会直接导致候选点被拒绝；真正障碍仍由 cost 阈值过滤。
|
||||||
|
|
||||||
|
关键参数
|
||||||
|
--------
|
||||||
|
* ``json_dir``: JSON 路线目录。
|
||||||
|
* ``goal_frame``: 下发 NavigateToPose 目标使用的坐标系，默认 ``odom``。
|
||||||
|
* ``costmap_topic``: 用于判断候选点占用状态的 costmap，默认
|
||||||
|
``/global_costmap/costmap``。
|
||||||
|
* ``navigate_action``: Nav2 action 名称，默认 ``/navigate_to_pose``。
|
||||||
|
* ``occupied_threshold``: costmap 占用阈值，默认 50。
|
||||||
|
* ``treat_unknown_as_occupied``: 是否把 cost=-1 视为不可通行，默认 False。
|
||||||
|
* ``goal_timeout_sec``: 单个候选目标超时时间，默认 180 秒。
|
||||||
|
* ``selection_period_sec``: 主循环周期，默认 0.5 秒。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
import re
|
||||||
|
import sys
|
||||||
|
import traceback
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Dict, List, Optional, Sequence, Set
|
||||||
|
|
||||||
|
ROS_IMPORT_ERROR: Optional[BaseException] = None
|
||||||
|
|
||||||
|
try:
|
||||||
|
import rclpy
|
||||||
|
from geometry_msgs.msg import PoseStamped
|
||||||
|
from nav2_msgs.action import NavigateToPose
|
||||||
|
from nav_msgs.msg import OccupancyGrid
|
||||||
|
from rclpy.action import ActionClient
|
||||||
|
from rclpy.duration import Duration
|
||||||
|
from rclpy.node import Node
|
||||||
|
from rclpy.qos import QoSProfile
|
||||||
|
except BaseException as exc: # pragma: no cover - used for remote runtime diagnosis
|
||||||
|
ROS_IMPORT_ERROR = exc
|
||||||
|
rclpy = None # type: ignore[assignment]
|
||||||
|
PoseStamped = object # type: ignore[assignment]
|
||||||
|
NavigateToPose = object # type: ignore[assignment]
|
||||||
|
OccupancyGrid = object # type: ignore[assignment]
|
||||||
|
ActionClient = object # type: ignore[assignment]
|
||||||
|
Duration = object # type: ignore[assignment]
|
||||||
|
Node = object # type: ignore[assignment]
|
||||||
|
QoSProfile = object # type: ignore[assignment]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class CandidateWaypoint:
|
||||||
|
q_index: int
|
||||||
|
route_index: int
|
||||||
|
file_name: str
|
||||||
|
point_name: str
|
||||||
|
x: float
|
||||||
|
y: float
|
||||||
|
yaw: float
|
||||||
|
|
||||||
|
|
||||||
|
def yaw_from_quaternion(q: Dict[str, float]) -> float:
|
||||||
|
x = float(q.get("x", 0.0))
|
||||||
|
y = float(q.get("y", 0.0))
|
||||||
|
z = float(q.get("z", 0.0))
|
||||||
|
w = float(q.get("w", 1.0))
|
||||||
|
return math.atan2(
|
||||||
|
2.0 * (w * z + x * y),
|
||||||
|
1.0 - 2.0 * (y * y + z * z),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def pose_from_json_point(point: Dict, q_index: int, route_index: int, file_name: str) -> CandidateWaypoint:
|
||||||
|
odom = point.get("odom", {})
|
||||||
|
pose = odom.get("pose", {}).get("pose", {})
|
||||||
|
position = pose.get("position", {})
|
||||||
|
orientation = pose.get("orientation", {})
|
||||||
|
|
||||||
|
x = float(position["x"])
|
||||||
|
y = float(position["y"])
|
||||||
|
yaw_degrees = point.get("yaw_degrees", None)
|
||||||
|
if yaw_degrees is None:
|
||||||
|
yaw = yaw_from_quaternion(orientation)
|
||||||
|
else:
|
||||||
|
yaw = math.radians(float(yaw_degrees))
|
||||||
|
|
||||||
|
return CandidateWaypoint(
|
||||||
|
q_index=q_index,
|
||||||
|
route_index=route_index,
|
||||||
|
file_name=file_name,
|
||||||
|
point_name=str(point.get("name", f"{Path(file_name).stem}_{q_index}")),
|
||||||
|
x=x,
|
||||||
|
y=y,
|
||||||
|
yaw=yaw,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def natural_json_sort_key(path: Path) -> List[object]:
|
||||||
|
return [
|
||||||
|
int(part) if part.isdigit() else part
|
||||||
|
for part in re.split(r"(\d+)", path.name.lower())
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def load_points_from_json(path: Path) -> List[Dict]:
|
||||||
|
try:
|
||||||
|
data = json.loads(path.read_text(encoding="utf-8"))
|
||||||
|
except Exception as exc:
|
||||||
|
raise RuntimeError(f"failed to parse JSON file {path}: {exc}") from exc
|
||||||
|
|
||||||
|
if isinstance(data, list):
|
||||||
|
points = data
|
||||||
|
elif isinstance(data, dict):
|
||||||
|
points = data.get("points", [])
|
||||||
|
else:
|
||||||
|
raise RuntimeError(
|
||||||
|
f"{path} must contain either a top-level list or a dict field named 'points'"
|
||||||
|
)
|
||||||
|
|
||||||
|
if not isinstance(points, list):
|
||||||
|
raise RuntimeError(f"{path} does not contain a list field named 'points'")
|
||||||
|
return points
|
||||||
|
|
||||||
|
|
||||||
|
def load_q_groups(json_dir: Path) -> tuple[List[List[CandidateWaypoint]], List[str], List[int]]:
|
||||||
|
if not json_dir.exists():
|
||||||
|
raise RuntimeError(f"JSON directory does not exist: {json_dir}")
|
||||||
|
if not json_dir.is_dir():
|
||||||
|
raise RuntimeError(f"JSON path is not a directory: {json_dir}")
|
||||||
|
|
||||||
|
files = sorted(json_dir.glob("*.json"), key=natural_json_sort_key)
|
||||||
|
if not files:
|
||||||
|
raise RuntimeError(f"no JSON files found in {json_dir}")
|
||||||
|
|
||||||
|
routes: List[List[Dict]] = []
|
||||||
|
for path in files:
|
||||||
|
routes.append(load_points_from_json(path))
|
||||||
|
|
||||||
|
max_points = max(len(route) for route in routes)
|
||||||
|
groups: List[List[CandidateWaypoint]] = []
|
||||||
|
for q_index in range(max_points):
|
||||||
|
group: List[CandidateWaypoint] = []
|
||||||
|
for route_index, points in enumerate(routes):
|
||||||
|
if q_index >= len(points):
|
||||||
|
continue
|
||||||
|
group.append(
|
||||||
|
pose_from_json_point(
|
||||||
|
points[q_index],
|
||||||
|
q_index=q_index,
|
||||||
|
route_index=route_index,
|
||||||
|
file_name=files[route_index].name,
|
||||||
|
)
|
||||||
|
)
|
||||||
|
groups.append(group)
|
||||||
|
return groups, [path.name for path in files], [len(route) for route in routes]
|
||||||
|
|
||||||
|
|
||||||
|
class QWaypointSelector(Node):
|
||||||
|
def __init__(self) -> None:
|
||||||
|
super().__init__("q_waypoint_selector")
|
||||||
|
|
||||||
|
self._declare_parameter_if_needed("use_sim_time", False)
|
||||||
|
self._declare_parameter_if_needed("json_dir", "")
|
||||||
|
self._declare_parameter_if_needed("goal_frame", "odom")
|
||||||
|
self._declare_parameter_if_needed("costmap_topic", "/global_costmap/costmap")
|
||||||
|
self._declare_parameter_if_needed("navigate_action", "/navigate_to_pose")
|
||||||
|
self._declare_parameter_if_needed("occupied_threshold", 50)
|
||||||
|
self._declare_parameter_if_needed("treat_unknown_as_occupied", False)
|
||||||
|
self._declare_parameter_if_needed("goal_timeout_sec", 180.0)
|
||||||
|
self._declare_parameter_if_needed("selection_period_sec", 0.5)
|
||||||
|
|
||||||
|
json_dir = Path(str(self.get_parameter("json_dir").value)).expanduser()
|
||||||
|
if not json_dir.is_absolute():
|
||||||
|
json_dir = Path.cwd() / json_dir
|
||||||
|
self.goal_frame = str(self.get_parameter("goal_frame").value)
|
||||||
|
self.costmap_topic = str(self.get_parameter("costmap_topic").value)
|
||||||
|
self.navigate_action = str(self.get_parameter("navigate_action").value)
|
||||||
|
self.occupied_threshold = int(self.get_parameter("occupied_threshold").value)
|
||||||
|
self.treat_unknown_as_occupied = bool(
|
||||||
|
self.get_parameter("treat_unknown_as_occupied").value
|
||||||
|
)
|
||||||
|
self.goal_timeout_sec = max(
|
||||||
|
1.0, float(self.get_parameter("goal_timeout_sec").value)
|
||||||
|
)
|
||||||
|
self.selection_period_sec = max(
|
||||||
|
0.1, float(self.get_parameter("selection_period_sec").value)
|
||||||
|
)
|
||||||
|
|
||||||
|
self.q_groups, self.route_file_names, self.route_point_counts = load_q_groups(json_dir)
|
||||||
|
self.current_q_index = 0
|
||||||
|
self.preferred_route_index = 0
|
||||||
|
self.rejected_routes_for_current_q: Set[int] = set()
|
||||||
|
self.active_candidate: Optional[CandidateWaypoint] = None
|
||||||
|
self.active_goal_handle = None
|
||||||
|
self.active_goal_start_time = None
|
||||||
|
self.active_goal_token: Optional[int] = None
|
||||||
|
self.next_goal_token = 0
|
||||||
|
self.done = False
|
||||||
|
self.latest_costmap: Optional[OccupancyGrid] = None
|
||||||
|
self.last_costmap_wait_log_ns = 0
|
||||||
|
|
||||||
|
qos = QoSProfile(depth=1)
|
||||||
|
self.costmap_sub = self.create_subscription(
|
||||||
|
OccupancyGrid, self.costmap_topic, self._costmap_callback, qos
|
||||||
|
)
|
||||||
|
self.action_client = ActionClient(self, NavigateToPose, self.navigate_action)
|
||||||
|
self.timer = self.create_timer(self.selection_period_sec, self._tick)
|
||||||
|
|
||||||
|
self.get_logger().info(
|
||||||
|
"loaded Q waypoint groups "
|
||||||
|
f"json_dir={json_dir} groups={len(self.q_groups)} "
|
||||||
|
f"routes={len(self.route_file_names)} "
|
||||||
|
f"goal_frame={self.goal_frame} costmap_topic={self.costmap_topic}"
|
||||||
|
)
|
||||||
|
route_summary = ", ".join(
|
||||||
|
f"{name}:{count}"
|
||||||
|
for name, count in zip(self.route_file_names, self.route_point_counts)
|
||||||
|
)
|
||||||
|
self.get_logger().info(f"loaded JSON waypoint routes {route_summary}")
|
||||||
|
|
||||||
|
def _declare_parameter_if_needed(self, name: str, default_value) -> None:
|
||||||
|
if not self.has_parameter(name):
|
||||||
|
self.declare_parameter(name, default_value)
|
||||||
|
|
||||||
|
def _costmap_callback(self, msg: OccupancyGrid) -> None:
|
||||||
|
self.latest_costmap = msg
|
||||||
|
|
||||||
|
def _log_waiting_for_costmap(self) -> None:
|
||||||
|
now_ns = self.get_clock().now().nanoseconds
|
||||||
|
if now_ns - self.last_costmap_wait_log_ns < 2_000_000_000:
|
||||||
|
return
|
||||||
|
self.last_costmap_wait_log_ns = now_ns
|
||||||
|
|
||||||
|
publishers = self.get_publishers_info_by_topic(self.costmap_topic)
|
||||||
|
if not publishers:
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"waiting for costmap {self.costmap_topic}; publisher_count=0; "
|
||||||
|
"no goal will be sent yet"
|
||||||
|
)
|
||||||
|
return
|
||||||
|
|
||||||
|
details = []
|
||||||
|
for info in publishers[:3]:
|
||||||
|
details.append(
|
||||||
|
f"{info.node_namespace}/{info.node_name} "
|
||||||
|
f"type={info.topic_type} qos={info.qos_profile}"
|
||||||
|
)
|
||||||
|
more = "" if len(publishers) <= 3 else f" ... +{len(publishers) - 3} more"
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"waiting for costmap {self.costmap_topic}; "
|
||||||
|
f"publisher_count={len(publishers)} publishers={details}{more}; "
|
||||||
|
"no goal will be sent yet"
|
||||||
|
)
|
||||||
|
|
||||||
|
def _tick(self) -> None:
|
||||||
|
if self.done:
|
||||||
|
return
|
||||||
|
|
||||||
|
if self.current_q_index >= len(self.q_groups):
|
||||||
|
self.done = True
|
||||||
|
self.get_logger().info("all Q waypoint groups completed")
|
||||||
|
return
|
||||||
|
|
||||||
|
if not self.action_client.server_is_ready():
|
||||||
|
self.get_logger().info(
|
||||||
|
f"waiting for NavigateToPose action server {self.navigate_action}"
|
||||||
|
)
|
||||||
|
self.action_client.wait_for_server(timeout_sec=0.1)
|
||||||
|
return
|
||||||
|
|
||||||
|
if self.latest_costmap is None:
|
||||||
|
self._log_waiting_for_costmap()
|
||||||
|
return
|
||||||
|
|
||||||
|
if self.active_goal_handle is not None:
|
||||||
|
if self._active_goal_timed_out():
|
||||||
|
self.get_logger().warn(
|
||||||
|
"active goal timed out; cancelling and trying next candidate "
|
||||||
|
f"q={self.current_q_index} candidate={self._candidate_label(self.active_candidate)}"
|
||||||
|
)
|
||||||
|
cancel_future = self.active_goal_handle.cancel_goal_async()
|
||||||
|
cancel_future.add_done_callback(lambda _: None)
|
||||||
|
self._reject_active_candidate()
|
||||||
|
return
|
||||||
|
|
||||||
|
candidate = self._select_candidate()
|
||||||
|
if candidate is None:
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"all candidates in Q_{self.current_q_index} are occupied/unusable; skipping group"
|
||||||
|
)
|
||||||
|
self.current_q_index += 1
|
||||||
|
self.rejected_routes_for_current_q.clear()
|
||||||
|
return
|
||||||
|
|
||||||
|
self._send_goal(candidate)
|
||||||
|
|
||||||
|
def _select_candidate(self) -> Optional[CandidateWaypoint]:
|
||||||
|
group = self.q_groups[self.current_q_index]
|
||||||
|
if not group:
|
||||||
|
return None
|
||||||
|
|
||||||
|
route_to_candidate = {candidate.route_index: candidate for candidate in group}
|
||||||
|
ordered_route_indices = self._cyclic_route_order(
|
||||||
|
sorted(route_to_candidate.keys()), self.preferred_route_index
|
||||||
|
)
|
||||||
|
|
||||||
|
for route_index in ordered_route_indices:
|
||||||
|
if route_index in self.rejected_routes_for_current_q:
|
||||||
|
continue
|
||||||
|
candidate = route_to_candidate[route_index]
|
||||||
|
occupied, reason = self._candidate_occupied(candidate)
|
||||||
|
if occupied:
|
||||||
|
self.rejected_routes_for_current_q.add(route_index)
|
||||||
|
self.get_logger().info(
|
||||||
|
f"skip occupied candidate Q_{candidate.q_index} "
|
||||||
|
f"{self._candidate_label(candidate)} reason={reason}"
|
||||||
|
)
|
||||||
|
continue
|
||||||
|
return candidate
|
||||||
|
return None
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _cyclic_route_order(route_indices: Sequence[int], preferred: int) -> List[int]:
|
||||||
|
if not route_indices:
|
||||||
|
return []
|
||||||
|
if preferred not in route_indices:
|
||||||
|
preferred = route_indices[0]
|
||||||
|
start = route_indices.index(preferred)
|
||||||
|
return list(route_indices[start:]) + list(route_indices[:start])
|
||||||
|
|
||||||
|
def _candidate_occupied(self, candidate: CandidateWaypoint) -> tuple[bool, str]:
|
||||||
|
costmap = self.latest_costmap
|
||||||
|
if costmap is None:
|
||||||
|
return True, "no_costmap"
|
||||||
|
|
||||||
|
origin = costmap.info.origin
|
||||||
|
resolution = costmap.info.resolution
|
||||||
|
width = int(costmap.info.width)
|
||||||
|
height = int(costmap.info.height)
|
||||||
|
if resolution <= 0.0 or width <= 0 or height <= 0:
|
||||||
|
return True, "invalid_costmap"
|
||||||
|
|
||||||
|
origin_yaw = yaw_from_quaternion(
|
||||||
|
{
|
||||||
|
"x": origin.orientation.x,
|
||||||
|
"y": origin.orientation.y,
|
||||||
|
"z": origin.orientation.z,
|
||||||
|
"w": origin.orientation.w,
|
||||||
|
}
|
||||||
|
)
|
||||||
|
dx = candidate.x - origin.position.x
|
||||||
|
dy = candidate.y - origin.position.y
|
||||||
|
cos_yaw = math.cos(-origin_yaw)
|
||||||
|
sin_yaw = math.sin(-origin_yaw)
|
||||||
|
local_x = cos_yaw * dx - sin_yaw * dy
|
||||||
|
local_y = sin_yaw * dx + cos_yaw * dy
|
||||||
|
mx = int(math.floor(local_x / resolution))
|
||||||
|
my = int(math.floor(local_y / resolution))
|
||||||
|
|
||||||
|
if mx < 0 or my < 0 or mx >= width or my >= height:
|
||||||
|
return True, f"outside_costmap cell=({mx},{my})"
|
||||||
|
|
||||||
|
index = my * width + mx
|
||||||
|
if index < 0 or index >= len(costmap.data):
|
||||||
|
return True, "costmap_data_too_short"
|
||||||
|
|
||||||
|
cost = int(costmap.data[index])
|
||||||
|
if cost < 0:
|
||||||
|
return self.treat_unknown_as_occupied, f"unknown cost={cost}"
|
||||||
|
if cost >= self.occupied_threshold:
|
||||||
|
return True, f"occupied cost={cost}"
|
||||||
|
return False, f"free cost={cost}"
|
||||||
|
|
||||||
|
def _send_goal(self, candidate: CandidateWaypoint) -> None:
|
||||||
|
goal_msg = NavigateToPose.Goal()
|
||||||
|
goal_msg.pose = self._to_pose_stamped(candidate)
|
||||||
|
self.next_goal_token += 1
|
||||||
|
token = self.next_goal_token
|
||||||
|
self.active_candidate = candidate
|
||||||
|
self.active_goal_start_time = self.get_clock().now()
|
||||||
|
self.active_goal_token = token
|
||||||
|
|
||||||
|
self.get_logger().info(
|
||||||
|
f"send Q_{candidate.q_index} candidate {self._candidate_label(candidate)} "
|
||||||
|
f"pose=({candidate.x:.3f},{candidate.y:.3f},{candidate.yaw:.3f})"
|
||||||
|
)
|
||||||
|
send_future = self.action_client.send_goal_async(goal_msg)
|
||||||
|
send_future.add_done_callback(
|
||||||
|
lambda future, goal_token=token: self._goal_response_callback(future, goal_token)
|
||||||
|
)
|
||||||
|
|
||||||
|
def _to_pose_stamped(self, candidate: CandidateWaypoint) -> PoseStamped:
|
||||||
|
pose = PoseStamped()
|
||||||
|
pose.header.frame_id = self.goal_frame
|
||||||
|
pose.header.stamp = self.get_clock().now().to_msg()
|
||||||
|
pose.pose.position.x = candidate.x
|
||||||
|
pose.pose.position.y = candidate.y
|
||||||
|
pose.pose.position.z = 0.0
|
||||||
|
pose.pose.orientation.z = math.sin(candidate.yaw * 0.5)
|
||||||
|
pose.pose.orientation.w = math.cos(candidate.yaw * 0.5)
|
||||||
|
return pose
|
||||||
|
|
||||||
|
def _goal_response_callback(self, future, goal_token: int) -> None:
|
||||||
|
if goal_token != self.active_goal_token:
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
goal_handle = future.result()
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().error(
|
||||||
|
"NavigateToPose send_goal_async failed; try next candidate: "
|
||||||
|
f"{type(exc).__name__}: {exc}"
|
||||||
|
)
|
||||||
|
self._reject_active_candidate()
|
||||||
|
return
|
||||||
|
if not goal_handle.accepted:
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"goal rejected by Nav2 {self._candidate_label(self.active_candidate)}"
|
||||||
|
)
|
||||||
|
self._reject_active_candidate()
|
||||||
|
return
|
||||||
|
|
||||||
|
self.active_goal_handle = goal_handle
|
||||||
|
result_future = goal_handle.get_result_async()
|
||||||
|
result_future.add_done_callback(
|
||||||
|
lambda future, token=goal_token: self._goal_result_callback(future, token)
|
||||||
|
)
|
||||||
|
|
||||||
|
def _goal_result_callback(self, future, goal_token: int) -> None:
|
||||||
|
if goal_token != self.active_goal_token:
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
result = future.result()
|
||||||
|
except Exception as exc:
|
||||||
|
self.get_logger().error(
|
||||||
|
"NavigateToPose result failed; try next candidate: "
|
||||||
|
f"{type(exc).__name__}: {exc}"
|
||||||
|
)
|
||||||
|
self._reject_active_candidate()
|
||||||
|
return
|
||||||
|
candidate = self.active_candidate
|
||||||
|
status = int(result.status)
|
||||||
|
|
||||||
|
if status == 4:
|
||||||
|
if candidate is not None:
|
||||||
|
self.preferred_route_index = candidate.route_index
|
||||||
|
self.get_logger().info(
|
||||||
|
f"reached Q_{candidate.q_index} {self._candidate_label(candidate)}; "
|
||||||
|
f"advance to Q_{candidate.q_index + 1}"
|
||||||
|
)
|
||||||
|
self.current_q_index += 1
|
||||||
|
self.rejected_routes_for_current_q.clear()
|
||||||
|
self.active_candidate = None
|
||||||
|
self.active_goal_handle = None
|
||||||
|
self.active_goal_start_time = None
|
||||||
|
self.active_goal_token = None
|
||||||
|
return
|
||||||
|
|
||||||
|
self.get_logger().warn(
|
||||||
|
f"goal failed status={status}; try next candidate "
|
||||||
|
f"{self._candidate_label(candidate)}"
|
||||||
|
)
|
||||||
|
self._reject_active_candidate()
|
||||||
|
|
||||||
|
def _active_goal_timed_out(self) -> bool:
|
||||||
|
if self.active_goal_start_time is None:
|
||||||
|
return False
|
||||||
|
elapsed = self.get_clock().now() - self.active_goal_start_time
|
||||||
|
return elapsed > Duration(seconds=self.goal_timeout_sec)
|
||||||
|
|
||||||
|
def _reject_active_candidate(self) -> None:
|
||||||
|
if self.active_candidate is not None:
|
||||||
|
self.rejected_routes_for_current_q.add(self.active_candidate.route_index)
|
||||||
|
self.active_candidate = None
|
||||||
|
self.active_goal_handle = None
|
||||||
|
self.active_goal_start_time = None
|
||||||
|
self.active_goal_token = None
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _candidate_label(candidate: Optional[CandidateWaypoint]) -> str:
|
||||||
|
if candidate is None:
|
||||||
|
return "none"
|
||||||
|
return (
|
||||||
|
f"route={candidate.route_index} file={candidate.file_name} "
|
||||||
|
f"name={candidate.point_name}"
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> int:
|
||||||
|
if ROS_IMPORT_ERROR is not None:
|
||||||
|
print(
|
||||||
|
"FATAL q_waypoint_selector: failed to import ROS/Nav2 Python modules.",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
traceback.print_exception(
|
||||||
|
type(ROS_IMPORT_ERROR),
|
||||||
|
ROS_IMPORT_ERROR,
|
||||||
|
ROS_IMPORT_ERROR.__traceback__,
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
print(
|
||||||
|
"Hint: source /opt/ros/humble/setup.bash and install/source Nav2 "
|
||||||
|
"packages, especially nav2_msgs.",
|
||||||
|
file=sys.stderr,
|
||||||
|
)
|
||||||
|
return 1
|
||||||
|
|
||||||
|
rclpy.init()
|
||||||
|
node = None
|
||||||
|
try:
|
||||||
|
node = QWaypointSelector()
|
||||||
|
rclpy.spin(node)
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
except BaseException:
|
||||||
|
print("FATAL q_waypoint_selector: unhandled exception", file=sys.stderr)
|
||||||
|
traceback.print_exc(file=sys.stderr)
|
||||||
|
return 1
|
||||||
|
finally:
|
||||||
|
if node is not None:
|
||||||
|
node.destroy_node()
|
||||||
|
if rclpy.ok():
|
||||||
|
rclpy.shutdown()
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,270 @@
|
|||||||
|
# Q 路点选择器 使用手册
|
||||||
|
|
||||||
|
## 1. 概述
|
||||||
|
|
||||||
|
`q_waypoint_selector.py` 是一个独立的 ROS 2 Python 节点，用于比赛场景下的**自动有序路点导航**。它从 JSON 文件中加载多路线、多候选的路点坐标，结合全局 costmap 实时判断候选点是否可通行，并通过 Nav2 的 `NavigateToPose` action 下发导航目标，依次逐个完成所有路点。
|
||||||
|
|
||||||
|
### 核心能力
|
||||||
|
|
||||||
|
- **多路线冗余**：同一个路点序号可配置多个候选坐标（来自不同 JSON 文件），一个被障碍物占据时自动切换
|
||||||
|
- **costmap 感知**：下发目标前先检查该坐标在全局 costmap 上是否被占用，避免朝障碍物导航
|
||||||
|
- **路线偏好记忆**：成功后记住当前路线编号，后续 Q 优先选择同一条路线
|
||||||
|
- **超时自动切换**：单个目标超时后自动取消，尝试下一个候选
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 2. 路点 JSON 文件格式
|
||||||
|
|
||||||
|
### 目录约定
|
||||||
|
|
||||||
|
所有 JSON 文件放在同一目录下，文件名按字母序排列。节点加载时：
|
||||||
|
|
||||||
|
```
|
||||||
|
json_dir/
|
||||||
|
├── test1.json # 路线 0
|
||||||
|
├── test2.json # 路线 1
|
||||||
|
├── test3.json # 路线 2
|
||||||
|
└── ...
|
||||||
|
```
|
||||||
|
|
||||||
|
每个 JSON 文件代表一条**完整路线**，包含有序的路点列表。
|
||||||
|
|
||||||
|
### JSON 结构
|
||||||
|
|
||||||
|
```json
|
||||||
|
{
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_001",
|
||||||
|
"yaw_degrees": 4.39,
|
||||||
|
"odom": {
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": { "x": 1.48, "y": 0.18, "z": 0.0 },
|
||||||
|
"orientation": { "x": 0.0, "y": 0.0, "z": 0.03, "w": 0.99 }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_002",
|
||||||
|
"yaw_degrees": 15.10,
|
||||||
|
"odom": {
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": { "x": 3.24, "y": 0.23, "z": 0.0 },
|
||||||
|
"orientation": { "x": 0.0, "y": 0.0, "z": 0.13, "w": 0.99 }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
|
```
|
||||||
|
|
||||||
|
### 字段说明
|
||||||
|
|
||||||
|
| 字段 | 类型 | 必需 | 说明 |
|
||||||
|
|------|------|------|------|
|
||||||
|
| `points` | array | 是 | 有序路点数组，按索引 0, 1, 2... 依次导航 |
|
||||||
|
| `points[i].name` | string | 否 | 路点名称（日志显示用，缺省为 `{文件名}_{索引}`） |
|
||||||
|
| `points[i].yaw_degrees` | number | 否 | 目标朝向角（度），优先级高于 orientation 四元数 |
|
||||||
|
| `points[i].odom.pose.pose.position` | object | 是 | 路点坐标 `{x, y, z}` |
|
||||||
|
| `points[i].odom.pose.pose.orientation` | object | 否 | 目标朝向四元数 `{x, y, z, w}`，仅在 `yaw_degrees` 缺失时使用 |
|
||||||
|
|
||||||
|
> **注意**：`position.x` 和 `position.y` 必须与 `goal_frame` 参数指定的坐标系一致（如 `odom` 或 `odom_combined`）。
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 3. Q 路点组概念
|
||||||
|
|
||||||
|
### 路点编组规则
|
||||||
|
|
||||||
|
假设有 2 个 JSON 文件，经过编组后：
|
||||||
|
|
||||||
|
```
|
||||||
|
test1.json: [p0, p1, p2] (路线 0)
|
||||||
|
test2.json: [p0, p1, p2] (路线 1)
|
||||||
|
↓ 转置编组
|
||||||
|
Q_0 = [test1[0], test2[0]] ← 两个候选路点，指向同一目标位置的不同路线
|
||||||
|
Q_1 = [test1[1], test2[1]]
|
||||||
|
Q_2 = [test1[2], test2[2]]
|
||||||
|
```
|
||||||
|
|
||||||
|
**路径数不同时的处理**：编组时以最长路线为准，较短路线在对应索引上缺失的点被跳过，不会影响其他路线的候选。
|
||||||
|
|
||||||
|
### 候选选择策略
|
||||||
|
|
||||||
|
节点在每个 Q 组内按以下优先级选择候选：
|
||||||
|
|
||||||
|
1. **偏好路线优先**（`preferred_route_index`）：上次导航成功的路线编号
|
||||||
|
2. **循环顺序**：若偏好路线不可用，按路线编号循环尝试
|
||||||
|
3. **costmap 过滤**：被 costmap 判定为占用的候选直接跳过
|
||||||
|
4. **已拒绝列表**：本次 Q 内已失败/被拒绝的候选不再重试
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 4. 运行流程
|
||||||
|
|
||||||
|
```
|
||||||
|
启动节点
|
||||||
|
│
|
||||||
|
├─ 加载 json_dir 下所有 .json → 编组为 Q_0, Q_1, ...
|
||||||
|
├─ 等待 NavigateToPose action server 就绪
|
||||||
|
├─ 等待 costmap 话题有数据
|
||||||
|
│
|
||||||
|
└─ 定时器循环 (selection_period_sec)
|
||||||
|
│
|
||||||
|
├─ 所有 Q 完成? → 退出
|
||||||
|
│
|
||||||
|
├─ 有活跃 goal ?
|
||||||
|
│ ├─ 进行中 → 等待
|
||||||
|
│ └─ 超时 → 取消 goal，拒绝当前候选，下一轮切换到下一个候选
|
||||||
|
│
|
||||||
|
├─ 从 Q_current 选择候选
|
||||||
|
│ ├─ 有可用候选 → 通过 NavigateToPose 下发
|
||||||
|
│ └─ 全部被占用 → 跳过当前 Q，前进到 Q_{current+1}
|
||||||
|
│
|
||||||
|
└─ goal 结果回调
|
||||||
|
├─ status=4 (SUCCEEDED) → 记录路线偏好，前进到下一 Q
|
||||||
|
└─ 其他 (失败/拒绝) → 拒绝当前候选，下次 tick 换候选
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 5. 参数说明
|
||||||
|
|
||||||
|
### 启动参数
|
||||||
|
|
||||||
|
| 参数 | 类型 | 默认值 | 说明 |
|
||||||
|
|------|------|--------|------|
|
||||||
|
| `json_dir` | string | `obstacle_nav2/config/json` | JSON 路点文件目录 |
|
||||||
|
| `goal_frame` | string | `odom` | 下发导航目标使用的坐标帧，实车应设为 `odom_combined` |
|
||||||
|
| `costmap_topic` | string | `/global_costmap/costmap` | 全局 costmap 话题，用于避占检测 |
|
||||||
|
| `navigate_action` | string | `/navigate_to_pose` | Nav2 的 NavigateToPose action 名称 |
|
||||||
|
| `occupied_threshold` | int | `50` | costmap 占用阈值 (0-100)，栅格值 >= 此值视为被占据 |
|
||||||
|
| `treat_unknown_as_occupied` | bool | `true` | 是否将未知区域 (costmap 值 = -1) 视为不可通行 |
|
||||||
|
| `goal_timeout_sec` | float | `180.0` | 单个导航目标超时时间（秒），取值范围 [1.0, ∞) |
|
||||||
|
| `selection_period_sec` | float | `0.5` | 选择器主循环周期（秒），取值范围 [0.1, ∞) |
|
||||||
|
| `use_sim_time` | bool | `false` | 是否使用仿真时间 |
|
||||||
|
|
||||||
|
### 参数调优建议
|
||||||
|
|
||||||
|
| 场景 | 建议设置 |
|
||||||
|
|------|---------|
|
||||||
|
| 路点间距大（>10m） | `goal_timeout_sec` 适当增大（如 300s） |
|
||||||
|
| 障碍物密集区域 | `occupied_threshold` 降低（如 30），更保守地避让 |
|
||||||
|
| 动态障碍物多 | `selection_period_sec` 设短（如 0.2s），更快响应 costmap 变化 |
|
||||||
|
| costmap 覆盖不全 | `treat_unknown_as_occupied` 视情况关闭，允许向未知区域导航 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 6. 启动方式
|
||||||
|
|
||||||
|
### 方式 A：独立启动（我使用的是这个，测试成功
|
||||||
|
|
||||||
|
```bash
|
||||||
|
python3 q_waypoint_selector.py \
|
||||||
|
--ros-args \
|
||||||
|
-p json_dir:=/home/user/waypoints \
|
||||||
|
-p goal_frame:=odom_combined \
|
||||||
|
-p costmap_topic:=/global_costmap/costmap \
|
||||||
|
-p goal_timeout_sec:=120.0
|
||||||
|
```
|
||||||
|
|
||||||
|
### 方式 B：通过 launch 文件启动
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \
|
||||||
|
use_sim_time:=false \
|
||||||
|
global_frame:=odom_combined \
|
||||||
|
json_dir:=/home/user/waypoints \
|
||||||
|
goal_frame:=odom_combined \
|
||||||
|
goal_timeout_sec:=120.0 \
|
||||||
|
occupied_threshold:=50
|
||||||
|
```
|
||||||
|
|
||||||
|
这个 launch 文件会**同时启动**：底盘驱动、激光雷达、obstacle_scanner、Nav2 导航栈 和 Q 路点选择器。
|
||||||
|
|
||||||
|
### 方式 C：在已有导航栈上单独启动
|
||||||
|
|
||||||
|
```bash
|
||||||
|
ros2 run obstacle_nav2 q_waypoint_selector.py \
|
||||||
|
--ros-args \
|
||||||
|
-p json_dir:=/home/user/waypoints \
|
||||||
|
-p goal_frame:=odom_combined
|
||||||
|
```
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 7. 运行日志解读
|
||||||
|
|
||||||
|
### 正常日志
|
||||||
|
|
||||||
|
```
|
||||||
|
[INFO] loaded Q waypoint groups json_dir=/... groups=5 routes=3 goal_frame=odom
|
||||||
|
```
|
||||||
|
启动成功，加载了 5 个 Q 组，每条 Q 最多 3 个候选路线。
|
||||||
|
|
||||||
|
```
|
||||||
|
[INFO] send Q_0 candidate route=0 file=test1.json name=goal_001 pose=(1.480,0.180,0.076)
|
||||||
|
```
|
||||||
|
下发了 Q_0 的第一个候选路点。
|
||||||
|
|
||||||
|
```
|
||||||
|
[INFO] reached Q_0 route=0 file=test1.json name=goal_001; advance to Q_1
|
||||||
|
```
|
||||||
|
到达目标，记录路线偏好并进入下一路点。
|
||||||
|
|
||||||
|
### 异常日志及处理
|
||||||
|
|
||||||
|
| 日志 | 含义 | 处理方式 |
|
||||||
|
|------|------|---------|
|
||||||
|
| `waiting for NavigateToPose action server` | Nav2 未启动或未就绪 | 等待 nav2_bringup 完成启动 |
|
||||||
|
| `waiting for costmap; publisher_count=0` | costmap 话题无发布者 | 检查 global_costmap 节点是否启动 |
|
||||||
|
| `skip occupied candidate ... reason=occupied cost=100` | 候选路点栅格被障碍物占据 | 自动尝试下一个候选路线 |
|
||||||
|
| `skip occupied candidate ... reason=outside_costmap` | 候选路点超出当前 costmap 范围 | 增大 costmap 尺寸或调整路点坐标 |
|
||||||
|
| `active goal timed out; cancelling` | 导航目标超时 | 自动取消并切换候选，可能需要增大 `goal_timeout_sec` |
|
||||||
|
| `all candidates in Q_N are occupied; skipping` | 该 Q 所有候选均不可用 | 自动跳过到下一个 Q |
|
||||||
|
| `all Q waypoint groups completed` | 所有路点已完成 | 正常退出信号 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 8. Nav2 goal 状态码参考
|
||||||
|
|
||||||
|
节点根据 `NavigateToPose` action 的结果状态码决定下一步行为：
|
||||||
|
|
||||||
|
| status | 含义 | 节点行为 |
|
||||||
|
|--------|------|---------|
|
||||||
|
| 0 | UNKNOWN | 拒绝当前候选，换下一个 |
|
||||||
|
| 1 | ACCEPTED | (中间状态，不触发回调) |
|
||||||
|
| 2 | EXECUTING | (中间状态，不触发回调) |
|
||||||
|
| 3 | CANCELING | (中间状态，不触发回调) |
|
||||||
|
| 4 | **SUCCEEDED** | 记录路线偏好，前进到下一 Q |
|
||||||
|
| 5 | CANCELED | 拒绝当前候选，换下一个 |
|
||||||
|
| 6 | ABORTED | 拒绝当前候选，换下一个 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 9. 常见问题
|
||||||
|
|
||||||
|
**Q: 节点启动后一直等待 costmap，不发送目标？**
|
||||||
|
|
||||||
|
检查 `costmap_topic` 参数是否与实际话题一致（默认 `/global_costmap/costmap`）：
|
||||||
|
```bash
|
||||||
|
ros2 topic list | grep costmap
|
||||||
|
```
|
||||||
|
|
||||||
|
**Q: 所有候选路点都被判定为占用？**
|
||||||
|
|
||||||
|
1. 确认路点坐标与 `goal_frame` 坐标系一致
|
||||||
|
2. 检查 `occupied_threshold` 是否过低（costmap 中膨胀区域可能为 50-100）
|
||||||
|
3. 尝试设置 `treat_unknown_as_occupied:=false` 如果 costmap 覆盖不全
|
||||||
|
|
||||||
|
**Q: 机器人到达路点后没有立即进入下一个？**
|
||||||
|
|
||||||
|
节点的定时器周期为 `selection_period_sec`（默认 0.5s），最坏情况下需要等待一个周期。如果 goal 结果回调已触发，下一个 tick 就会下发新目标。
|
||||||
|
|
||||||
|
**Q: 如何录制路点 JSON 文件？**
|
||||||
|
|
||||||
|
路点 JSON 可以通过监听 `/odom_combined` (实车) 或 `/odom` (仿真) 话题并手动记录坐标生成。JSON 格式参见第 2 节。
|
||||||
27
src/navigation/obstacle_nav2/config/trajectory_guard.yaml
Normal file
27
src/navigation/obstacle_nav2/config/trajectory_guard.yaml
Normal file
@@ -0,0 +1,27 @@
|
|||||||
|
trajectory_guard_node:
|
||||||
|
ros__parameters:
|
||||||
|
input_path_topic: /trajectory_guard/input_path
|
||||||
|
patched_path_topic: /trajectory_guard/patched_path
|
||||||
|
costmap_topic: /local_costmap/costmap
|
||||||
|
odom_topic: /odom_combined
|
||||||
|
planner_action: /compute_path_to_pose
|
||||||
|
follow_action: /follow_path
|
||||||
|
planner_id: GridBased
|
||||||
|
controller_id: FollowPath
|
||||||
|
goal_checker_id: ""
|
||||||
|
check_period_sec: 0.5
|
||||||
|
lookahead_distance: 2.0
|
||||||
|
rejoin_min_distance: 1.5
|
||||||
|
rejoin_max_distance: 5.0
|
||||||
|
occupied_threshold: 50
|
||||||
|
treat_unknown_as_occupied: true
|
||||||
|
footprint_half_length: 0.14
|
||||||
|
footprint_half_width: 0.085
|
||||||
|
footprint_padding: 0.04
|
||||||
|
footprint_sample_step: 0.05
|
||||||
|
repair_cooldown_sec: 0.5
|
||||||
|
blocked_retry_wait_sec: 0.8
|
||||||
|
wait_for_planner_costmap_update: true
|
||||||
|
min_repair_progress_indices: 5
|
||||||
|
publish_zero_on_blocked: true
|
||||||
|
cmd_vel_topic: /cmd_vel
|
||||||
@@ -0,0 +1,31 @@
|
|||||||
|
#ifndef OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_
|
||||||
|
#define OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
#include <set>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "rclcpp/parameter.hpp"
|
||||||
|
|
||||||
|
namespace obstacle_nav2
|
||||||
|
{
|
||||||
|
|
||||||
|
struct NodeParameterSet
|
||||||
|
{
|
||||||
|
std::string node_name;
|
||||||
|
std::vector<rclcpp::Parameter> parameters;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct Nav2Profile
|
||||||
|
{
|
||||||
|
std::vector<NodeParameterSet> nodes;
|
||||||
|
};
|
||||||
|
|
||||||
|
Nav2Profile loadNav2Profile(const std::string & path);
|
||||||
|
std::vector<rclcpp::Parameter> filterDeclaredParameters(
|
||||||
|
const std::vector<rclcpp::Parameter> & parameters,
|
||||||
|
const std::set<std::string> & declared_names);
|
||||||
|
|
||||||
|
} // namespace obstacle_nav2
|
||||||
|
|
||||||
|
#endif // OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_
|
||||||
@@ -5,6 +5,7 @@
|
|||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
|
#include <optional>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <thread>
|
#include <thread>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
@@ -67,6 +68,10 @@ public:
|
|||||||
nav2_costmap_2d::Costmap2D & grid,
|
nav2_costmap_2d::Costmap2D & grid,
|
||||||
const std::vector<CircleObstacle> & obstacles,
|
const std::vector<CircleObstacle> & obstacles,
|
||||||
double resolution, double origin_x, double origin_y);
|
double resolution, double origin_x, double origin_y);
|
||||||
|
static std::optional<SnapshotBounds> applySnapshotIfNotEmpty(
|
||||||
|
nav2_costmap_2d::Costmap2D & grid,
|
||||||
|
const std::vector<CircleObstacle> & obstacles,
|
||||||
|
double resolution, double origin_x, double origin_y);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
void obstacleCallback(const obstacle_scanner::msg::ObstacleArray::SharedPtr msg);
|
void obstacleCallback(const obstacle_scanner::msg::ObstacleArray::SharedPtr msg);
|
||||||
@@ -95,6 +100,7 @@ private:
|
|||||||
std::string global_frame_;
|
std::string global_frame_;
|
||||||
double obstacle_timeout_{0.5};
|
double obstacle_timeout_{0.5};
|
||||||
double transform_tolerance_{0.2};
|
double transform_tolerance_{0.2};
|
||||||
|
bool retain_previous_on_empty_snapshot_{true};
|
||||||
double default_obstacle_radius_{0.05};
|
double default_obstacle_radius_{0.05};
|
||||||
double minimum_obstacle_radius_{0.02};
|
double minimum_obstacle_radius_{0.02};
|
||||||
double maximum_obstacle_radius_{0.50};
|
double maximum_obstacle_radius_{0.50};
|
||||||
|
|||||||
@@ -0,0 +1,89 @@
|
|||||||
|
#ifndef OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
|
||||||
|
#define OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
|
||||||
|
|
||||||
|
#include <cstddef>
|
||||||
|
#include <optional>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||||
|
#include "nav_msgs/msg/occupancy_grid.hpp"
|
||||||
|
#include "nav_msgs/msg/path.hpp"
|
||||||
|
|
||||||
|
namespace obstacle_nav2
|
||||||
|
{
|
||||||
|
|
||||||
|
struct CollisionCheckResult
|
||||||
|
{
|
||||||
|
bool blocked{false};
|
||||||
|
std::size_t path_index{0};
|
||||||
|
std::string reason{"clear"};
|
||||||
|
};
|
||||||
|
|
||||||
|
struct GuardSettings
|
||||||
|
{
|
||||||
|
double lookahead_distance{2.0};
|
||||||
|
double rejoin_min_distance{1.0};
|
||||||
|
double rejoin_max_distance{4.0};
|
||||||
|
int occupied_threshold{50};
|
||||||
|
bool treat_unknown_as_occupied{true};
|
||||||
|
double footprint_half_length{0.14};
|
||||||
|
double footprint_half_width{0.085};
|
||||||
|
double footprint_padding{0.04};
|
||||||
|
double footprint_sample_step{0.05};
|
||||||
|
};
|
||||||
|
|
||||||
|
double yawFromPose(const geometry_msgs::msg::PoseStamped & pose);
|
||||||
|
|
||||||
|
double distance2d(
|
||||||
|
const geometry_msgs::msg::PoseStamped & a,
|
||||||
|
const geometry_msgs::msg::PoseStamped & b);
|
||||||
|
|
||||||
|
std::size_t nearestPathIndex(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
const geometry_msgs::msg::PoseStamped & pose);
|
||||||
|
|
||||||
|
std::size_t advanceByDistance(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
double distance_m);
|
||||||
|
|
||||||
|
CollisionCheckResult checkPathAhead(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
const nav_msgs::msg::OccupancyGrid & costmap,
|
||||||
|
const GuardSettings & settings);
|
||||||
|
|
||||||
|
std::optional<std::size_t> findClearRejoinIndex(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
const nav_msgs::msg::OccupancyGrid & costmap,
|
||||||
|
const GuardSettings & settings);
|
||||||
|
|
||||||
|
std::optional<std::size_t> findClearRejoinIndexWithPlannerCostmap(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
const nav_msgs::msg::OccupancyGrid & local_costmap,
|
||||||
|
const nav_msgs::msg::OccupancyGrid * planner_costmap,
|
||||||
|
const GuardSettings & settings);
|
||||||
|
|
||||||
|
bool shouldRetryBlockedRepair(
|
||||||
|
const std::optional<std::size_t> & last_repair_nearest_index,
|
||||||
|
std::size_t nearest_index,
|
||||||
|
int min_repair_progress_indices,
|
||||||
|
double seconds_since_last_repair,
|
||||||
|
double blocked_retry_wait_sec,
|
||||||
|
bool costmap_updated_since_repair);
|
||||||
|
|
||||||
|
nav_msgs::msg::Path slicePath(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
std::size_t end_index_inclusive);
|
||||||
|
|
||||||
|
nav_msgs::msg::Path stitchPaths(
|
||||||
|
const nav_msgs::msg::Path & bypass,
|
||||||
|
const nav_msgs::msg::Path & original,
|
||||||
|
std::size_t rejoin_index);
|
||||||
|
|
||||||
|
} // namespace obstacle_nav2
|
||||||
|
|
||||||
|
#endif // OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
|
||||||
Binary file not shown.
@@ -0,0 +1,48 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
import os
|
||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
pkg_dir = get_package_share_directory('obstacle_nav2')
|
||||||
|
profile_10_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
|
||||||
|
profile_11_path = os.path.join(pkg_dir, 'config', 'nav2_profile_11.yaml')
|
||||||
|
|
||||||
|
profile_10_yaml = LaunchConfiguration('profile_10_yaml')
|
||||||
|
profile_11_yaml = LaunchConfiguration('profile_11_yaml')
|
||||||
|
profile_trigger_topic = LaunchConfiguration('profile_trigger_topic')
|
||||||
|
|
||||||
|
nav2_profile_tuner = Node(
|
||||||
|
package='obstacle_nav2',
|
||||||
|
executable='nav2_profile_tuner',
|
||||||
|
name='nav2_profile_tuner',
|
||||||
|
output='screen',
|
||||||
|
parameters=[{
|
||||||
|
'profile_10_yaml': profile_10_yaml,
|
||||||
|
'profile_11_yaml': profile_11_yaml,
|
||||||
|
'trigger_topic': profile_trigger_topic,
|
||||||
|
}],
|
||||||
|
)
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'profile_10_yaml',
|
||||||
|
default_value=profile_10_path,
|
||||||
|
description='YAML profile applied by default and when sign4return=10'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'profile_11_yaml',
|
||||||
|
default_value=profile_11_path,
|
||||||
|
description='YAML profile applied when sign4return=11'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'profile_trigger_topic',
|
||||||
|
default_value='/sign4return',
|
||||||
|
description='Int32 topic used to switch Nav2 runtime profiles'),
|
||||||
|
|
||||||
|
nav2_profile_tuner,
|
||||||
|
])
|
||||||
@@ -22,13 +22,15 @@ from ament_index_python.packages import get_package_share_directory
|
|||||||
from launch import LaunchDescription
|
from launch import LaunchDescription
|
||||||
from launch.actions import (
|
from launch.actions import (
|
||||||
DeclareLaunchArgument,
|
DeclareLaunchArgument,
|
||||||
|
ExecuteProcess,
|
||||||
GroupAction,
|
GroupAction,
|
||||||
IncludeLaunchDescription,
|
IncludeLaunchDescription,
|
||||||
|
SetEnvironmentVariable,
|
||||||
)
|
)
|
||||||
from launch.conditions import IfCondition
|
from launch.conditions import IfCondition
|
||||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
from launch.substitutions import LaunchConfiguration, PythonExpression
|
from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression
|
||||||
from launch_ros.actions import SetRemap
|
from launch_ros.actions import Node, SetRemap
|
||||||
from nav2_common.launch import RewrittenYaml
|
from nav2_common.launch import RewrittenYaml
|
||||||
|
|
||||||
|
|
||||||
@@ -47,16 +49,20 @@ def generate_launch_description():
|
|||||||
start_base = LaunchConfiguration('start_base', default='true')
|
start_base = LaunchConfiguration('start_base', default='true')
|
||||||
start_lidar = LaunchConfiguration('start_lidar', default='true')
|
start_lidar = LaunchConfiguration('start_lidar', default='true')
|
||||||
start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
|
start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
|
||||||
|
start_map_publisher = LaunchConfiguration('start_map_publisher', default='true')
|
||||||
|
map_yaml_path = LaunchConfiguration('map_yaml_path')
|
||||||
|
|
||||||
nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_params.yaml')
|
nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
|
||||||
nav_to_pose_bt_path = os.path.join(
|
nav_to_pose_bt_path = os.path.join(
|
||||||
pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
|
pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
|
||||||
|
nav_through_poses_bt_path = os.path.join(
|
||||||
|
pkg_dir, 'behavior_tree', 'nav_through_poses_ackermann.xml')
|
||||||
configured_params = RewrittenYaml(
|
configured_params = RewrittenYaml(
|
||||||
source_file=nav2_param_path,
|
source_file=nav2_param_path,
|
||||||
root_key='',
|
root_key='',
|
||||||
param_rewrites={
|
param_rewrites={
|
||||||
'global_frame': global_frame,
|
|
||||||
'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
|
'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
|
||||||
|
'default_nav_through_poses_bt_xml': nav_through_poses_bt_path,
|
||||||
},
|
},
|
||||||
convert_types=True,
|
convert_types=True,
|
||||||
)
|
)
|
||||||
@@ -93,6 +99,37 @@ def generate_launch_description():
|
|||||||
condition=IfCondition(start_obstacle_scanner),
|
condition=IfCondition(start_obstacle_scanner),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# ========================== Static Map Publisher ==========================
|
||||||
|
static_map_publisher_script = os.path.join(pkg_dir, 'scripts', 'static_map_publisher.py')
|
||||||
|
static_map_publisher = ExecuteProcess(
|
||||||
|
cmd=[
|
||||||
|
FindExecutable(name='python3'),
|
||||||
|
static_map_publisher_script,
|
||||||
|
'--ros-args',
|
||||||
|
'-p', ['yaml_filename:=', map_yaml_path],
|
||||||
|
'-p', 'publish_rate:=1.0',
|
||||||
|
'-p', 'map_frame:=map',
|
||||||
|
],
|
||||||
|
output='screen',
|
||||||
|
emulate_tty=True,
|
||||||
|
condition=IfCondition(start_map_publisher),
|
||||||
|
)
|
||||||
|
|
||||||
|
# ========================== Initial Pose -> TF (map->odom) ================
|
||||||
|
initial_pose_to_tf_script = os.path.join(pkg_dir, 'scripts', 'initial_pose_to_tf.py')
|
||||||
|
initial_pose_to_tf = ExecuteProcess(
|
||||||
|
cmd=[
|
||||||
|
FindExecutable(name='python3'),
|
||||||
|
initial_pose_to_tf_script,
|
||||||
|
'--ros-args',
|
||||||
|
'-p', 'odom_frame:=odom',
|
||||||
|
'-p', 'base_frame:=base_footprint',
|
||||||
|
'-p', 'map_frame:=map',
|
||||||
|
],
|
||||||
|
output='screen',
|
||||||
|
emulate_tty=True,
|
||||||
|
)
|
||||||
|
|
||||||
# ========================== Nav2 Navigation ===============================
|
# ========================== Nav2 Navigation ===============================
|
||||||
navigation_launch = IncludeLaunchDescription(
|
navigation_launch = IncludeLaunchDescription(
|
||||||
PythonLaunchDescriptionSource(
|
PythonLaunchDescriptionSource(
|
||||||
@@ -103,6 +140,7 @@ def generate_launch_description():
|
|||||||
'autostart': 'true',
|
'autostart': 'true',
|
||||||
}.items(),
|
}.items(),
|
||||||
)
|
)
|
||||||
|
|
||||||
# ========================== Assembly ======================================
|
# ========================== Assembly ======================================
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
@@ -137,9 +175,20 @@ def generate_launch_description():
|
|||||||
'start_obstacle_scanner',
|
'start_obstacle_scanner',
|
||||||
default_value='true',
|
default_value='true',
|
||||||
description='Also start the obstacle_scanner node'),
|
description='Also start the obstacle_scanner node'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'start_map_publisher',
|
||||||
|
default_value='true',
|
||||||
|
description='Start the static map publisher node'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'map_yaml_path',
|
||||||
|
default_value='/home/sunrise/yiliao_ws/src/map/nav2_costmap_map.yaml',
|
||||||
|
description='Path to the map YAML file'),
|
||||||
|
|
||||||
|
|
||||||
safe_base_bringup,
|
safe_base_bringup,
|
||||||
lslidar_launch,
|
lslidar_launch,
|
||||||
obstacle_scanner_launch,
|
obstacle_scanner_launch,
|
||||||
|
static_map_publisher,
|
||||||
|
initial_pose_to_tf,
|
||||||
navigation_launch,
|
navigation_launch,
|
||||||
])
|
])
|
||||||
|
|||||||
@@ -0,0 +1,191 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
# ============================================================================
|
||||||
|
# obstacle_nav2.launch.py
|
||||||
|
#
|
||||||
|
# Odometry-only Nav2 navigation using obstacle_scanner detections.
|
||||||
|
#
|
||||||
|
# Architecture:
|
||||||
|
# origincar_base (bringup) — chassis serial driver + EKF + TF
|
||||||
|
# lslidar_driver — lidar → /scan
|
||||||
|
# obstacle_scanner (optional) — /scan → /obstacles
|
||||||
|
# Nav2 navigation_launch — rolling costmaps + Hybrid A* + MPPI
|
||||||
|
#
|
||||||
|
# No map_server, AMCL, or slam_toolbox in the default path.
|
||||||
|
#
|
||||||
|
# Usage:
|
||||||
|
# ros2 launch obstacle_nav2 obstacle_nav2.launch.py
|
||||||
|
# ros2 launch obstacle_nav2 obstacle_nav2.launch.py enable_motion:=true
|
||||||
|
# ============================================================================
|
||||||
|
|
||||||
|
import os
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import (
|
||||||
|
DeclareLaunchArgument,
|
||||||
|
ExecuteProcess,
|
||||||
|
GroupAction,
|
||||||
|
IncludeLaunchDescription,
|
||||||
|
SetEnvironmentVariable,
|
||||||
|
)
|
||||||
|
from launch.conditions import IfCondition
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from launch.substitutions import FindExecutable, LaunchConfiguration, PythonExpression
|
||||||
|
from launch_ros.actions import Node, SetRemap
|
||||||
|
from nav2_common.launch import RewrittenYaml
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
"""Odometry-only obstacle navigation."""
|
||||||
|
|
||||||
|
pkg_dir = get_package_share_directory('obstacle_nav2')
|
||||||
|
nav2_bringup_dir = get_package_share_directory('nav2_bringup')
|
||||||
|
|
||||||
|
# ========================== Launch Arguments ==============================
|
||||||
|
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
|
||||||
|
global_frame = LaunchConfiguration('global_frame', default='odom')
|
||||||
|
use_static_map = LaunchConfiguration('use_static_map', default='false')
|
||||||
|
map_yaml = LaunchConfiguration('map_yaml', default='')
|
||||||
|
enable_motion = LaunchConfiguration('enable_motion', default='true')
|
||||||
|
start_base = LaunchConfiguration('start_base', default='true')
|
||||||
|
start_lidar = LaunchConfiguration('start_lidar', default='true')
|
||||||
|
start_obstacle_scanner = LaunchConfiguration('start_obstacle_scanner', default='true')
|
||||||
|
start_map_publisher = LaunchConfiguration('start_map_publisher', default='true')
|
||||||
|
map_yaml_path = LaunchConfiguration('map_yaml_path')
|
||||||
|
|
||||||
|
nav2_param_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
|
||||||
|
nav_to_pose_bt_path = os.path.join(
|
||||||
|
pkg_dir, 'behavior_tree', 'nav_to_pose_ackermann.xml')
|
||||||
|
configured_params = RewrittenYaml(
|
||||||
|
source_file=nav2_param_path,
|
||||||
|
root_key='',
|
||||||
|
param_rewrites={
|
||||||
|
'default_nav_to_pose_bt_xml': nav_to_pose_bt_path,
|
||||||
|
},
|
||||||
|
convert_types=True,
|
||||||
|
)
|
||||||
|
base_cmd_vel_topic = PythonExpression([
|
||||||
|
"'/cmd_vel' if '", enable_motion,
|
||||||
|
"' == 'true' else '/cmd_vel_hardware_disabled'",
|
||||||
|
])
|
||||||
|
|
||||||
|
# ========================== Base Bringup ==================================
|
||||||
|
origincar_bringup = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
[get_package_share_directory('origincar_base'),
|
||||||
|
'/launch', '/origincar_bringup.launch.py']),
|
||||||
|
condition=IfCondition(start_base),
|
||||||
|
)
|
||||||
|
safe_base_bringup = GroupAction([
|
||||||
|
SetRemap(src='cmd_vel', dst=base_cmd_vel_topic),
|
||||||
|
origincar_bringup,
|
||||||
|
])
|
||||||
|
|
||||||
|
# ========================== Lidar Driver ==================================
|
||||||
|
lslidar_launch = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
[get_package_share_directory('lslidar_driver'),
|
||||||
|
'/launch', '/lsn10_launch.py']),
|
||||||
|
condition=IfCondition(start_lidar),
|
||||||
|
)
|
||||||
|
|
||||||
|
# ========================== Obstacle Scanner ==============================
|
||||||
|
obstacle_scanner_launch = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
[get_package_share_directory('obstacle_scanner'),
|
||||||
|
'/launch', '/obstacle_scanner.launch.py']),
|
||||||
|
condition=IfCondition(start_obstacle_scanner),
|
||||||
|
)
|
||||||
|
|
||||||
|
# ========================== Static Map Publisher ==========================
|
||||||
|
static_map_publisher_script = os.path.join(pkg_dir, 'scripts', 'static_map_publisher.py')
|
||||||
|
static_map_publisher = ExecuteProcess(
|
||||||
|
cmd=[
|
||||||
|
FindExecutable(name='python3'),
|
||||||
|
static_map_publisher_script,
|
||||||
|
'--ros-args',
|
||||||
|
'-p', ['yaml_filename:=', map_yaml_path],
|
||||||
|
'-p', 'publish_rate:=1.0',
|
||||||
|
'-p', 'map_frame:=map',
|
||||||
|
],
|
||||||
|
output='screen',
|
||||||
|
emulate_tty=True,
|
||||||
|
condition=IfCondition(start_map_publisher),
|
||||||
|
)
|
||||||
|
|
||||||
|
# ========================== Initial Pose -> TF (map->odom) ================
|
||||||
|
initial_pose_to_tf_script = os.path.join(pkg_dir, 'scripts', 'initial_pose_to_tf.py')
|
||||||
|
initial_pose_to_tf = ExecuteProcess(
|
||||||
|
cmd=[
|
||||||
|
FindExecutable(name='python3'),
|
||||||
|
initial_pose_to_tf_script,
|
||||||
|
'--ros-args',
|
||||||
|
'-p', 'odom_frame:=odom',
|
||||||
|
'-p', 'base_frame:=base_footprint',
|
||||||
|
'-p', 'map_frame:=map',
|
||||||
|
],
|
||||||
|
output='screen',
|
||||||
|
emulate_tty=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
# ========================== Nav2 Navigation ===============================
|
||||||
|
navigation_launch = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(
|
||||||
|
[nav2_bringup_dir, '/launch', '/navigation_launch.py']),
|
||||||
|
launch_arguments={
|
||||||
|
'use_sim_time': use_sim_time,
|
||||||
|
'params_file': configured_params,
|
||||||
|
'autostart': 'true',
|
||||||
|
}.items(),
|
||||||
|
)
|
||||||
|
|
||||||
|
# ========================== Assembly ======================================
|
||||||
|
return LaunchDescription([
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'use_sim_time',
|
||||||
|
default_value='false',
|
||||||
|
description='Use simulation (Gazebo) clock'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'global_frame',
|
||||||
|
default_value='odom',
|
||||||
|
description='Global frame for both costmaps'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'use_static_map',
|
||||||
|
default_value='false',
|
||||||
|
description='Reserved: enable static map + AMCL (not yet implemented)'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'map_yaml',
|
||||||
|
default_value='',
|
||||||
|
description='Reserved: path to map YAML file'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'enable_motion',
|
||||||
|
default_value='true',
|
||||||
|
description='Route Nav2 cmd_vel to the real base topic'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'start_base',
|
||||||
|
default_value='true',
|
||||||
|
description='Start the base driver, EKF, and robot TF publishers'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'start_lidar',
|
||||||
|
default_value='true',
|
||||||
|
description='Start the lidar driver'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'start_obstacle_scanner',
|
||||||
|
default_value='true',
|
||||||
|
description='Also start the obstacle_scanner node'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'start_map_publisher',
|
||||||
|
default_value='true',
|
||||||
|
description='Start the static map publisher node'),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
'map_yaml_path',
|
||||||
|
default_value='/home/sunrise/yiliao_ws/src/map/nav2_costmap_map.yaml',
|
||||||
|
description='Path to the map YAML file'),
|
||||||
|
|
||||||
|
|
||||||
|
safe_base_bringup,
|
||||||
|
lslidar_launch,
|
||||||
|
obstacle_scanner_launch,
|
||||||
|
static_map_publisher,
|
||||||
|
initial_pose_to_tf,
|
||||||
|
navigation_launch,
|
||||||
|
])
|
||||||
@@ -0,0 +1,265 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""
|
||||||
|
obstacle_nav2_q_waypoints.launch.py
|
||||||
|
====================================
|
||||||
|
复合启动文件：同时启动 obstacle_nav2 导航系统 + Q 路点候选选择器节点。
|
||||||
|
|
||||||
|
架构概览
|
||||||
|
--------
|
||||||
|
本 launch 文件包含两个核心组件：
|
||||||
|
1. obstacle_nav2 : 完整的障碍物导航栈（底盘驱动、激光雷达、Nav2 导航、障碍物扫描）
|
||||||
|
2. q_waypoint_selector : 独立的 Python 节点，周期性从 costmap 中评估并选择
|
||||||
|
最优的 Q 路点候选目标，通过 NavigateToPose action 下发导航目标。
|
||||||
|
|
||||||
|
Q 路点选择器的设计意图
|
||||||
|
----------------------
|
||||||
|
在比赛中，我们需要依次访问一系列路点（Q₁, Q₂, ..., Qₙ），但每个路点可能有多个
|
||||||
|
候选位置。选择器从 JSON 文件中加载有序路点及其候选坐标，结合全局 costmap 的
|
||||||
|
占用栅格信息，过滤掉被障碍物占据的候选点，并向 Nav2 下发当前最优的导航目标。
|
||||||
|
|
||||||
|
启动方式
|
||||||
|
--------
|
||||||
|
ros2 launch obstacle_nav2 obstacle_nav2_q_waypoints.launch.py \
|
||||||
|
use_sim_time:=false \
|
||||||
|
global_frame:=odom \
|
||||||
|
json_dir:=/path/to/json \
|
||||||
|
goal_frame:=odom \
|
||||||
|
costmap_topic:=/global_costmap/costmap
|
||||||
|
"""
|
||||||
|
|
||||||
|
import os
|
||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch.substitutions import FindExecutable
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
# =========================================================================
|
||||||
|
# 路径配置 —— 定位 obstacle_nav2 包内的关键文件
|
||||||
|
# =========================================================================
|
||||||
|
|
||||||
|
# obstacle_nav2 包的共享目录（install 后的 share/obstacle_nav2）
|
||||||
|
pkg_dir = get_package_share_directory("obstacle_nav2")
|
||||||
|
|
||||||
|
# 基础导航 launch 文件路径，将被 IncludeLaunchDescription 引入
|
||||||
|
base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py")
|
||||||
|
|
||||||
|
# Q 路点选择器的 Python 脚本路径，将被 ExecuteProcess 作为独立进程运行
|
||||||
|
selector_script = os.path.join(pkg_dir, "config", "q_waypoint_selector.py")
|
||||||
|
|
||||||
|
# 默认的 JSON 路点文件目录
|
||||||
|
default_json_dir = os.path.join(pkg_dir, "config", "json")
|
||||||
|
|
||||||
|
# =========================================================================
|
||||||
|
# LaunchConfiguration —— 运行时参数占位符
|
||||||
|
# 这些变量在 launch 时被替换为实际的参数值（命令行传入或默认值）。
|
||||||
|
# 使用 LaunchConfiguration 而非直接读取值，是为了支持 launch 系统的
|
||||||
|
# 延迟求值（lazy evaluation）机制。
|
||||||
|
# =========================================================================
|
||||||
|
|
||||||
|
# ---- obstacle_nav2 基础导航栈参数 ----
|
||||||
|
|
||||||
|
# 是否使用仿真时间（实车=false，Gazebo 仿真=true）
|
||||||
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||||
|
|
||||||
|
# 全局坐标系名称（仿真用 "odom"，实车用 "odom_combined"）
|
||||||
|
global_frame = LaunchConfiguration("global_frame")
|
||||||
|
|
||||||
|
# 是否加载静态地图（已知地图导航=true，纯 SLAM 探索=false）
|
||||||
|
use_static_map = LaunchConfiguration("use_static_map")
|
||||||
|
|
||||||
|
# 静态地图 YAML 文件路径（为空时不加载 map_server）
|
||||||
|
map_yaml = LaunchConfiguration("map_yaml")
|
||||||
|
|
||||||
|
# 是否启用底盘运动控制（调试时可关闭以只运行感知）
|
||||||
|
enable_motion = LaunchConfiguration("enable_motion")
|
||||||
|
|
||||||
|
# 是否启动底盘驱动节点（实车=true，纯仿真可视化=false）
|
||||||
|
start_base = LaunchConfiguration("start_base")
|
||||||
|
|
||||||
|
# 是否启动激光雷达驱动节点
|
||||||
|
start_lidar = LaunchConfiguration("start_lidar")
|
||||||
|
|
||||||
|
# 是否启动障碍物扫描节点（obstacle_scanner）
|
||||||
|
start_obstacle_scanner = LaunchConfiguration("start_obstacle_scanner")
|
||||||
|
|
||||||
|
# ---- Q 路点选择器专属参数 ----
|
||||||
|
|
||||||
|
# JSON 路点文件所在目录，包含如 Q1.json, Q2.json 等有序路点定义
|
||||||
|
json_dir = LaunchConfiguration("json_dir")
|
||||||
|
|
||||||
|
# 下发 NavigateToPose 导航目标时使用的坐标系
|
||||||
|
goal_frame = LaunchConfiguration("goal_frame")
|
||||||
|
|
||||||
|
# 全局 costmap 话题，用于判断候选路点是否被障碍物占据
|
||||||
|
costmap_topic = LaunchConfiguration("costmap_topic")
|
||||||
|
|
||||||
|
# Nav2 的 NavigateToPose action 名称
|
||||||
|
navigate_action = LaunchConfiguration("navigate_action")
|
||||||
|
|
||||||
|
# 占用阈值：costmap 栅格值 >= 此值的单元格视为被占用（范围 0-100）
|
||||||
|
occupied_threshold = LaunchConfiguration("occupied_threshold")
|
||||||
|
|
||||||
|
# 是否将未知区域（costmap 值 == -1）视为占用
|
||||||
|
treat_unknown_as_occupied = LaunchConfiguration("treat_unknown_as_occupied")
|
||||||
|
|
||||||
|
# 单个候选目标的超时时间（秒），超时后取消当前目标并尝试下一个候选
|
||||||
|
goal_timeout_sec = LaunchConfiguration("goal_timeout_sec")
|
||||||
|
|
||||||
|
# 选择器的控制周期（秒），即多久重新评估一次最优候选路点
|
||||||
|
selection_period_sec = LaunchConfiguration("selection_period_sec")
|
||||||
|
|
||||||
|
# =========================================================================
|
||||||
|
# 组件 1 —— obstacle_nav2 基础导航栈
|
||||||
|
# 通过 IncludeLaunchDescription 引入 obstacle_nav2.launch.py，
|
||||||
|
# 将其作为一个子 launch 嵌入当前 launch 描述中。
|
||||||
|
# 传入的参数会透传给子 launch 文件，实现参数的统一管理。
|
||||||
|
# =========================================================================
|
||||||
|
obstacle_nav2 = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(base_launch),
|
||||||
|
launch_arguments={
|
||||||
|
"use_sim_time": use_sim_time,
|
||||||
|
"global_frame": global_frame,
|
||||||
|
"use_static_map": use_static_map,
|
||||||
|
"map_yaml": map_yaml,
|
||||||
|
"enable_motion": enable_motion,
|
||||||
|
"start_base": start_base,
|
||||||
|
"start_lidar": start_lidar,
|
||||||
|
"start_obstacle_scanner": start_obstacle_scanner,
|
||||||
|
}.items(),
|
||||||
|
)
|
||||||
|
|
||||||
|
# =========================================================================
|
||||||
|
# 组件 2 —— Q 路点候选选择器
|
||||||
|
# 作为一个独立的 Python 进程运行（ExecuteProcess），而非 ROS 2 Node action。
|
||||||
|
# 选择器内部会创建一个 rclpy 节点，订阅 costmap、发布导航目标。
|
||||||
|
# 参数通过 ROS 2 标准的 --ros-args -p 方式传递。
|
||||||
|
# =========================================================================
|
||||||
|
q_waypoint_selector = ExecuteProcess(
|
||||||
|
cmd=[
|
||||||
|
# 使用 FindExecutable 查找 python3 可执行文件，确保跨平台兼容
|
||||||
|
FindExecutable(name="python3"),
|
||||||
|
selector_script,
|
||||||
|
# ROS 2 命令行参数传递标准格式
|
||||||
|
"--ros-args",
|
||||||
|
"-p", ["json_dir:=", json_dir],
|
||||||
|
"-p", ["goal_frame:=", goal_frame],
|
||||||
|
"-p", ["costmap_topic:=", costmap_topic],
|
||||||
|
"-p", ["navigate_action:=", navigate_action],
|
||||||
|
"-p", ["occupied_threshold:=", occupied_threshold],
|
||||||
|
"-p", ["treat_unknown_as_occupied:=", treat_unknown_as_occupied],
|
||||||
|
"-p", ["goal_timeout_sec:=", goal_timeout_sec],
|
||||||
|
"-p", ["selection_period_sec:=", selection_period_sec],
|
||||||
|
"-p", ["use_sim_time:=", use_sim_time],
|
||||||
|
],
|
||||||
|
# 将进程的标准输出打印到终端，便于调试
|
||||||
|
output="screen",
|
||||||
|
emulate_tty=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
# =========================================================================
|
||||||
|
# LaunchDescription —— 组装并返回完整的 launch 描述
|
||||||
|
# 包含三部分：
|
||||||
|
# 1. DeclareLaunchArgument —— 声明所有可配置参数及其默认值
|
||||||
|
# 2. obstacle_nav2 —— 基础导航栈
|
||||||
|
# 3. q_waypoint_selector —— Q 路点选择器
|
||||||
|
# =========================================================================
|
||||||
|
return LaunchDescription(
|
||||||
|
[
|
||||||
|
# =================================================================
|
||||||
|
# obstacle_nav2 基础导航栈参数声明
|
||||||
|
# =================================================================
|
||||||
|
|
||||||
|
# 是否使用仿真时间（/clock 话题），实车场景应设为 false
|
||||||
|
DeclareLaunchArgument("use_sim_time", default_value="false"),
|
||||||
|
|
||||||
|
# 全局坐标系：仿真用 "odom"，实车 EKF 融合后用 "odom_combined"
|
||||||
|
DeclareLaunchArgument("global_frame", default_value="odom"),
|
||||||
|
|
||||||
|
# 是否启用静态地图层（map_server），在线 SLAM 建图时设为 false
|
||||||
|
DeclareLaunchArgument("use_static_map", default_value="false"),
|
||||||
|
|
||||||
|
# 静态地图 yaml 文件路径，为空字符串时不加载地图
|
||||||
|
DeclareLaunchArgument("map_yaml", default_value=""),
|
||||||
|
|
||||||
|
# 是否启用底盘运动控制指令下发，调试时可关闭
|
||||||
|
DeclareLaunchArgument("enable_motion", default_value="true"),
|
||||||
|
|
||||||
|
# 是否启动底盘串口驱动节点（实车需要，仿真不需要）
|
||||||
|
DeclareLaunchArgument("start_base", default_value="true"),
|
||||||
|
|
||||||
|
# 是否启动激光雷达驱动节点
|
||||||
|
DeclareLaunchArgument("start_lidar", default_value="true"),
|
||||||
|
|
||||||
|
# 是否启动障碍物扫描与避障节点
|
||||||
|
DeclareLaunchArgument("start_obstacle_scanner", default_value="true"),
|
||||||
|
|
||||||
|
# =================================================================
|
||||||
|
# Q 路点选择器参数声明
|
||||||
|
# =================================================================
|
||||||
|
|
||||||
|
# JSON 路点文件目录，每个 JSON 文件定义一个路点的候选坐标列表
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"json_dir",
|
||||||
|
default_value=default_json_dir,
|
||||||
|
description="Directory containing ordered waypoint JSON files.",
|
||||||
|
),
|
||||||
|
|
||||||
|
# 导航目标使用的坐标帧，应与全局 costmap 的坐标系一致
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"goal_frame",
|
||||||
|
default_value="odom",
|
||||||
|
description="Frame used for NavigateToPose goals.",
|
||||||
|
),
|
||||||
|
|
||||||
|
# 全局 costmap 话题，选择器订阅此话题来评估候选点的可通行性
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"costmap_topic",
|
||||||
|
default_value="/global_costmap/costmap",
|
||||||
|
description="OccupancyGrid topic used to reject occupied candidates.",
|
||||||
|
),
|
||||||
|
|
||||||
|
# NavigateToPose action 的服务名，与 Nav2 的 bt_navigator 对应
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"navigate_action",
|
||||||
|
default_value="/navigate_to_pose",
|
||||||
|
description="Nav2 NavigateToPose action name.",
|
||||||
|
),
|
||||||
|
|
||||||
|
# costmap 占用判定阈值（0-100），>= 此值的栅格视为障碍物
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"occupied_threshold",
|
||||||
|
default_value="50",
|
||||||
|
description="Costmap cells >= this value are considered occupied.",
|
||||||
|
),
|
||||||
|
|
||||||
|
# 是否将未知区域（costmap 值 == -1）视为不可通行
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"treat_unknown_as_occupied",
|
||||||
|
default_value="true",
|
||||||
|
description="Treat costmap value -1 as occupied.",
|
||||||
|
),
|
||||||
|
|
||||||
|
# 单个导航目标的超时时间，超时后自动取消并切换候选
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"goal_timeout_sec",
|
||||||
|
default_value="60.0",
|
||||||
|
description="Cancel a candidate goal if it does not finish before this timeout.",
|
||||||
|
),
|
||||||
|
|
||||||
|
# 选择器主循环的控制周期（秒），值越小响应越快但 CPU 开销越大
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"selection_period_sec",
|
||||||
|
default_value="0.5",
|
||||||
|
description="Selector control period.",
|
||||||
|
),
|
||||||
|
|
||||||
|
# ---- 组装已配置的 action ----
|
||||||
|
obstacle_nav2,
|
||||||
|
q_waypoint_selector,
|
||||||
|
]
|
||||||
|
)
|
||||||
@@ -0,0 +1,58 @@
|
|||||||
|
#!/usr/bin/env python3
|
||||||
|
"""Launch obstacle_nav2 with the C++ trajectory guard node."""
|
||||||
|
|
||||||
|
import os
|
||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
pkg_dir = get_package_share_directory("obstacle_nav2")
|
||||||
|
base_launch = os.path.join(pkg_dir, "launch", "obstacle_nav2.launch.py")
|
||||||
|
guard_params = LaunchConfiguration("guard_params")
|
||||||
|
|
||||||
|
obstacle_nav2 = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource(base_launch),
|
||||||
|
launch_arguments={
|
||||||
|
"use_sim_time": LaunchConfiguration("use_sim_time"),
|
||||||
|
"global_frame": LaunchConfiguration("global_frame"),
|
||||||
|
"use_static_map": LaunchConfiguration("use_static_map"),
|
||||||
|
"map_yaml": LaunchConfiguration("map_yaml"),
|
||||||
|
"enable_motion": LaunchConfiguration("enable_motion"),
|
||||||
|
"start_base": LaunchConfiguration("start_base"),
|
||||||
|
"start_lidar": LaunchConfiguration("start_lidar"),
|
||||||
|
"start_obstacle_scanner": LaunchConfiguration("start_obstacle_scanner"),
|
||||||
|
}.items(),
|
||||||
|
)
|
||||||
|
|
||||||
|
trajectory_guard = Node(
|
||||||
|
package="obstacle_nav2",
|
||||||
|
executable="trajectory_guard_node",
|
||||||
|
name="trajectory_guard_node",
|
||||||
|
output="screen",
|
||||||
|
parameters=[guard_params],
|
||||||
|
)
|
||||||
|
|
||||||
|
return LaunchDescription(
|
||||||
|
[
|
||||||
|
DeclareLaunchArgument("use_sim_time", default_value="false"),
|
||||||
|
DeclareLaunchArgument("global_frame", default_value="odom"),
|
||||||
|
DeclareLaunchArgument("use_static_map", default_value="false"),
|
||||||
|
DeclareLaunchArgument("map_yaml", default_value=""),
|
||||||
|
DeclareLaunchArgument("enable_motion", default_value="true"),
|
||||||
|
DeclareLaunchArgument("start_base", default_value="true"),
|
||||||
|
DeclareLaunchArgument("start_lidar", default_value="true"),
|
||||||
|
DeclareLaunchArgument("start_obstacle_scanner", default_value="true"),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"guard_params",
|
||||||
|
default_value=os.path.join(pkg_dir, "config", "trajectory_guard.yaml"),
|
||||||
|
),
|
||||||
|
obstacle_nav2,
|
||||||
|
trajectory_guard,
|
||||||
|
]
|
||||||
|
)
|
||||||
@@ -19,6 +19,10 @@
|
|||||||
<depend>geometry_msgs</depend>
|
<depend>geometry_msgs</depend>
|
||||||
<depend>nav_msgs</depend>
|
<depend>nav_msgs</depend>
|
||||||
<depend>std_msgs</depend>
|
<depend>std_msgs</depend>
|
||||||
|
<depend>rcl_interfaces</depend>
|
||||||
|
<depend>nav2_msgs</depend>
|
||||||
|
<depend>rclcpp_action</depend>
|
||||||
|
<depend>yaml-cpp</depend>
|
||||||
|
|
||||||
<exec_depend>launch</exec_depend>
|
<exec_depend>launch</exec_depend>
|
||||||
<exec_depend>launch_ros</exec_depend>
|
<exec_depend>launch_ros</exec_depend>
|
||||||
|
|||||||
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()
|
||||||
238
src/navigation/obstacle_nav2/src/nav2_profile_loader.cpp
Normal file
238
src/navigation/obstacle_nav2/src/nav2_profile_loader.cpp
Normal file
@@ -0,0 +1,238 @@
|
|||||||
|
#include "obstacle_nav2/nav2_profile_loader.hpp"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cctype>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <regex>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "yaml-cpp/yaml.h"
|
||||||
|
|
||||||
|
namespace obstacle_nav2
|
||||||
|
{
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
|
||||||
|
std::string lowerCopy(std::string value)
|
||||||
|
{
|
||||||
|
std::transform(
|
||||||
|
value.begin(), value.end(), value.begin(),
|
||||||
|
[](unsigned char c) {return static_cast<char>(std::tolower(c));});
|
||||||
|
return value;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isBoolScalar(const std::string & value)
|
||||||
|
{
|
||||||
|
const auto lower = lowerCopy(value);
|
||||||
|
return lower == "true" || lower == "false";
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isIntegerScalar(const std::string & value)
|
||||||
|
{
|
||||||
|
static const std::regex pattern(R"(^[-+]?[0-9]+$)");
|
||||||
|
return std::regex_match(value, pattern);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isDoubleScalar(const std::string & value)
|
||||||
|
{
|
||||||
|
static const std::regex pattern(
|
||||||
|
R"(^[-+]?(([0-9]+\.?[0-9]*)|(\.[0-9]+))([eE][-+]?[0-9]+)?$)");
|
||||||
|
return std::regex_match(value, pattern);
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::Parameter scalarToParameter(const std::string & name, const YAML::Node & node)
|
||||||
|
{
|
||||||
|
const auto value = node.Scalar();
|
||||||
|
if (isBoolScalar(value)) {
|
||||||
|
return rclcpp::Parameter(name, node.as<bool>());
|
||||||
|
}
|
||||||
|
if (isIntegerScalar(value)) {
|
||||||
|
return rclcpp::Parameter(name, static_cast<int64_t>(node.as<int64_t>()));
|
||||||
|
}
|
||||||
|
if (isDoubleScalar(value)) {
|
||||||
|
return rclcpp::Parameter(name, node.as<double>());
|
||||||
|
}
|
||||||
|
return rclcpp::Parameter(name, value);
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::Parameter sequenceToParameter(const std::string & name, const YAML::Node & node)
|
||||||
|
{
|
||||||
|
std::vector<std::string> values;
|
||||||
|
values.reserve(node.size());
|
||||||
|
for (const auto & item : node) {
|
||||||
|
if (!item.IsScalar()) {
|
||||||
|
throw std::runtime_error("Parameter array '" + name + "' contains a non-scalar value");
|
||||||
|
}
|
||||||
|
values.push_back(item.Scalar());
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto all_bool = std::all_of(values.begin(), values.end(), isBoolScalar);
|
||||||
|
if (all_bool) {
|
||||||
|
std::vector<bool> parsed;
|
||||||
|
parsed.reserve(values.size());
|
||||||
|
for (const auto & value : values) {
|
||||||
|
parsed.push_back(YAML::Load(value).as<bool>());
|
||||||
|
}
|
||||||
|
return rclcpp::Parameter(name, parsed);
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto all_integer = std::all_of(values.begin(), values.end(), isIntegerScalar);
|
||||||
|
if (all_integer) {
|
||||||
|
std::vector<int64_t> parsed;
|
||||||
|
parsed.reserve(values.size());
|
||||||
|
for (const auto & value : values) {
|
||||||
|
parsed.push_back(YAML::Load(value).as<int64_t>());
|
||||||
|
}
|
||||||
|
return rclcpp::Parameter(name, parsed);
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto all_double = std::all_of(values.begin(), values.end(), isDoubleScalar);
|
||||||
|
if (all_double) {
|
||||||
|
std::vector<double> parsed;
|
||||||
|
parsed.reserve(values.size());
|
||||||
|
for (const auto & value : values) {
|
||||||
|
parsed.push_back(YAML::Load(value).as<double>());
|
||||||
|
}
|
||||||
|
return rclcpp::Parameter(name, parsed);
|
||||||
|
}
|
||||||
|
|
||||||
|
return rclcpp::Parameter(name, values);
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::Parameter yamlToParameter(const std::string & name, const YAML::Node & node)
|
||||||
|
{
|
||||||
|
if (node.IsScalar()) {
|
||||||
|
return scalarToParameter(name, node);
|
||||||
|
}
|
||||||
|
if (node.IsSequence()) {
|
||||||
|
return sequenceToParameter(name, node);
|
||||||
|
}
|
||||||
|
throw std::runtime_error("Parameter '" + name + "' must be a scalar or sequence");
|
||||||
|
}
|
||||||
|
|
||||||
|
void appendFlattenedParameters(
|
||||||
|
const YAML::Node & parameters,
|
||||||
|
const std::string & prefix,
|
||||||
|
std::vector<rclcpp::Parameter> & output)
|
||||||
|
{
|
||||||
|
if (!parameters.IsMap()) {
|
||||||
|
throw std::runtime_error("ros__parameters must contain a parameter map");
|
||||||
|
}
|
||||||
|
|
||||||
|
for (const auto & parameter_entry : parameters) {
|
||||||
|
const auto key = parameter_entry.first.as<std::string>();
|
||||||
|
if (key.empty()) {
|
||||||
|
throw std::runtime_error("ros__parameters contains an empty parameter name");
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto parameter_name = prefix.empty() ? key : prefix + "." + key;
|
||||||
|
const auto value = parameter_entry.second;
|
||||||
|
if (value.IsMap()) {
|
||||||
|
appendFlattenedParameters(value, parameter_name, output);
|
||||||
|
} else {
|
||||||
|
output.push_back(yamlToParameter(parameter_name, value));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
NodeParameterSet loadNodeParameterSet(
|
||||||
|
const std::string & node_name,
|
||||||
|
const YAML::Node & parameters)
|
||||||
|
{
|
||||||
|
if (node_name.empty()) {
|
||||||
|
throw std::runtime_error("Nav2 profile contains an empty node name");
|
||||||
|
}
|
||||||
|
if (!parameters.IsMap()) {
|
||||||
|
throw std::runtime_error("Node '" + node_name + "' must contain a parameter map");
|
||||||
|
}
|
||||||
|
|
||||||
|
NodeParameterSet node_set;
|
||||||
|
node_set.node_name = node_name;
|
||||||
|
for (const auto & parameter_entry : parameters) {
|
||||||
|
const auto parameter_name = parameter_entry.first.as<std::string>();
|
||||||
|
if (parameter_name.empty()) {
|
||||||
|
throw std::runtime_error("Node '" + node_name + "' contains an empty parameter name");
|
||||||
|
}
|
||||||
|
node_set.parameters.push_back(yamlToParameter(parameter_name, parameter_entry.second));
|
||||||
|
}
|
||||||
|
return node_set;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool isNativeRuntimeProfileNode(const std::string & path)
|
||||||
|
{
|
||||||
|
return path == "controller_server" || path == "velocity_smoother";
|
||||||
|
}
|
||||||
|
|
||||||
|
void collectRosParameterNodes(
|
||||||
|
const YAML::Node & yaml_node,
|
||||||
|
const std::string & path,
|
||||||
|
Nav2Profile & profile)
|
||||||
|
{
|
||||||
|
if (!yaml_node.IsMap()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto ros_parameters = yaml_node["ros__parameters"];
|
||||||
|
if (ros_parameters) {
|
||||||
|
if (path.empty()) {
|
||||||
|
throw std::runtime_error("ros__parameters must be nested under a node name");
|
||||||
|
}
|
||||||
|
|
||||||
|
NodeParameterSet node_set;
|
||||||
|
node_set.node_name = path.front() == '/' ? path : "/" + path;
|
||||||
|
appendFlattenedParameters(ros_parameters, "", node_set.parameters);
|
||||||
|
if (isNativeRuntimeProfileNode(path)) {
|
||||||
|
profile.nodes.push_back(node_set);
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (const auto & child_entry : yaml_node) {
|
||||||
|
const auto key = child_entry.first.as<std::string>();
|
||||||
|
const auto child_path = path.empty() ? key : path + "/" + key;
|
||||||
|
collectRosParameterNodes(child_entry.second, child_path, profile);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
Nav2Profile loadNav2Profile(const std::string & path)
|
||||||
|
{
|
||||||
|
const auto root = YAML::LoadFile(path);
|
||||||
|
const auto nodes = root["nodes"];
|
||||||
|
|
||||||
|
Nav2Profile profile;
|
||||||
|
if (nodes && nodes.IsMap()) {
|
||||||
|
for (const auto & node_entry : nodes) {
|
||||||
|
const auto node_name = node_entry.first.as<std::string>();
|
||||||
|
profile.nodes.push_back(loadNodeParameterSet(node_name, node_entry.second));
|
||||||
|
}
|
||||||
|
return profile;
|
||||||
|
}
|
||||||
|
|
||||||
|
collectRosParameterNodes(root, "", profile);
|
||||||
|
if (profile.nodes.empty()) {
|
||||||
|
throw std::runtime_error(
|
||||||
|
"Nav2 profile '" + path + "' must contain a nodes map or ros__parameters blocks");
|
||||||
|
}
|
||||||
|
|
||||||
|
return profile;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<rclcpp::Parameter> filterDeclaredParameters(
|
||||||
|
const std::vector<rclcpp::Parameter> & parameters,
|
||||||
|
const std::set<std::string> & declared_names)
|
||||||
|
{
|
||||||
|
std::vector<rclcpp::Parameter> filtered;
|
||||||
|
filtered.reserve(parameters.size());
|
||||||
|
for (const auto & parameter : parameters) {
|
||||||
|
if (declared_names.count(parameter.get_name()) > 0) {
|
||||||
|
filtered.push_back(parameter);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return filtered;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace obstacle_nav2
|
||||||
210
src/navigation/obstacle_nav2/src/nav2_profile_tuner.cpp
Normal file
210
src/navigation/obstacle_nav2/src/nav2_profile_tuner.cpp
Normal file
@@ -0,0 +1,210 @@
|
|||||||
|
#include <chrono>
|
||||||
|
#include <map>
|
||||||
|
#include <memory>
|
||||||
|
#include <set>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include "obstacle_nav2/nav2_profile_loader.hpp"
|
||||||
|
#include "rcl_interfaces/srv/list_parameters.hpp"
|
||||||
|
#include "rcl_interfaces/srv/set_parameters.hpp"
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include "std_msgs/msg/int32.hpp"
|
||||||
|
|
||||||
|
namespace obstacle_nav2
|
||||||
|
{
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
|
||||||
|
std::string setParametersServiceName(const std::string & node_name)
|
||||||
|
{
|
||||||
|
if (node_name.empty()) {
|
||||||
|
return "/set_parameters";
|
||||||
|
}
|
||||||
|
if (node_name.front() == '/') {
|
||||||
|
return node_name + "/set_parameters";
|
||||||
|
}
|
||||||
|
return "/" + node_name + "/set_parameters";
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string listParametersServiceName(const std::string & node_name)
|
||||||
|
{
|
||||||
|
if (node_name.empty()) {
|
||||||
|
return "/list_parameters";
|
||||||
|
}
|
||||||
|
if (node_name.front() == '/') {
|
||||||
|
return node_name + "/list_parameters";
|
||||||
|
}
|
||||||
|
return "/" + node_name + "/list_parameters";
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
class Nav2ProfileTuner : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
Nav2ProfileTuner()
|
||||||
|
: Node("nav2_profile_tuner")
|
||||||
|
{
|
||||||
|
const auto profile_10_path = declare_parameter<std::string>("profile_10_yaml", "");
|
||||||
|
const auto profile_11_path = declare_parameter<std::string>("profile_11_yaml", "");
|
||||||
|
const auto trigger_topic = declare_parameter<std::string>("trigger_topic", "/sign4return");
|
||||||
|
|
||||||
|
if (profile_10_path.empty() || profile_11_path.empty()) {
|
||||||
|
throw std::runtime_error("profile_10_yaml and profile_11_yaml must both be set");
|
||||||
|
}
|
||||||
|
|
||||||
|
profiles_[10] = loadNav2Profile(profile_10_path);
|
||||||
|
profiles_[11] = loadNav2Profile(profile_11_path);
|
||||||
|
|
||||||
|
trigger_sub_ = create_subscription<std_msgs::msg::Int32>(
|
||||||
|
trigger_topic, 10,
|
||||||
|
[this](const std_msgs::msg::Int32::SharedPtr msg) {
|
||||||
|
handleTrigger(msg->data);
|
||||||
|
});
|
||||||
|
|
||||||
|
retry_timer_ = create_wall_timer(
|
||||||
|
std::chrono::milliseconds(500),
|
||||||
|
[this]() {
|
||||||
|
tryApplyPendingProfile();
|
||||||
|
});
|
||||||
|
|
||||||
|
requestProfile(10);
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
using SetParameters = rcl_interfaces::srv::SetParameters;
|
||||||
|
using ListParameters = rcl_interfaces::srv::ListParameters;
|
||||||
|
|
||||||
|
void handleTrigger(const int32_t command)
|
||||||
|
{
|
||||||
|
if (command == 10 || command == 11) {
|
||||||
|
requestProfile(command);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
RCLCPP_DEBUG(get_logger(), "Ignoring sign4return=%d", command);
|
||||||
|
}
|
||||||
|
|
||||||
|
void requestProfile(const int command)
|
||||||
|
{
|
||||||
|
if (active_profile_ == command && !has_pending_profile_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
pending_profile_ = command;
|
||||||
|
has_pending_profile_ = true;
|
||||||
|
RCLCPP_INFO(get_logger(), "Queued Nav2 profile %d", command);
|
||||||
|
tryApplyPendingProfile();
|
||||||
|
}
|
||||||
|
|
||||||
|
void tryApplyPendingProfile()
|
||||||
|
{
|
||||||
|
if (!has_pending_profile_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto profile_it = profiles_.find(pending_profile_);
|
||||||
|
if (profile_it == profiles_.end()) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "No Nav2 profile loaded for command %d", pending_profile_);
|
||||||
|
has_pending_profile_ = false;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (const auto & node_set : profile_it->second.nodes) {
|
||||||
|
const auto set_service_name = setParametersServiceName(node_set.node_name);
|
||||||
|
auto & set_client = set_clients_[set_service_name];
|
||||||
|
if (!set_client) {
|
||||||
|
set_client = create_client<SetParameters>(set_service_name);
|
||||||
|
}
|
||||||
|
if (!set_client->wait_for_service(std::chrono::milliseconds(0))) {
|
||||||
|
RCLCPP_WARN_THROTTLE(
|
||||||
|
get_logger(), *get_clock(), 3000,
|
||||||
|
"Waiting for parameter service %s", set_service_name.c_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto list_service_name = listParametersServiceName(node_set.node_name);
|
||||||
|
auto & list_client = list_clients_[list_service_name];
|
||||||
|
if (!list_client) {
|
||||||
|
list_client = create_client<ListParameters>(list_service_name);
|
||||||
|
}
|
||||||
|
if (!list_client->wait_for_service(std::chrono::milliseconds(0))) {
|
||||||
|
RCLCPP_WARN_THROTTLE(
|
||||||
|
get_logger(), *get_clock(), 3000,
|
||||||
|
"Waiting for parameter service %s", list_service_name.c_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto command = pending_profile_;
|
||||||
|
for (const auto & node_set : profile_it->second.nodes) {
|
||||||
|
const auto list_service_name = listParametersServiceName(node_set.node_name);
|
||||||
|
auto request = std::make_shared<ListParameters::Request>();
|
||||||
|
request->depth = ListParameters::Request::DEPTH_RECURSIVE;
|
||||||
|
|
||||||
|
list_clients_.at(list_service_name)->async_send_request(
|
||||||
|
request,
|
||||||
|
[this, node_set](rclcpp::Client<ListParameters>::SharedFuture future) {
|
||||||
|
const auto response = future.get();
|
||||||
|
const std::set<std::string> declared_names(
|
||||||
|
response->result.names.begin(), response->result.names.end());
|
||||||
|
const auto parameters = filterDeclaredParameters(node_set.parameters, declared_names);
|
||||||
|
|
||||||
|
if (parameters.empty()) {
|
||||||
|
RCLCPP_WARN(
|
||||||
|
get_logger(), "No declared parameters from profile for %s",
|
||||||
|
node_set.node_name.c_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto set_request = std::make_shared<SetParameters::Request>();
|
||||||
|
set_request->parameters.reserve(parameters.size());
|
||||||
|
for (const auto & parameter : parameters) {
|
||||||
|
set_request->parameters.push_back(parameter.to_parameter_msg());
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto set_service_name = setParametersServiceName(node_set.node_name);
|
||||||
|
set_clients_.at(set_service_name)->async_send_request(
|
||||||
|
set_request,
|
||||||
|
[this, node_name = node_set.node_name](
|
||||||
|
rclcpp::Client<SetParameters>::SharedFuture set_future) {
|
||||||
|
const auto set_response = set_future.get();
|
||||||
|
for (const auto & result : set_response->results) {
|
||||||
|
if (!result.successful) {
|
||||||
|
RCLCPP_WARN(
|
||||||
|
get_logger(), "Parameter set failed on %s: %s",
|
||||||
|
node_name.c_str(), result.reason.c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
});
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
active_profile_ = command;
|
||||||
|
has_pending_profile_ = false;
|
||||||
|
RCLCPP_INFO(get_logger(), "Applied Nav2 profile %d", active_profile_);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::map<int, Nav2Profile> profiles_;
|
||||||
|
std::map<std::string, rclcpp::Client<SetParameters>::SharedPtr> set_clients_;
|
||||||
|
std::map<std::string, rclcpp::Client<ListParameters>::SharedPtr> list_clients_;
|
||||||
|
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr trigger_sub_;
|
||||||
|
rclcpp::TimerBase::SharedPtr retry_timer_;
|
||||||
|
int active_profile_{0};
|
||||||
|
int pending_profile_{0};
|
||||||
|
bool has_pending_profile_{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace obstacle_nav2
|
||||||
|
|
||||||
|
int main(int argc, char ** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
try {
|
||||||
|
rclcpp::spin(std::make_shared<obstacle_nav2::Nav2ProfileTuner>());
|
||||||
|
} catch (const std::exception & e) {
|
||||||
|
RCLCPP_FATAL(rclcpp::get_logger("nav2_profile_tuner"), "%s", e.what());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -3,6 +3,7 @@
|
|||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
#include <optional>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
@@ -53,11 +54,12 @@ void ObstacleArrayLayer::onInitialize()
|
|||||||
node->declare_parameter(name_ + ".enabled", true);
|
node->declare_parameter(name_ + ".enabled", true);
|
||||||
node->declare_parameter(name_ + ".topic", std::string("/obstacles"));
|
node->declare_parameter(name_ + ".topic", std::string("/obstacles"));
|
||||||
node->declare_parameter(name_ + ".obstacle_timeout", 0.5);
|
node->declare_parameter(name_ + ".obstacle_timeout", 0.5);
|
||||||
node->declare_parameter(name_ + ".transform_tolerance", 0.2);
|
node->declare_parameter(name_ + ".transform_tolerance", 0.02);
|
||||||
node->declare_parameter(name_ + ".default_obstacle_radius", 0.05);
|
node->declare_parameter(name_ + ".default_obstacle_radius", 0.05);
|
||||||
node->declare_parameter(name_ + ".minimum_obstacle_radius", 0.02);
|
node->declare_parameter(name_ + ".minimum_obstacle_radius", 0.02);
|
||||||
node->declare_parameter(name_ + ".maximum_obstacle_radius", 0.50);
|
node->declare_parameter(name_ + ".maximum_obstacle_radius", 0.50);
|
||||||
node->declare_parameter(name_ + ".extra_inflation", 0.02);
|
node->declare_parameter(name_ + ".extra_inflation", 0.02);
|
||||||
|
node->declare_parameter(name_ + ".retain_previous_on_empty_snapshot", true);
|
||||||
|
|
||||||
node->get_parameter(name_ + ".enabled", enabled_);
|
node->get_parameter(name_ + ".enabled", enabled_);
|
||||||
node->get_parameter(name_ + ".topic", topic_);
|
node->get_parameter(name_ + ".topic", topic_);
|
||||||
@@ -67,6 +69,8 @@ void ObstacleArrayLayer::onInitialize()
|
|||||||
node->get_parameter(name_ + ".minimum_obstacle_radius", minimum_obstacle_radius_);
|
node->get_parameter(name_ + ".minimum_obstacle_radius", minimum_obstacle_radius_);
|
||||||
node->get_parameter(name_ + ".maximum_obstacle_radius", maximum_obstacle_radius_);
|
node->get_parameter(name_ + ".maximum_obstacle_radius", maximum_obstacle_radius_);
|
||||||
node->get_parameter(name_ + ".extra_inflation", extra_inflation_);
|
node->get_parameter(name_ + ".extra_inflation", extra_inflation_);
|
||||||
|
node->get_parameter(
|
||||||
|
name_ + ".retain_previous_on_empty_snapshot", retain_previous_on_empty_snapshot_);
|
||||||
|
|
||||||
global_frame_ = layered_costmap_->getGlobalFrameID();
|
global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||||
|
|
||||||
@@ -78,7 +82,7 @@ void ObstacleArrayLayer::onInitialize()
|
|||||||
rclcpp::SubscriptionOptions subscription_options;
|
rclcpp::SubscriptionOptions subscription_options;
|
||||||
subscription_options.callback_group = callback_group_;
|
subscription_options.callback_group = callback_group_;
|
||||||
obstacle_sub_ = node->create_subscription<obstacle_scanner::msg::ObstacleArray>(
|
obstacle_sub_ = node->create_subscription<obstacle_scanner::msg::ObstacleArray>(
|
||||||
topic_, rclcpp::QoS(10).reliable(),
|
topic_, rclcpp::QoS(rclcpp::KeepLast(1)).reliable(),
|
||||||
std::bind(&ObstacleArrayLayer::obstacleCallback, this, std::placeholders::_1),
|
std::bind(&ObstacleArrayLayer::obstacleCallback, this, std::placeholders::_1),
|
||||||
subscription_options);
|
subscription_options);
|
||||||
callback_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
|
callback_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
|
||||||
@@ -87,9 +91,7 @@ void ObstacleArrayLayer::onInitialize()
|
|||||||
callback_stop_.store(false, std::memory_order_release);
|
callback_stop_.store(false, std::memory_order_release);
|
||||||
callback_thread_ = std::thread(
|
callback_thread_ = std::thread(
|
||||||
[this]() {
|
[this]() {
|
||||||
while (!callback_stop_.load(std::memory_order_acquire)) {
|
callback_executor_->spin();
|
||||||
callback_executor_->spin_once(std::chrono::milliseconds(100));
|
|
||||||
}
|
|
||||||
});
|
});
|
||||||
|
|
||||||
current_ = true;
|
current_ = true;
|
||||||
@@ -290,6 +292,17 @@ ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::applySnapshot(
|
|||||||
return bounds;
|
return bounds;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::optional<ObstacleArrayLayer::SnapshotBounds> ObstacleArrayLayer::applySnapshotIfNotEmpty(
|
||||||
|
nav2_costmap_2d::Costmap2D & grid,
|
||||||
|
const std::vector<CircleObstacle> & obstacles,
|
||||||
|
double resolution, double origin_x, double origin_y)
|
||||||
|
{
|
||||||
|
if (obstacles.empty()) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
return applySnapshot(grid, obstacles, resolution, origin_x, origin_y);
|
||||||
|
}
|
||||||
|
|
||||||
ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::mergeBounds(
|
ObstacleArrayLayer::SnapshotBounds ObstacleArrayLayer::mergeBounds(
|
||||||
const SnapshotBounds & first, const SnapshotBounds & second)
|
const SnapshotBounds & first, const SnapshotBounds & second)
|
||||||
{
|
{
|
||||||
@@ -378,18 +391,20 @@ void ObstacleArrayLayer::obstacleCallback(
|
|||||||
valid_obstacles.push_back({obs.center_x, obs.center_y, effective_r});
|
valid_obstacles.push_back({obs.center_x, obs.center_y, effective_r});
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
if (valid_obstacles.empty() && retain_previous_on_empty_snapshot_) {
|
||||||
std::lock_guard<std::mutex> lock(data_mutex_);
|
return;
|
||||||
const SnapshotBounds previous_bounds = current_bounds_;
|
|
||||||
current_bounds_ = applySnapshot(
|
|
||||||
*this, valid_obstacles, getResolution(), origin_x, origin_y);
|
|
||||||
pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, previous_bounds);
|
|
||||||
if (previous_bounds.valid || current_bounds_.valid) {
|
|
||||||
++bounds_generation_;
|
|
||||||
}
|
|
||||||
has_received_obstacles_ = current_bounds_.valid;
|
|
||||||
last_obstacle_time_ = node->now();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::lock_guard<std::mutex> lock(data_mutex_);
|
||||||
|
const SnapshotBounds previous_bounds = current_bounds_;
|
||||||
|
current_bounds_ = applySnapshot(
|
||||||
|
*this, valid_obstacles, getResolution(), origin_x, origin_y);
|
||||||
|
pending_clear_bounds_ = mergeBounds(pending_clear_bounds_, previous_bounds);
|
||||||
|
if (previous_bounds.valid || current_bounds_.valid) {
|
||||||
|
++bounds_generation_;
|
||||||
|
}
|
||||||
|
has_received_obstacles_ = current_bounds_.valid;
|
||||||
|
last_obstacle_time_ = node->now();
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace obstacle_nav2
|
} // namespace obstacle_nav2
|
||||||
|
|||||||
290
src/navigation/obstacle_nav2/src/trajectory_guard.cpp
Normal file
290
src/navigation/obstacle_nav2/src/trajectory_guard.cpp
Normal file
@@ -0,0 +1,290 @@
|
|||||||
|
#include "obstacle_nav2/trajectory_guard.hpp"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <cmath>
|
||||||
|
#include <limits>
|
||||||
|
|
||||||
|
namespace obstacle_nav2
|
||||||
|
{
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
|
||||||
|
bool worldToMap(
|
||||||
|
const nav_msgs::msg::OccupancyGrid & grid,
|
||||||
|
double wx,
|
||||||
|
double wy,
|
||||||
|
int & mx,
|
||||||
|
int & my)
|
||||||
|
{
|
||||||
|
const double resolution = grid.info.resolution;
|
||||||
|
if (resolution <= 0.0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const double ox = grid.info.origin.position.x;
|
||||||
|
const double oy = grid.info.origin.position.y;
|
||||||
|
if (wx < ox || wy < oy) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
mx = static_cast<int>((wx - ox) / resolution);
|
||||||
|
my = static_cast<int>((wy - oy) / resolution);
|
||||||
|
return mx >= 0 && my >= 0 &&
|
||||||
|
mx < static_cast<int>(grid.info.width) &&
|
||||||
|
my < static_cast<int>(grid.info.height);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool occupiedAt(
|
||||||
|
const nav_msgs::msg::OccupancyGrid & grid,
|
||||||
|
double wx,
|
||||||
|
double wy,
|
||||||
|
const GuardSettings & settings)
|
||||||
|
{
|
||||||
|
int mx = 0;
|
||||||
|
int my = 0;
|
||||||
|
if (!worldToMap(grid, wx, wy, mx, my)) {
|
||||||
|
return settings.treat_unknown_as_occupied;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto value = grid.data[my * grid.info.width + mx];
|
||||||
|
if (value < 0) {
|
||||||
|
return settings.treat_unknown_as_occupied;
|
||||||
|
}
|
||||||
|
return value >= settings.occupied_threshold;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool footprintOccupied(
|
||||||
|
const nav_msgs::msg::OccupancyGrid & costmap,
|
||||||
|
double cx,
|
||||||
|
double cy,
|
||||||
|
double yaw,
|
||||||
|
const GuardSettings & settings)
|
||||||
|
{
|
||||||
|
const double half_l = settings.footprint_half_length + settings.footprint_padding;
|
||||||
|
const double half_w = settings.footprint_half_width + settings.footprint_padding;
|
||||||
|
const double step = std::max(0.02, settings.footprint_sample_step);
|
||||||
|
const double c = std::cos(yaw);
|
||||||
|
const double s = std::sin(yaw);
|
||||||
|
|
||||||
|
auto sample = [&](double x, double y) {
|
||||||
|
const double wx = cx + c * x - s * y;
|
||||||
|
const double wy = cy + s * x + c * y;
|
||||||
|
return occupiedAt(costmap, wx, wy, settings);
|
||||||
|
};
|
||||||
|
|
||||||
|
if (sample(0.0, 0.0)) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
for (double x = -half_l; x <= half_l + 1.0e-9; x += step) {
|
||||||
|
if (sample(x, -half_w) || sample(x, 0.0) || sample(x, half_w)) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (double y = -half_w; y <= half_w + 1.0e-9; y += step) {
|
||||||
|
if (sample(-half_l, y) || sample(0.0, y) || sample(half_l, y)) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
double yawFromPose(const geometry_msgs::msg::PoseStamped & pose)
|
||||||
|
{
|
||||||
|
const auto & q = pose.pose.orientation;
|
||||||
|
return std::atan2(
|
||||||
|
2.0 * (q.w * q.z + q.x * q.y),
|
||||||
|
1.0 - 2.0 * (q.y * q.y + q.z * q.z));
|
||||||
|
}
|
||||||
|
|
||||||
|
double distance2d(
|
||||||
|
const geometry_msgs::msg::PoseStamped & a,
|
||||||
|
const geometry_msgs::msg::PoseStamped & b)
|
||||||
|
{
|
||||||
|
const double dx = a.pose.position.x - b.pose.position.x;
|
||||||
|
const double dy = a.pose.position.y - b.pose.position.y;
|
||||||
|
return std::hypot(dx, dy);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::size_t nearestPathIndex(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
const geometry_msgs::msg::PoseStamped & pose)
|
||||||
|
{
|
||||||
|
std::size_t best_index = 0;
|
||||||
|
double best_distance = std::numeric_limits<double>::infinity();
|
||||||
|
|
||||||
|
for (std::size_t i = 0; i < path.poses.size(); ++i) {
|
||||||
|
const double d = distance2d(path.poses[i], pose);
|
||||||
|
if (d < best_distance) {
|
||||||
|
best_distance = d;
|
||||||
|
best_index = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return best_index;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::size_t advanceByDistance(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
double distance_m)
|
||||||
|
{
|
||||||
|
if (path.poses.empty()) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::size_t index = std::min(start_index, path.poses.size() - 1);
|
||||||
|
double traveled = 0.0;
|
||||||
|
while (index + 1 < path.poses.size() && traveled < distance_m) {
|
||||||
|
traveled += distance2d(path.poses[index], path.poses[index + 1]);
|
||||||
|
++index;
|
||||||
|
}
|
||||||
|
return index;
|
||||||
|
}
|
||||||
|
|
||||||
|
CollisionCheckResult checkPathAhead(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
const nav_msgs::msg::OccupancyGrid & costmap,
|
||||||
|
const GuardSettings & settings)
|
||||||
|
{
|
||||||
|
if (path.poses.empty()) {
|
||||||
|
return CollisionCheckResult{true, 0, "empty_path"};
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto clamped_start = std::min(start_index, path.poses.size() - 1);
|
||||||
|
const auto end_index =
|
||||||
|
advanceByDistance(path, clamped_start, settings.lookahead_distance);
|
||||||
|
const double step = std::max(0.02, settings.footprint_sample_step);
|
||||||
|
|
||||||
|
for (std::size_t i = clamped_start; i <= end_index && i < path.poses.size(); ++i) {
|
||||||
|
if (i + 1 >= path.poses.size() || i == end_index) {
|
||||||
|
const auto & pose = path.poses[i];
|
||||||
|
if (footprintOccupied(
|
||||||
|
costmap, pose.pose.position.x, pose.pose.position.y, yawFromPose(pose), settings))
|
||||||
|
{
|
||||||
|
return CollisionCheckResult{true, i, "occupied"};
|
||||||
|
}
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto & a = path.poses[i];
|
||||||
|
const auto & b = path.poses[i + 1];
|
||||||
|
const double dx = b.pose.position.x - a.pose.position.x;
|
||||||
|
const double dy = b.pose.position.y - a.pose.position.y;
|
||||||
|
const double segment_length = std::hypot(dx, dy);
|
||||||
|
const double yaw = segment_length > 1.0e-6 ? std::atan2(dy, dx) : yawFromPose(a);
|
||||||
|
const int samples = std::max(1, static_cast<int>(std::ceil(segment_length / step)));
|
||||||
|
|
||||||
|
for (int sample_index = 0; sample_index <= samples; ++sample_index) {
|
||||||
|
const double t = static_cast<double>(sample_index) / static_cast<double>(samples);
|
||||||
|
const double x = a.pose.position.x + t * dx;
|
||||||
|
const double y = a.pose.position.y + t * dy;
|
||||||
|
if (footprintOccupied(costmap, x, y, yaw, settings)) {
|
||||||
|
return CollisionCheckResult{true, i, "occupied"};
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return CollisionCheckResult{false, end_index, "clear"};
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<std::size_t> findClearRejoinIndex(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
const nav_msgs::msg::OccupancyGrid & costmap,
|
||||||
|
const GuardSettings & settings)
|
||||||
|
{
|
||||||
|
if (path.poses.empty()) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto first = advanceByDistance(path, start_index, settings.rejoin_min_distance);
|
||||||
|
const auto last = advanceByDistance(path, start_index, settings.rejoin_max_distance);
|
||||||
|
GuardSettings single_pose_settings = settings;
|
||||||
|
single_pose_settings.lookahead_distance = 0.0;
|
||||||
|
|
||||||
|
for (std::size_t i = first; i <= last && i < path.poses.size(); ++i) {
|
||||||
|
const auto result = checkPathAhead(path, i, costmap, single_pose_settings);
|
||||||
|
if (!result.blocked) {
|
||||||
|
return i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<std::size_t> findClearRejoinIndexWithPlannerCostmap(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
const nav_msgs::msg::OccupancyGrid & local_costmap,
|
||||||
|
const nav_msgs::msg::OccupancyGrid * planner_costmap,
|
||||||
|
const GuardSettings & settings)
|
||||||
|
{
|
||||||
|
if (planner_costmap != nullptr) {
|
||||||
|
return findClearRejoinIndex(path, start_index, *planner_costmap, settings);
|
||||||
|
}
|
||||||
|
return findClearRejoinIndex(path, start_index, local_costmap, settings);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool shouldRetryBlockedRepair(
|
||||||
|
const std::optional<std::size_t> & last_repair_nearest_index,
|
||||||
|
std::size_t nearest_index,
|
||||||
|
int min_repair_progress_indices,
|
||||||
|
double seconds_since_last_repair,
|
||||||
|
double blocked_retry_wait_sec,
|
||||||
|
bool costmap_updated_since_repair)
|
||||||
|
{
|
||||||
|
if (!last_repair_nearest_index.has_value()) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto min_progress =
|
||||||
|
static_cast<std::size_t>(std::max(0, min_repair_progress_indices));
|
||||||
|
if (nearest_index > *last_repair_nearest_index + min_progress) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return seconds_since_last_repair >= blocked_retry_wait_sec &&
|
||||||
|
costmap_updated_since_repair;
|
||||||
|
}
|
||||||
|
|
||||||
|
nav_msgs::msg::Path slicePath(
|
||||||
|
const nav_msgs::msg::Path & path,
|
||||||
|
std::size_t start_index,
|
||||||
|
std::size_t end_index_inclusive)
|
||||||
|
{
|
||||||
|
nav_msgs::msg::Path out;
|
||||||
|
out.header = path.header;
|
||||||
|
if (path.poses.empty()) {
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto begin = std::min(start_index, path.poses.size() - 1);
|
||||||
|
const auto end = std::min(end_index_inclusive, path.poses.size() - 1);
|
||||||
|
for (std::size_t i = begin; i <= end; ++i) {
|
||||||
|
out.poses.push_back(path.poses[i]);
|
||||||
|
}
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
nav_msgs::msg::Path stitchPaths(
|
||||||
|
const nav_msgs::msg::Path & bypass,
|
||||||
|
const nav_msgs::msg::Path & original,
|
||||||
|
std::size_t rejoin_index)
|
||||||
|
{
|
||||||
|
nav_msgs::msg::Path out;
|
||||||
|
out.header = bypass.header.frame_id.empty() ? original.header : bypass.header;
|
||||||
|
out.poses = bypass.poses;
|
||||||
|
|
||||||
|
if (original.poses.empty()) {
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto begin = std::min(rejoin_index, original.poses.size());
|
||||||
|
for (std::size_t i = begin; i < original.poses.size(); ++i) {
|
||||||
|
out.poses.push_back(original.poses[i]);
|
||||||
|
}
|
||||||
|
return out;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace obstacle_nav2
|
||||||
459
src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp
Normal file
459
src/navigation/obstacle_nav2/src/trajectory_guard_node.cpp
Normal file
@@ -0,0 +1,459 @@
|
|||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <memory>
|
||||||
|
#include <optional>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||||
|
#include "geometry_msgs/msg/twist.hpp"
|
||||||
|
#include "nav2_msgs/action/compute_path_to_pose.hpp"
|
||||||
|
#include "nav2_msgs/action/follow_path.hpp"
|
||||||
|
#include "nav_msgs/msg/occupancy_grid.hpp"
|
||||||
|
#include "nav_msgs/msg/odometry.hpp"
|
||||||
|
#include "nav_msgs/msg/path.hpp"
|
||||||
|
#include "obstacle_nav2/trajectory_guard.hpp"
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include "rclcpp_action/rclcpp_action.hpp"
|
||||||
|
|
||||||
|
using namespace std::chrono_literals;
|
||||||
|
|
||||||
|
namespace obstacle_nav2
|
||||||
|
{
|
||||||
|
|
||||||
|
class TrajectoryGuardNode : public rclcpp::Node
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
using ComputePathToPose = nav2_msgs::action::ComputePathToPose;
|
||||||
|
using FollowPath = nav2_msgs::action::FollowPath;
|
||||||
|
using ComputeGoalHandle = rclcpp_action::ClientGoalHandle<ComputePathToPose>;
|
||||||
|
using FollowGoalHandle = rclcpp_action::ClientGoalHandle<FollowPath>;
|
||||||
|
|
||||||
|
TrajectoryGuardNode()
|
||||||
|
: Node("trajectory_guard_node")
|
||||||
|
{
|
||||||
|
declare_parameter("input_path_topic", "/trajectory_guard/input_path");
|
||||||
|
declare_parameter("patched_path_topic", "/trajectory_guard/patched_path");
|
||||||
|
declare_parameter("reject_path_topic", "/reject_path");
|
||||||
|
declare_parameter("costmap_topic", "/local_costmap/costmap");
|
||||||
|
declare_parameter("planner_costmap_topic", "/global_costmap/costmap");
|
||||||
|
declare_parameter("odom_topic", "/odom_combined");
|
||||||
|
declare_parameter("planner_action", "/compute_path_to_pose");
|
||||||
|
declare_parameter("follow_action", "/follow_path");
|
||||||
|
declare_parameter("planner_id", "GridBased");
|
||||||
|
declare_parameter("controller_id", "FollowPath");
|
||||||
|
declare_parameter("goal_checker_id", "");
|
||||||
|
declare_parameter("check_period_sec", 0.5);
|
||||||
|
declare_parameter("lookahead_distance", 2.0);
|
||||||
|
declare_parameter("rejoin_min_distance", 1.0);
|
||||||
|
declare_parameter("rejoin_max_distance", 4.0);
|
||||||
|
declare_parameter("occupied_threshold", 50);
|
||||||
|
declare_parameter("treat_unknown_as_occupied", true);
|
||||||
|
declare_parameter("footprint_half_length", 0.14);
|
||||||
|
declare_parameter("footprint_half_width", 0.085);
|
||||||
|
declare_parameter("footprint_padding", 0.04);
|
||||||
|
declare_parameter("footprint_sample_step", 0.05);
|
||||||
|
declare_parameter("repair_cooldown_sec", 1.0);
|
||||||
|
declare_parameter("blocked_retry_wait_sec", 0.8);
|
||||||
|
declare_parameter("wait_for_planner_costmap_update", true);
|
||||||
|
declare_parameter("min_repair_progress_indices", 5);
|
||||||
|
declare_parameter("publish_zero_on_blocked", true);
|
||||||
|
declare_parameter("cmd_vel_topic", "/cmd_vel");
|
||||||
|
declare_parameter("execute_follow_path", true);
|
||||||
|
|
||||||
|
loadParameters();
|
||||||
|
last_repair_request_time_ =
|
||||||
|
get_clock()->now() - rclcpp::Duration::from_seconds(repair_cooldown_sec_);
|
||||||
|
|
||||||
|
path_sub_ = create_subscription<nav_msgs::msg::Path>(
|
||||||
|
get_parameter("input_path_topic").as_string(), 1,
|
||||||
|
[this](nav_msgs::msg::Path::SharedPtr msg) { onPath(msg); });
|
||||||
|
costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
|
||||||
|
get_parameter("costmap_topic").as_string(), 1,
|
||||||
|
[this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
|
||||||
|
latest_costmap_ = msg;
|
||||||
|
++local_costmap_generation_;
|
||||||
|
});
|
||||||
|
planner_costmap_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
|
||||||
|
get_parameter("planner_costmap_topic").as_string(), 1,
|
||||||
|
[this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) {
|
||||||
|
latest_planner_costmap_ = msg;
|
||||||
|
++planner_costmap_generation_;
|
||||||
|
});
|
||||||
|
odom_sub_ = create_subscription<nav_msgs::msg::Odometry>(
|
||||||
|
get_parameter("odom_topic").as_string(), 10,
|
||||||
|
[this](nav_msgs::msg::Odometry::SharedPtr msg) { latest_odom_ = msg; });
|
||||||
|
|
||||||
|
patched_path_pub_ = create_publisher<nav_msgs::msg::Path>(
|
||||||
|
get_parameter("patched_path_topic").as_string(), 1);
|
||||||
|
reject_path_pub_ = create_publisher<nav_msgs::msg::Path>(
|
||||||
|
get_parameter("reject_path_topic").as_string(), 1);
|
||||||
|
cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>(
|
||||||
|
get_parameter("cmd_vel_topic").as_string(), 1);
|
||||||
|
|
||||||
|
planner_client_ = rclcpp_action::create_client<ComputePathToPose>(
|
||||||
|
this, get_parameter("planner_action").as_string());
|
||||||
|
follow_client_ = rclcpp_action::create_client<FollowPath>(
|
||||||
|
this, get_parameter("follow_action").as_string());
|
||||||
|
|
||||||
|
const auto period = std::chrono::duration<double>(
|
||||||
|
std::max(0.1, get_parameter("check_period_sec").as_double()));
|
||||||
|
timer_ = create_wall_timer(
|
||||||
|
std::chrono::duration_cast<std::chrono::milliseconds>(period),
|
||||||
|
[this]() { tick(); });
|
||||||
|
|
||||||
|
RCLCPP_INFO(
|
||||||
|
get_logger(), "trajectory guard ready check_period_sec=%.2f input=%s patched=%s",
|
||||||
|
get_parameter("check_period_sec").as_double(),
|
||||||
|
get_parameter("input_path_topic").as_string().c_str(),
|
||||||
|
get_parameter("patched_path_topic").as_string().c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
void loadParameters()
|
||||||
|
{
|
||||||
|
settings_.lookahead_distance = get_parameter("lookahead_distance").as_double();
|
||||||
|
settings_.rejoin_min_distance = get_parameter("rejoin_min_distance").as_double();
|
||||||
|
settings_.rejoin_max_distance = get_parameter("rejoin_max_distance").as_double();
|
||||||
|
settings_.occupied_threshold = get_parameter("occupied_threshold").as_int();
|
||||||
|
settings_.treat_unknown_as_occupied =
|
||||||
|
get_parameter("treat_unknown_as_occupied").as_bool();
|
||||||
|
settings_.footprint_half_length = get_parameter("footprint_half_length").as_double();
|
||||||
|
settings_.footprint_half_width = get_parameter("footprint_half_width").as_double();
|
||||||
|
settings_.footprint_padding = get_parameter("footprint_padding").as_double();
|
||||||
|
settings_.footprint_sample_step = get_parameter("footprint_sample_step").as_double();
|
||||||
|
repair_cooldown_sec_ = std::max(0.0, get_parameter("repair_cooldown_sec").as_double());
|
||||||
|
blocked_retry_wait_sec_ =
|
||||||
|
std::max(0.0, get_parameter("blocked_retry_wait_sec").as_double());
|
||||||
|
wait_for_planner_costmap_update_ =
|
||||||
|
get_parameter("wait_for_planner_costmap_update").as_bool();
|
||||||
|
min_repair_progress_indices_ =
|
||||||
|
std::max(0, static_cast<int>(get_parameter("min_repair_progress_indices").as_int()));
|
||||||
|
publish_zero_on_blocked_ = get_parameter("publish_zero_on_blocked").as_bool();
|
||||||
|
execute_follow_path_ = get_parameter("execute_follow_path").as_bool();
|
||||||
|
}
|
||||||
|
|
||||||
|
void onPath(const nav_msgs::msg::Path::SharedPtr msg)
|
||||||
|
{
|
||||||
|
active_path_ = *msg;
|
||||||
|
sent_original_for_current_path_ = false;
|
||||||
|
last_repair_nearest_index_.reset();
|
||||||
|
last_repair_rejoin_index_.reset();
|
||||||
|
retry_without_planner_costmap_update_ = false;
|
||||||
|
waiting_for_planner_costmap_update_ = false;
|
||||||
|
RCLCPP_INFO(
|
||||||
|
get_logger(), "received replay path poses=%zu frame=%s",
|
||||||
|
active_path_.poses.size(), active_path_.header.frame_id.c_str());
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<geometry_msgs::msg::PoseStamped> currentPose() const
|
||||||
|
{
|
||||||
|
if (!latest_odom_) {
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
geometry_msgs::msg::PoseStamped pose;
|
||||||
|
pose.header = latest_odom_->header;
|
||||||
|
pose.pose = latest_odom_->pose.pose;
|
||||||
|
return pose;
|
||||||
|
}
|
||||||
|
|
||||||
|
void tick()
|
||||||
|
{
|
||||||
|
if (active_path_.poses.empty() || !latest_costmap_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto pose = currentPose();
|
||||||
|
if (!pose.has_value()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto nearest = nearestPathIndex(active_path_, *pose);
|
||||||
|
const auto result = checkPathAhead(active_path_, nearest, *latest_costmap_, settings_);
|
||||||
|
if (!result.blocked) {
|
||||||
|
if (!sent_original_for_current_path_) {
|
||||||
|
publishAndMaybeFollow(active_path_, "original_clear");
|
||||||
|
sent_original_for_current_path_ = true;
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto now = get_clock()->now();
|
||||||
|
if (repair_in_flight_ ||
|
||||||
|
(now - last_repair_request_time_).seconds() < repair_cooldown_sec_)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!waiting_for_planner_costmap_update_) {
|
||||||
|
waiting_for_planner_costmap_update_ = true;
|
||||||
|
waiting_planner_costmap_generation_ = planner_costmap_generation_;
|
||||||
|
RCLCPP_WARN_THROTTLE(
|
||||||
|
get_logger(), *get_clock(), 2000,
|
||||||
|
"blocked replay path; waiting for next planner costmap frame before requesting bypass");
|
||||||
|
publishStop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (planner_costmap_generation_ <= waiting_planner_costmap_generation_) {
|
||||||
|
if (retry_without_planner_costmap_update_ &&
|
||||||
|
(now - last_repair_request_time_).seconds() >= blocked_retry_wait_sec_)
|
||||||
|
{
|
||||||
|
waiting_for_planner_costmap_update_ = false;
|
||||||
|
} else {
|
||||||
|
RCLCPP_WARN_THROTTLE(
|
||||||
|
get_logger(), *get_clock(), 2000,
|
||||||
|
"blocked replay path; planner costmap has not updated yet");
|
||||||
|
publishStop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
waiting_for_planner_costmap_update_ = false;
|
||||||
|
|
||||||
|
const bool costmap_updated_since_repair =
|
||||||
|
!wait_for_planner_costmap_update_ ||
|
||||||
|
planner_costmap_generation_ > last_repair_planner_costmap_generation_;
|
||||||
|
const bool timeout_retry_allowed =
|
||||||
|
retry_without_planner_costmap_update_ &&
|
||||||
|
(now - last_repair_request_time_).seconds() >= blocked_retry_wait_sec_;
|
||||||
|
if (!shouldRetryBlockedRepair(
|
||||||
|
last_repair_nearest_index_, nearest, min_repair_progress_indices_,
|
||||||
|
(now - last_repair_request_time_).seconds(), blocked_retry_wait_sec_,
|
||||||
|
costmap_updated_since_repair) && !timeout_retry_allowed)
|
||||||
|
{
|
||||||
|
RCLCPP_WARN_THROTTLE(
|
||||||
|
get_logger(), *get_clock(), 2000,
|
||||||
|
"patched path still blocked near the last repair point; waiting for path progress or costmap update");
|
||||||
|
publishStop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
retry_without_planner_costmap_update_ = false;
|
||||||
|
|
||||||
|
std::size_t search_start = nearest;
|
||||||
|
if (last_repair_rejoin_index_.has_value()) {
|
||||||
|
const auto bump = static_cast<std::size_t>(std::max(1, min_repair_progress_indices_));
|
||||||
|
const auto advanced = std::min(
|
||||||
|
active_path_.poses.size() - 1, *last_repair_rejoin_index_ + bump);
|
||||||
|
search_start = std::max(search_start, advanced);
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto rejoin = findClearRejoinIndexWithPlannerCostmap(
|
||||||
|
active_path_, search_start, *latest_costmap_, latest_planner_costmap_.get(), settings_);
|
||||||
|
if (!rejoin.has_value()) {
|
||||||
|
RCLCPP_WARN(get_logger(), "blocked replay path but no clear rejoin point found");
|
||||||
|
publishStop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_WARN(
|
||||||
|
get_logger(), "blocked replay path index=%zu reason=%s rejoin_index=%zu",
|
||||||
|
result.path_index, result.reason.c_str(), *rejoin);
|
||||||
|
requestBypass(nearest, *rejoin);
|
||||||
|
}
|
||||||
|
|
||||||
|
void publishAndMaybeFollow(const nav_msgs::msg::Path & path, const std::string & label)
|
||||||
|
{
|
||||||
|
if (path.poses.empty()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
nav_msgs::msg::Path stamped_path = path;
|
||||||
|
stamped_path.header.stamp = now();
|
||||||
|
for (auto & pose : stamped_path.poses) {
|
||||||
|
pose.header.stamp = stamped_path.header.stamp;
|
||||||
|
if (pose.header.frame_id.empty()) {
|
||||||
|
pose.header.frame_id = stamped_path.header.frame_id;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
patched_path_pub_->publish(stamped_path);
|
||||||
|
|
||||||
|
if (!execute_follow_path_) {
|
||||||
|
RCLCPP_INFO(
|
||||||
|
get_logger(), "published %s path poses=%zu without FollowPath execution",
|
||||||
|
label.c_str(), stamped_path.poses.size());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
sendFollowPath(stamped_path, label);
|
||||||
|
}
|
||||||
|
|
||||||
|
void sendFollowPath(const nav_msgs::msg::Path & path, const std::string & label)
|
||||||
|
{
|
||||||
|
if (!follow_client_->wait_for_action_server(100ms)) {
|
||||||
|
RCLCPP_WARN(get_logger(), "FollowPath action server not ready");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (active_follow_goal_handle_) {
|
||||||
|
follow_client_->async_cancel_goal(active_follow_goal_handle_);
|
||||||
|
active_follow_goal_handle_.reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
FollowPath::Goal goal;
|
||||||
|
goal.path = path;
|
||||||
|
goal.controller_id = get_parameter("controller_id").as_string();
|
||||||
|
goal.goal_checker_id = get_parameter("goal_checker_id").as_string();
|
||||||
|
|
||||||
|
auto options = rclcpp_action::Client<FollowPath>::SendGoalOptions();
|
||||||
|
options.goal_response_callback =
|
||||||
|
[this, label](FollowGoalHandle::SharedPtr goal_handle) {
|
||||||
|
if (!goal_handle) {
|
||||||
|
RCLCPP_WARN(get_logger(), "FollowPath %s rejected", label.c_str());
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
active_follow_goal_handle_ = goal_handle;
|
||||||
|
RCLCPP_INFO(get_logger(), "FollowPath %s accepted", label.c_str());
|
||||||
|
};
|
||||||
|
options.result_callback =
|
||||||
|
[this, label](const FollowGoalHandle::WrappedResult & result) {
|
||||||
|
active_follow_goal_handle_.reset();
|
||||||
|
RCLCPP_INFO(
|
||||||
|
get_logger(), "FollowPath %s finished code=%d",
|
||||||
|
label.c_str(), static_cast<int>(result.code));
|
||||||
|
};
|
||||||
|
|
||||||
|
follow_client_->async_send_goal(goal, options);
|
||||||
|
RCLCPP_INFO(get_logger(), "sent FollowPath %s poses=%zu", label.c_str(), path.poses.size());
|
||||||
|
}
|
||||||
|
|
||||||
|
void requestBypass(std::size_t nearest_index, std::size_t rejoin_index)
|
||||||
|
{
|
||||||
|
const auto start = currentPose();
|
||||||
|
if (!start.has_value() || rejoin_index >= active_path_.poses.size()) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!planner_client_->wait_for_action_server(100ms)) {
|
||||||
|
RCLCPP_WARN(get_logger(), "ComputePathToPose action server not ready");
|
||||||
|
publishStop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
repair_in_flight_ = true;
|
||||||
|
last_repair_request_time_ = get_clock()->now();
|
||||||
|
last_repair_nearest_index_ = nearest_index;
|
||||||
|
last_repair_rejoin_index_ = rejoin_index;
|
||||||
|
last_repair_planner_costmap_generation_ = planner_costmap_generation_;
|
||||||
|
waiting_for_planner_costmap_update_ = false;
|
||||||
|
|
||||||
|
ComputePathToPose::Goal goal;
|
||||||
|
goal.start = *start;
|
||||||
|
goal.goal = active_path_.poses[rejoin_index];
|
||||||
|
goal.planner_id = get_parameter("planner_id").as_string();
|
||||||
|
goal.use_start = true;
|
||||||
|
|
||||||
|
auto options = rclcpp_action::Client<ComputePathToPose>::SendGoalOptions();
|
||||||
|
options.goal_response_callback =
|
||||||
|
[this](ComputeGoalHandle::SharedPtr goal_handle) {
|
||||||
|
if (!goal_handle) {
|
||||||
|
repair_in_flight_ = false;
|
||||||
|
RCLCPP_WARN(get_logger(), "bypass planning goal rejected");
|
||||||
|
}
|
||||||
|
};
|
||||||
|
options.result_callback =
|
||||||
|
[this, rejoin_index](const ComputeGoalHandle::WrappedResult & result) {
|
||||||
|
repair_in_flight_ = false;
|
||||||
|
if (result.code != rclcpp_action::ResultCode::SUCCEEDED ||
|
||||||
|
result.result->path.poses.empty())
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(get_logger(), "bypass planning failed code=%d", static_cast<int>(result.code));
|
||||||
|
publishStop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto patched = stitchPaths(result.result->path, active_path_, rejoin_index);
|
||||||
|
if (!patchedPathIsClear(patched)) {
|
||||||
|
reject_path_pub_->publish(patched);
|
||||||
|
retry_without_planner_costmap_update_ = true;
|
||||||
|
waiting_for_planner_costmap_update_ = false;
|
||||||
|
RCLCPP_WARN(
|
||||||
|
get_logger(),
|
||||||
|
"planned bypass still blocked in local costmap; stopping and retrying from a later rejoin point");
|
||||||
|
publishStop();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
active_path_ = patched;
|
||||||
|
sent_original_for_current_path_ = true;
|
||||||
|
waiting_for_planner_costmap_update_ = false;
|
||||||
|
retry_without_planner_costmap_update_ = false;
|
||||||
|
publishAndMaybeFollow(patched, "patched_bypass");
|
||||||
|
};
|
||||||
|
|
||||||
|
planner_client_->async_send_goal(goal, options);
|
||||||
|
RCLCPP_INFO(get_logger(), "requested bypass to rejoin_index=%zu", rejoin_index);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool patchedPathIsClear(const nav_msgs::msg::Path & patched) const
|
||||||
|
{
|
||||||
|
if (!latest_costmap_) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto pose = currentPose();
|
||||||
|
if (!pose.has_value()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto nearest = nearestPathIndex(patched, *pose);
|
||||||
|
const auto result = checkPathAhead(patched, nearest, *latest_costmap_, settings_);
|
||||||
|
return !result.blocked;
|
||||||
|
}
|
||||||
|
|
||||||
|
void publishStop()
|
||||||
|
{
|
||||||
|
if (!publish_zero_on_blocked_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
geometry_msgs::msg::Twist stop;
|
||||||
|
cmd_vel_pub_->publish(stop);
|
||||||
|
}
|
||||||
|
|
||||||
|
GuardSettings settings_;
|
||||||
|
double repair_cooldown_sec_{1.0};
|
||||||
|
double blocked_retry_wait_sec_{0.8};
|
||||||
|
int min_repair_progress_indices_{5};
|
||||||
|
bool publish_zero_on_blocked_{true};
|
||||||
|
bool execute_follow_path_{true};
|
||||||
|
bool wait_for_planner_costmap_update_{true};
|
||||||
|
bool retry_without_planner_costmap_update_{false};
|
||||||
|
bool repair_in_flight_{false};
|
||||||
|
bool sent_original_for_current_path_{false};
|
||||||
|
bool waiting_for_planner_costmap_update_{false};
|
||||||
|
rclcpp::Time last_repair_request_time_{0, 0, RCL_ROS_TIME};
|
||||||
|
std::optional<std::size_t> last_repair_nearest_index_;
|
||||||
|
std::optional<std::size_t> last_repair_rejoin_index_;
|
||||||
|
std::uint64_t local_costmap_generation_{0};
|
||||||
|
std::uint64_t planner_costmap_generation_{0};
|
||||||
|
std::uint64_t last_repair_planner_costmap_generation_{0};
|
||||||
|
std::uint64_t waiting_planner_costmap_generation_{0};
|
||||||
|
|
||||||
|
nav_msgs::msg::Path active_path_;
|
||||||
|
nav_msgs::msg::OccupancyGrid::SharedPtr latest_costmap_;
|
||||||
|
nav_msgs::msg::OccupancyGrid::SharedPtr latest_planner_costmap_;
|
||||||
|
nav_msgs::msg::Odometry::SharedPtr latest_odom_;
|
||||||
|
FollowGoalHandle::SharedPtr active_follow_goal_handle_;
|
||||||
|
|
||||||
|
rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
|
||||||
|
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr costmap_sub_;
|
||||||
|
rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr planner_costmap_sub_;
|
||||||
|
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||||
|
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr patched_path_pub_;
|
||||||
|
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr reject_path_pub_;
|
||||||
|
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
|
||||||
|
rclcpp_action::Client<ComputePathToPose>::SharedPtr planner_client_;
|
||||||
|
rclcpp_action::Client<FollowPath>::SharedPtr follow_client_;
|
||||||
|
rclcpp::TimerBase::SharedPtr timer_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace obstacle_nav2
|
||||||
|
|
||||||
|
int main(int argc, char ** argv)
|
||||||
|
{
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
rclcpp::spin(std::make_shared<obstacle_nav2::TrajectoryGuardNode>());
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
127
src/navigation/obstacle_nav2/test/test_nav2_profile_loader.cpp
Normal file
127
src/navigation/obstacle_nav2/test/test_nav2_profile_loader.cpp
Normal file
@@ -0,0 +1,127 @@
|
|||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include <fstream>
|
||||||
|
#include <set>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include "obstacle_nav2/nav2_profile_loader.hpp"
|
||||||
|
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
|
||||||
|
std::string writeTempProfile(const std::string & yaml)
|
||||||
|
{
|
||||||
|
const auto path = "/tmp/nav2_profile_loader_test.yaml";
|
||||||
|
std::ofstream out(path);
|
||||||
|
out << yaml;
|
||||||
|
out.close();
|
||||||
|
return path;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
TEST(Nav2ProfileLoaderTest, LoadsScalarAndArrayParameters)
|
||||||
|
{
|
||||||
|
const auto path = writeTempProfile(R"(
|
||||||
|
nodes:
|
||||||
|
/controller_server:
|
||||||
|
FollowPath.vx_max: 0.35
|
||||||
|
FollowPath.vx_min: -0.10
|
||||||
|
FollowPath.wz_max: 1.2
|
||||||
|
FollowPath.AckermannConstraints.min_turning_r: 0.75
|
||||||
|
/velocity_smoother:
|
||||||
|
max_velocity: [0.35, 0.0, 1.2]
|
||||||
|
min_velocity: [-0.10, 0.0, -1.2]
|
||||||
|
)");
|
||||||
|
|
||||||
|
const auto profile = obstacle_nav2::loadNav2Profile(path);
|
||||||
|
|
||||||
|
ASSERT_EQ(profile.nodes.size(), 2u);
|
||||||
|
EXPECT_EQ(profile.nodes[0].node_name, "/controller_server");
|
||||||
|
ASSERT_EQ(profile.nodes[0].parameters.size(), 4u);
|
||||||
|
EXPECT_EQ(profile.nodes[0].parameters[0].get_name(), "FollowPath.vx_max");
|
||||||
|
EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[0].as_double(), 0.35);
|
||||||
|
EXPECT_EQ(
|
||||||
|
profile.nodes[0].parameters[3].get_name(),
|
||||||
|
"FollowPath.AckermannConstraints.min_turning_r");
|
||||||
|
EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[3].as_double(), 0.75);
|
||||||
|
|
||||||
|
EXPECT_EQ(profile.nodes[1].node_name, "/velocity_smoother");
|
||||||
|
ASSERT_EQ(profile.nodes[1].parameters.size(), 2u);
|
||||||
|
const auto max_velocity = profile.nodes[1].parameters[0].as_double_array();
|
||||||
|
ASSERT_EQ(max_velocity.size(), 3u);
|
||||||
|
EXPECT_DOUBLE_EQ(max_velocity[0], 0.35);
|
||||||
|
EXPECT_DOUBLE_EQ(max_velocity[2], 1.2);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(Nav2ProfileLoaderTest, LoadsNativeNav2RosParametersFile)
|
||||||
|
{
|
||||||
|
const auto path = writeTempProfile(R"(
|
||||||
|
bt_navigator:
|
||||||
|
ros__parameters:
|
||||||
|
default_server_timeout: 20
|
||||||
|
controller_server:
|
||||||
|
ros__parameters:
|
||||||
|
controller_frequency: 20.0
|
||||||
|
FollowPath:
|
||||||
|
vx_max: 0.35
|
||||||
|
wz_max: 1.2
|
||||||
|
motion_model: "Ackermann"
|
||||||
|
critics: ["ConstraintCritic", "CostCritic"]
|
||||||
|
AckermannConstraints:
|
||||||
|
min_turning_r: 0.75
|
||||||
|
velocity_smoother:
|
||||||
|
ros__parameters:
|
||||||
|
max_velocity: [0.35, 0.0, 1.2]
|
||||||
|
min_velocity: [-0.10, 0.0, -1.2]
|
||||||
|
)");
|
||||||
|
|
||||||
|
const auto profile = obstacle_nav2::loadNav2Profile(path);
|
||||||
|
|
||||||
|
ASSERT_EQ(profile.nodes.size(), 2u);
|
||||||
|
EXPECT_EQ(profile.nodes[0].node_name, "/controller_server");
|
||||||
|
ASSERT_EQ(profile.nodes[0].parameters.size(), 6u);
|
||||||
|
EXPECT_EQ(profile.nodes[0].parameters[1].get_name(), "FollowPath.vx_max");
|
||||||
|
EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[1].as_double(), 0.35);
|
||||||
|
EXPECT_EQ(
|
||||||
|
profile.nodes[0].parameters[5].get_name(),
|
||||||
|
"FollowPath.AckermannConstraints.min_turning_r");
|
||||||
|
EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[5].as_double(), 0.75);
|
||||||
|
|
||||||
|
EXPECT_EQ(profile.nodes[1].node_name, "/velocity_smoother");
|
||||||
|
ASSERT_EQ(profile.nodes[1].parameters.size(), 2u);
|
||||||
|
const auto max_velocity = profile.nodes[1].parameters[0].as_double_array();
|
||||||
|
ASSERT_EQ(max_velocity.size(), 3u);
|
||||||
|
EXPECT_DOUBLE_EQ(max_velocity[2], 1.2);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(Nav2ProfileLoaderTest, RejectsProfileWithoutNodesMap)
|
||||||
|
{
|
||||||
|
const auto path = writeTempProfile(R"(
|
||||||
|
controller_server:
|
||||||
|
FollowPath.vx_max: 0.35
|
||||||
|
)");
|
||||||
|
|
||||||
|
EXPECT_THROW(
|
||||||
|
obstacle_nav2::loadNav2Profile(path),
|
||||||
|
std::runtime_error);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(Nav2ProfileLoaderTest, FiltersParametersByDeclaredNames)
|
||||||
|
{
|
||||||
|
const std::vector<rclcpp::Parameter> parameters{
|
||||||
|
rclcpp::Parameter("FollowPath.vx_max", 0.35),
|
||||||
|
rclcpp::Parameter("FollowPath.CostCritic.trajectory_point_step", 2),
|
||||||
|
rclcpp::Parameter("max_velocity", std::vector<double>{0.35, 0.0, 1.2}),
|
||||||
|
};
|
||||||
|
const std::set<std::string> declared_names{
|
||||||
|
"FollowPath.vx_max",
|
||||||
|
"max_velocity",
|
||||||
|
};
|
||||||
|
|
||||||
|
const auto filtered = obstacle_nav2::filterDeclaredParameters(parameters, declared_names);
|
||||||
|
|
||||||
|
ASSERT_EQ(filtered.size(), 2u);
|
||||||
|
EXPECT_EQ(filtered[0].get_name(), "FollowPath.vx_max");
|
||||||
|
EXPECT_EQ(filtered[1].get_name(), "max_velocity");
|
||||||
|
}
|
||||||
@@ -186,6 +186,26 @@ TEST(ObstacleArrayLayerTest, EmptySnapshotClearsPreviousObstacle)
|
|||||||
EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE);
|
EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::FREE_SPACE);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST(ObstacleArrayLayerTest, EmptySnapshotCanBeIgnoredToPreservePreviousObstacle)
|
||||||
|
{
|
||||||
|
nav2_costmap_2d::Costmap2D grid(40, 40, kResolution, kOriginX, kOriginY);
|
||||||
|
const std::vector<ObstacleArrayLayer::CircleObstacle> occupied{{0.0, 0.0, 0.1}};
|
||||||
|
|
||||||
|
const auto occupied_bounds = ObstacleArrayLayer::applySnapshot(
|
||||||
|
grid, occupied, kResolution, kOriginX, kOriginY);
|
||||||
|
|
||||||
|
unsigned int mx, my;
|
||||||
|
ASSERT_TRUE(grid.worldToMap(0.0, 0.0, mx, my));
|
||||||
|
EXPECT_TRUE(occupied_bounds.valid);
|
||||||
|
EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||||
|
|
||||||
|
const auto retained = ObstacleArrayLayer::applySnapshotIfNotEmpty(
|
||||||
|
grid, {}, kResolution, kOriginX, kOriginY);
|
||||||
|
|
||||||
|
EXPECT_FALSE(retained.has_value());
|
||||||
|
EXPECT_EQ(grid.getCost(mx, my), nav2_costmap_2d::LETHAL_OBSTACLE);
|
||||||
|
}
|
||||||
|
|
||||||
TEST(ObstacleArrayLayerTest, TransformSnapshotUsesMessageTimestamp)
|
TEST(ObstacleArrayLayerTest, TransformSnapshotUsesMessageTimestamp)
|
||||||
{
|
{
|
||||||
auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
|
auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
|
||||||
|
|||||||
190
src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp
Normal file
190
src/navigation/obstacle_nav2/test/test_trajectory_guard.cpp
Normal file
@@ -0,0 +1,190 @@
|
|||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include "obstacle_nav2/trajectory_guard.hpp"
|
||||||
|
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
|
||||||
|
geometry_msgs::msg::PoseStamped pose(double x, double y)
|
||||||
|
{
|
||||||
|
geometry_msgs::msg::PoseStamped p;
|
||||||
|
p.header.frame_id = "odom";
|
||||||
|
p.pose.position.x = x;
|
||||||
|
p.pose.position.y = y;
|
||||||
|
p.pose.orientation.w = 1.0;
|
||||||
|
return p;
|
||||||
|
}
|
||||||
|
|
||||||
|
nav_msgs::msg::Path straightPath()
|
||||||
|
{
|
||||||
|
nav_msgs::msg::Path path;
|
||||||
|
path.header.frame_id = "odom";
|
||||||
|
for (int i = 0; i <= 10; ++i) {
|
||||||
|
path.poses.push_back(pose(0.2 * i, 0.0));
|
||||||
|
}
|
||||||
|
return path;
|
||||||
|
}
|
||||||
|
|
||||||
|
nav_msgs::msg::OccupancyGrid gridWithOrigin(double origin_x, double origin_y)
|
||||||
|
{
|
||||||
|
nav_msgs::msg::OccupancyGrid grid;
|
||||||
|
grid.header.frame_id = "odom";
|
||||||
|
grid.info.resolution = 0.1;
|
||||||
|
grid.info.width = 40;
|
||||||
|
grid.info.height = 30;
|
||||||
|
grid.info.origin.position.x = origin_x;
|
||||||
|
grid.info.origin.position.y = origin_y;
|
||||||
|
grid.data.assign(grid.info.width * grid.info.height, 0);
|
||||||
|
return grid;
|
||||||
|
}
|
||||||
|
|
||||||
|
void markOccupied(nav_msgs::msg::OccupancyGrid & grid, int mx, int my)
|
||||||
|
{
|
||||||
|
ASSERT_GE(mx, 0);
|
||||||
|
ASSERT_GE(my, 0);
|
||||||
|
ASSERT_LT(mx, static_cast<int>(grid.info.width));
|
||||||
|
ASSERT_LT(my, static_cast<int>(grid.info.height));
|
||||||
|
grid.data[my * grid.info.width + mx] = 100;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, NearestPathIndexFindsClosestPose)
|
||||||
|
{
|
||||||
|
const auto path = straightPath();
|
||||||
|
EXPECT_EQ(obstacle_nav2::nearestPathIndex(path, pose(0.43, 0.02)), 2u);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, AdvanceByDistanceStopsAtRequestedArcLength)
|
||||||
|
{
|
||||||
|
const auto path = straightPath();
|
||||||
|
EXPECT_EQ(obstacle_nav2::advanceByDistance(path, 0, 0.55), 3u);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, SlicePathIncludesEndIndex)
|
||||||
|
{
|
||||||
|
const auto path = straightPath();
|
||||||
|
const auto sliced = obstacle_nav2::slicePath(path, 2, 4);
|
||||||
|
ASSERT_EQ(sliced.poses.size(), 3u);
|
||||||
|
EXPECT_DOUBLE_EQ(sliced.poses.front().pose.position.x, 0.4);
|
||||||
|
EXPECT_DOUBLE_EQ(sliced.poses.back().pose.position.x, 0.8);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, StitchPathsAppendsOriginalFromRejoinIndex)
|
||||||
|
{
|
||||||
|
const auto original = straightPath();
|
||||||
|
nav_msgs::msg::Path bypass;
|
||||||
|
bypass.header.frame_id = "odom";
|
||||||
|
bypass.poses.push_back(pose(0.0, 0.0));
|
||||||
|
bypass.poses.push_back(pose(0.5, 0.3));
|
||||||
|
|
||||||
|
const auto stitched = obstacle_nav2::stitchPaths(bypass, original, 4);
|
||||||
|
ASSERT_EQ(stitched.poses.size(), 9u);
|
||||||
|
EXPECT_DOUBLE_EQ(stitched.poses[0].pose.position.y, 0.0);
|
||||||
|
EXPECT_DOUBLE_EQ(stitched.poses[1].pose.position.y, 0.3);
|
||||||
|
EXPECT_DOUBLE_EQ(stitched.poses[2].pose.position.x, 0.8);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, CheckPathAheadReportsOccupiedCell)
|
||||||
|
{
|
||||||
|
auto path = straightPath();
|
||||||
|
auto grid = gridWithOrigin(-1.0, -1.0);
|
||||||
|
markOccupied(grid, 15, 10);
|
||||||
|
|
||||||
|
obstacle_nav2::GuardSettings settings;
|
||||||
|
settings.lookahead_distance = 2.0;
|
||||||
|
settings.occupied_threshold = 50;
|
||||||
|
settings.footprint_half_length = 0.01;
|
||||||
|
settings.footprint_half_width = 0.01;
|
||||||
|
settings.footprint_padding = 0.0;
|
||||||
|
|
||||||
|
const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
|
||||||
|
EXPECT_TRUE(result.blocked);
|
||||||
|
EXPECT_EQ(result.reason, "occupied");
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, CheckPathAheadKeepsClearPathUnblocked)
|
||||||
|
{
|
||||||
|
auto path = straightPath();
|
||||||
|
auto grid = gridWithOrigin(-1.0, -1.0);
|
||||||
|
|
||||||
|
obstacle_nav2::GuardSettings settings;
|
||||||
|
settings.lookahead_distance = 2.0;
|
||||||
|
settings.occupied_threshold = 50;
|
||||||
|
settings.footprint_half_length = 0.01;
|
||||||
|
settings.footprint_half_width = 0.01;
|
||||||
|
settings.footprint_padding = 0.0;
|
||||||
|
|
||||||
|
const auto result = obstacle_nav2::checkPathAhead(path, 0, grid, settings);
|
||||||
|
EXPECT_FALSE(result.blocked);
|
||||||
|
EXPECT_EQ(result.reason, "clear");
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, FindClearRejoinIndexSkipsBlockedArea)
|
||||||
|
{
|
||||||
|
auto path = straightPath();
|
||||||
|
auto grid = gridWithOrigin(-1.0, -1.0);
|
||||||
|
for (int mx = 13; mx <= 18; ++mx) {
|
||||||
|
markOccupied(grid, mx, 10);
|
||||||
|
}
|
||||||
|
|
||||||
|
obstacle_nav2::GuardSettings settings;
|
||||||
|
settings.rejoin_min_distance = 0.5;
|
||||||
|
settings.rejoin_max_distance = 2.0;
|
||||||
|
settings.occupied_threshold = 50;
|
||||||
|
settings.footprint_half_length = 0.01;
|
||||||
|
settings.footprint_half_width = 0.01;
|
||||||
|
settings.footprint_padding = 0.0;
|
||||||
|
|
||||||
|
const auto rejoin = obstacle_nav2::findClearRejoinIndex(path, 0, grid, settings);
|
||||||
|
ASSERT_TRUE(rejoin.has_value());
|
||||||
|
EXPECT_GE(*rejoin, 5u);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, FindClearRejoinIndexCanUsePlannerCostmapBeyondLocalWindow)
|
||||||
|
{
|
||||||
|
auto path = straightPath();
|
||||||
|
auto local_grid = gridWithOrigin(-0.5, -1.0);
|
||||||
|
local_grid.info.width = 15;
|
||||||
|
local_grid.data.assign(local_grid.info.width * local_grid.info.height, 0);
|
||||||
|
|
||||||
|
auto planner_grid = gridWithOrigin(-1.0, -1.0);
|
||||||
|
planner_grid.info.width = 80;
|
||||||
|
planner_grid.data.assign(planner_grid.info.width * planner_grid.info.height, 0);
|
||||||
|
|
||||||
|
obstacle_nav2::GuardSettings settings;
|
||||||
|
settings.rejoin_min_distance = 1.5;
|
||||||
|
settings.rejoin_max_distance = 3.0;
|
||||||
|
settings.occupied_threshold = 50;
|
||||||
|
settings.treat_unknown_as_occupied = true;
|
||||||
|
settings.footprint_half_length = 0.01;
|
||||||
|
settings.footprint_half_width = 0.01;
|
||||||
|
settings.footprint_padding = 0.0;
|
||||||
|
|
||||||
|
EXPECT_FALSE(obstacle_nav2::findClearRejoinIndex(path, 0, local_grid, settings).has_value());
|
||||||
|
|
||||||
|
const auto rejoin = obstacle_nav2::findClearRejoinIndexWithPlannerCostmap(
|
||||||
|
path, 0, local_grid, &planner_grid, settings);
|
||||||
|
ASSERT_TRUE(rejoin.has_value());
|
||||||
|
EXPECT_GE(*rejoin, 8u);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, RetryBlockedRepairWaitsForCostmapUpdateAndRetryDelay)
|
||||||
|
{
|
||||||
|
const std::optional<std::size_t> last_repair_index = 10u;
|
||||||
|
|
||||||
|
EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
|
||||||
|
last_repair_index, 11u, 5, 0.8, 1.0, true));
|
||||||
|
EXPECT_FALSE(obstacle_nav2::shouldRetryBlockedRepair(
|
||||||
|
last_repair_index, 11u, 5, 1.2, 1.0, false));
|
||||||
|
EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
|
||||||
|
last_repair_index, 11u, 5, 1.2, 1.0, true));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TrajectoryGuard, RetryBlockedRepairAllowsProgressedPathImmediately)
|
||||||
|
{
|
||||||
|
const std::optional<std::size_t> last_repair_index = 10u;
|
||||||
|
|
||||||
|
EXPECT_TRUE(obstacle_nav2::shouldRetryBlockedRepair(
|
||||||
|
last_repair_index, 16u, 5, 0.1, 1.0, false));
|
||||||
|
}
|
||||||
@@ -63,7 +63,7 @@ public:
|
|||||||
1, static_cast<int>(declare_parameter<int>("debug_info_stride", 1)));
|
1, static_cast<int>(declare_parameter<int>("debug_info_stride", 1)));
|
||||||
|
|
||||||
obstacles_pub_ = create_publisher<obstacle_scanner::msg::ObstacleArray>(
|
obstacles_pub_ = create_publisher<obstacle_scanner::msg::ObstacleArray>(
|
||||||
"/obstacles", 10);
|
"/obstacles", rclcpp::QoS(rclcpp::KeepLast(1)).reliable());
|
||||||
|
|
||||||
if (debug_) {
|
if (debug_) {
|
||||||
debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
|
debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
|
||||||
|
|||||||
@@ -46,6 +46,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
|||||||
"msg/Position.msg"
|
"msg/Position.msg"
|
||||||
)
|
)
|
||||||
if(BUILD_TESTING)
|
if(BUILD_TESTING)
|
||||||
|
find_package(ament_cmake_gtest REQUIRED)
|
||||||
find_package(ament_lint_auto REQUIRED)
|
find_package(ament_lint_auto REQUIRED)
|
||||||
# the following line skips the linter which checks for copyrights
|
# the following line skips the linter which checks for copyrights
|
||||||
# uncomment the line when a copyright and license is not present in all source files
|
# uncomment the line when a copyright and license is not present in all source files
|
||||||
@@ -64,6 +65,12 @@ set(origincar_base_node_SRCS
|
|||||||
src/origincar_base.cpp
|
src/origincar_base.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
|
add_library(origincar_base_log STATIC src/log.cpp)
|
||||||
|
target_include_directories(origincar_base_log PUBLIC
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
|
||||||
add_executable(origincar_base_node src/origincar_base.cpp)
|
add_executable(origincar_base_node src/origincar_base.cpp)
|
||||||
ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description)
|
ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description)
|
||||||
|
|
||||||
@@ -80,7 +87,7 @@ target_include_directories(wall_kalman_filter PUBLIC
|
|||||||
)
|
)
|
||||||
target_link_libraries(wall_kalman_filter wall_fit_core)
|
target_link_libraries(wall_kalman_filter wall_fit_core)
|
||||||
|
|
||||||
target_link_libraries(origincar_base_node wall_kalman_filter wall_fit_core)
|
target_link_libraries(origincar_base_node origincar_base_log wall_kalman_filter wall_fit_core)
|
||||||
|
|
||||||
add_executable(wall_fit_calibrator src/wall_fit_calibrator.cpp)
|
add_executable(wall_fit_calibrator src/wall_fit_calibrator.cpp)
|
||||||
target_link_libraries(wall_fit_calibrator wall_fit_core)
|
target_link_libraries(wall_fit_calibrator wall_fit_core)
|
||||||
@@ -102,6 +109,19 @@ if(BUILD_TESTING)
|
|||||||
add_executable(wall_kalman_filter_test test/wall_kalman_filter_test.cpp)
|
add_executable(wall_kalman_filter_test test/wall_kalman_filter_test.cpp)
|
||||||
target_link_libraries(wall_kalman_filter_test wall_kalman_filter)
|
target_link_libraries(wall_kalman_filter_test wall_kalman_filter)
|
||||||
add_test(NAME wall_kalman_filter_test COMMAND wall_kalman_filter_test)
|
add_test(NAME wall_kalman_filter_test COMMAND wall_kalman_filter_test)
|
||||||
|
|
||||||
|
add_executable(scan_odom_timing_logger_test test/scan_odom_timing_logger_test.cpp)
|
||||||
|
target_link_libraries(scan_odom_timing_logger_test origincar_base_log)
|
||||||
|
add_test(NAME scan_odom_timing_logger_test COMMAND scan_odom_timing_logger_test)
|
||||||
|
|
||||||
|
ament_add_gtest(command_gate_test test/command_gate_test.cpp)
|
||||||
|
target_include_directories(command_gate_test PRIVATE include)
|
||||||
|
|
||||||
|
ament_add_gtest(command_frame_test test/command_frame_test.cpp)
|
||||||
|
target_include_directories(command_frame_test PRIVATE include)
|
||||||
|
|
||||||
|
ament_add_gtest(serial_frame_parser_test test/serial_frame_parser_test.cpp)
|
||||||
|
target_include_directories(serial_frame_parser_test PRIVATE include)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
#add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)
|
#add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
### ekf config file ###
|
### ekf config file ###
|
||||||
ekf_filter_node:
|
ekf_filter_node:
|
||||||
ros__parameters:
|
ros__parameters:
|
||||||
frequency: 20.0
|
frequency: 50.0
|
||||||
sensor_timeout: 2.0
|
sensor_timeout: 2.0
|
||||||
two_d_mode: true
|
two_d_mode: true
|
||||||
transform_time_offset: 0.0
|
transform_time_offset: 0.0
|
||||||
|
|||||||
43
src/origincar_base/include/origincar_base/command_frame.hpp
Normal file
43
src/origincar_base/include/origincar_base/command_frame.hpp
Normal file
@@ -0,0 +1,43 @@
|
|||||||
|
#ifndef ORIGINCAR_BASE__COMMAND_FRAME_HPP_
|
||||||
|
#define ORIGINCAR_BASE__COMMAND_FRAME_HPP_
|
||||||
|
|
||||||
|
#include <array>
|
||||||
|
#include <cstddef>
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
#include "origincar_base/command_gate.hpp"
|
||||||
|
|
||||||
|
namespace origincar_base_core
|
||||||
|
{
|
||||||
|
|
||||||
|
inline void writeInt16(std::array<uint8_t, 11> * frame, std::size_t high_index, int16_t value)
|
||||||
|
{
|
||||||
|
const auto bits = static_cast<uint16_t>(value);
|
||||||
|
(*frame)[high_index] = static_cast<uint8_t>(bits >> 8);
|
||||||
|
(*frame)[high_index + 1] = static_cast<uint8_t>(bits & 0xFF);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline std::array<uint8_t, 11> encodeCommandFrame(const Command & command)
|
||||||
|
{
|
||||||
|
std::array<uint8_t, 11> frame{};
|
||||||
|
frame[0] = 0x7B;
|
||||||
|
|
||||||
|
writeInt16(&frame, 3, static_cast<int16_t>(command.linear_x * 1000.0));
|
||||||
|
if (command.ackermann) {
|
||||||
|
writeInt16(&frame, 5, 0);
|
||||||
|
writeInt16(&frame, 7, static_cast<int16_t>(command.steering_angle * 500.0));
|
||||||
|
} else {
|
||||||
|
writeInt16(&frame, 5, static_cast<int16_t>(command.linear_y * 1000.0));
|
||||||
|
writeInt16(&frame, 7, static_cast<int16_t>(command.angular_z * 1000.0));
|
||||||
|
}
|
||||||
|
|
||||||
|
for (std::size_t index = 0; index < 9; ++index) {
|
||||||
|
frame[9] ^= frame[index];
|
||||||
|
}
|
||||||
|
frame[10] = 0x7D;
|
||||||
|
return frame;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace origincar_base_core
|
||||||
|
|
||||||
|
#endif // ORIGINCAR_BASE__COMMAND_FRAME_HPP_
|
||||||
63
src/origincar_base/include/origincar_base/command_gate.hpp
Normal file
63
src/origincar_base/include/origincar_base/command_gate.hpp
Normal file
@@ -0,0 +1,63 @@
|
|||||||
|
#ifndef ORIGINCAR_BASE__COMMAND_GATE_HPP_
|
||||||
|
#define ORIGINCAR_BASE__COMMAND_GATE_HPP_
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <mutex>
|
||||||
|
|
||||||
|
namespace origincar_base_core
|
||||||
|
{
|
||||||
|
|
||||||
|
struct Command
|
||||||
|
{
|
||||||
|
double linear_x{0.0};
|
||||||
|
double linear_y{0.0};
|
||||||
|
double angular_z{0.0};
|
||||||
|
double steering_angle{0.0};
|
||||||
|
bool ackermann{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
inline bool operator==(const Command & lhs, const Command & rhs)
|
||||||
|
{
|
||||||
|
return lhs.linear_x == rhs.linear_x &&
|
||||||
|
lhs.linear_y == rhs.linear_y &&
|
||||||
|
lhs.angular_z == rhs.angular_z &&
|
||||||
|
lhs.steering_angle == rhs.steering_angle &&
|
||||||
|
lhs.ackermann == rhs.ackermann;
|
||||||
|
}
|
||||||
|
|
||||||
|
class CommandGate
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
explicit CommandGate(std::chrono::milliseconds timeout)
|
||||||
|
: timeout_(timeout)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void update(const Command & command, std::chrono::steady_clock::time_point now)
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
latest_command_ = command;
|
||||||
|
last_command_time_ = now;
|
||||||
|
has_command_ = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
Command commandAt(std::chrono::steady_clock::time_point now) const
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (!has_command_ || now - last_command_time_ >= timeout_) {
|
||||||
|
return {};
|
||||||
|
}
|
||||||
|
return latest_command_;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
const std::chrono::milliseconds timeout_;
|
||||||
|
mutable std::mutex mutex_;
|
||||||
|
Command latest_command_{};
|
||||||
|
std::chrono::steady_clock::time_point last_command_time_{};
|
||||||
|
bool has_command_{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace origincar_base_core
|
||||||
|
|
||||||
|
#endif // ORIGINCAR_BASE__COMMAND_GATE_HPP_
|
||||||
59
src/origincar_base/include/origincar_base/log.hpp
Normal file
59
src/origincar_base/include/origincar_base/log.hpp
Normal file
@@ -0,0 +1,59 @@
|
|||||||
|
#ifndef ORIGINCAR_BASE_LOG_HPP_
|
||||||
|
#define ORIGINCAR_BASE_LOG_HPP_
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <fstream>
|
||||||
|
#include <limits>
|
||||||
|
#include <string>
|
||||||
|
#include <unordered_map>
|
||||||
|
|
||||||
|
namespace origincar_base_logging
|
||||||
|
{
|
||||||
|
class ScanOdomTimingLogger
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
using Clock = std::chrono::steady_clock;
|
||||||
|
using TimePoint = Clock::time_point;
|
||||||
|
|
||||||
|
explicit ScanOdomTimingLogger(
|
||||||
|
const std::string &node_name,
|
||||||
|
const std::string &base_log_dir = "/home/sunrise/yiliao_ws/running_logs",
|
||||||
|
TimePoint start_time = Clock::now());
|
||||||
|
|
||||||
|
uint64_t startFrame(TimePoint now = Clock::now());
|
||||||
|
bool finishFrame(uint64_t frame_id, TimePoint now = Clock::now());
|
||||||
|
bool cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now = Clock::now());
|
||||||
|
bool makeSummaryLineIfDue(TimePoint now, std::string *line);
|
||||||
|
void setPrintToTerminal(bool enabled);
|
||||||
|
bool printToTerminal() const;
|
||||||
|
|
||||||
|
const std::string &logPath() const;
|
||||||
|
bool isOpen() const;
|
||||||
|
|
||||||
|
private:
|
||||||
|
struct Stats
|
||||||
|
{
|
||||||
|
uint64_t count{0};
|
||||||
|
double total_ms{0.0};
|
||||||
|
double min_ms{std::numeric_limits<double>::max()};
|
||||||
|
double max_ms{0.0};
|
||||||
|
double last_ms{0.0};
|
||||||
|
};
|
||||||
|
|
||||||
|
std::string makeLogPath(const std::string &node_name, const std::string &base_log_dir) const;
|
||||||
|
void writeFinishedFrame(uint64_t frame_id, double duration_ms);
|
||||||
|
void writeCanceledFrame(uint64_t frame_id, double elapsed_ms, const std::string &reason);
|
||||||
|
std::string formatStatsLine(const std::string &prefix) const;
|
||||||
|
|
||||||
|
std::unordered_map<uint64_t, TimePoint> active_frames_;
|
||||||
|
uint64_t next_frame_id_{1};
|
||||||
|
Stats stats_;
|
||||||
|
TimePoint last_summary_time_;
|
||||||
|
std::string log_path_;
|
||||||
|
std::ofstream log_file_;
|
||||||
|
bool print_to_terminal_{false};
|
||||||
|
};
|
||||||
|
} // namespace origincar_base_logging
|
||||||
|
|
||||||
|
#endif // ORIGINCAR_BASE_LOG_HPP_
|
||||||
@@ -1,13 +1,19 @@
|
|||||||
#ifndef _ORIGINCAR_BASE_H_
|
#ifndef _ORIGINCAR_BASE_H_
|
||||||
#define _ORIGINCAR_BASE_H_
|
#define _ORIGINCAR_BASE_H_
|
||||||
|
|
||||||
|
#include <atomic>
|
||||||
|
#include <chrono>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
#include <array>
|
#include <array>
|
||||||
|
#include <cstdint>
|
||||||
#include <mutex>
|
#include <mutex>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "std_msgs/msg/string.hpp"
|
#include "std_msgs/msg/string.hpp"
|
||||||
|
#include "origincar_base/log.hpp"
|
||||||
|
#include "origincar_base/command_gate.hpp"
|
||||||
|
#include "origincar_base/serial_frame_parser.hpp"
|
||||||
#include "origincar_base/wall_fit_core.hpp"
|
#include "origincar_base/wall_fit_core.hpp"
|
||||||
#include "origincar_base/wall_kalman_filter.hpp"
|
#include "origincar_base/wall_kalman_filter.hpp"
|
||||||
#include <csignal>
|
#include <csignal>
|
||||||
@@ -151,6 +157,11 @@ private:
|
|||||||
auto createQuaternionMsgFromYaw(double yaw);
|
auto createQuaternionMsgFromYaw(double yaw);
|
||||||
void Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
|
void Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
|
||||||
void Apply_Wall_Update();
|
void Apply_Wall_Update();
|
||||||
|
void Print_Timing_Log_If_Due();
|
||||||
|
void Sensor_Receive_Loop();
|
||||||
|
void Control_Timer_Callback();
|
||||||
|
void Tx_Timer_Callback();
|
||||||
|
void Send_Command(const origincar_base_core::Command & command);
|
||||||
|
|
||||||
bool Get_Sensor_Data();
|
bool Get_Sensor_Data();
|
||||||
unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode);
|
unsigned char Check_Sum(unsigned char Count_Number, unsigned char mode);
|
||||||
@@ -212,6 +223,7 @@ private:
|
|||||||
rclcpp::TimerBase::SharedPtr test_timer;
|
rclcpp::TimerBase::SharedPtr test_timer;
|
||||||
|
|
||||||
rclcpp::TimerBase::SharedPtr odom_timer;
|
rclcpp::TimerBase::SharedPtr odom_timer;
|
||||||
|
rclcpp::TimerBase::SharedPtr tx_timer;
|
||||||
rclcpp::TimerBase::SharedPtr imu_timer;
|
rclcpp::TimerBase::SharedPtr imu_timer;
|
||||||
rclcpp::TimerBase::SharedPtr voltage_timer;
|
rclcpp::TimerBase::SharedPtr voltage_timer;
|
||||||
|
|
||||||
@@ -225,10 +237,15 @@ private:
|
|||||||
string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
|
string usart_port_name, robot_frame_id, gyro_frame_id, odom_frame_id, akm_cmd_vel, test;
|
||||||
string scan_topic_, wall_config_path_, combined_odom_topic_;
|
string scan_topic_, wall_config_path_, combined_odom_topic_;
|
||||||
bool publish_tf_;
|
bool publish_tf_;
|
||||||
|
bool scan_odom_timing_to_terminal_;
|
||||||
double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_;
|
double odom_pose_cov_x_, odom_pose_cov_y_, odom_pose_cov_yaw_;
|
||||||
int wall_scan_stride_;
|
int wall_scan_stride_;
|
||||||
std::string cmd_vel;
|
std::string cmd_vel;
|
||||||
int serial_baud_rate;
|
int serial_baud_rate;
|
||||||
|
int cmd_watchdog_timeout_ms_;
|
||||||
|
int tx_period_ms_;
|
||||||
|
int serial_read_timeout_ms_;
|
||||||
|
int control_period_ms_;
|
||||||
RECEIVE_DATA Receive_Data;
|
RECEIVE_DATA Receive_Data;
|
||||||
SEND_DATA Send_Data;
|
SEND_DATA Send_Data;
|
||||||
|
|
||||||
@@ -252,10 +269,23 @@ private:
|
|||||||
::origincar_wall::WallFitConfig wall_fit_config_;
|
::origincar_wall::WallFitConfig wall_fit_config_;
|
||||||
std::unique_ptr<::origincar_wall::WallKalmanFilter> wall_filter_;
|
std::unique_ptr<::origincar_wall::WallKalmanFilter> wall_filter_;
|
||||||
::origincar_wall::Pose2D laser_pose_;
|
::origincar_wall::Pose2D laser_pose_;
|
||||||
|
::origincar_base_logging::ScanOdomTimingLogger scan_odom_timing_logger_;
|
||||||
std::mutex wall_scan_mutex_;
|
std::mutex wall_scan_mutex_;
|
||||||
std::vector<::origincar_wall::WallPoint> latest_scan_points_;
|
std::vector<::origincar_wall::WallPoint> latest_scan_points_;
|
||||||
|
uint64_t latest_scan_timing_frame_id_;
|
||||||
|
uint64_t pending_odom_timing_frame_id_;
|
||||||
bool has_latest_scan_;
|
bool has_latest_scan_;
|
||||||
bool latest_scan_consumed_;
|
bool latest_scan_consumed_;
|
||||||
|
bool has_latest_scan_timing_frame_;
|
||||||
|
bool has_pending_odom_timing_frame_;
|
||||||
|
std::unique_ptr<origincar_base_core::CommandGate> command_gate_;
|
||||||
|
origincar_base_core::SerialFrameParser serial_frame_parser_;
|
||||||
|
std::mutex serial_mutex_;
|
||||||
|
std::mutex sensor_data_mutex_;
|
||||||
|
std::thread sensor_thread_;
|
||||||
|
std::atomic<bool> sensor_thread_stop_{true};
|
||||||
|
std::atomic<uint64_t> sensor_sequence_{0};
|
||||||
|
uint64_t last_processed_sensor_sequence_{0};
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif //_ORIGINCAR_BASE_H_
|
#endif //_ORIGINCAR_BASE_H_
|
||||||
|
|||||||
@@ -0,0 +1,56 @@
|
|||||||
|
#ifndef ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_
|
||||||
|
#define ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <array>
|
||||||
|
#include <cstddef>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace origincar_base_core
|
||||||
|
{
|
||||||
|
|
||||||
|
class SerialFrameParser
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static constexpr std::size_t kFrameSize = 24;
|
||||||
|
|
||||||
|
static constexpr uint8_t frameHeader() {return 0x7B;}
|
||||||
|
static constexpr uint8_t frameTail() {return 0x7D;}
|
||||||
|
|
||||||
|
void append(const uint8_t * data, std::size_t size)
|
||||||
|
{
|
||||||
|
buffer_.insert(buffer_.end(), data, data + size);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool popFrame(std::array<uint8_t, kFrameSize> * frame)
|
||||||
|
{
|
||||||
|
while (!buffer_.empty()) {
|
||||||
|
const auto header = std::find(buffer_.begin(), buffer_.end(), frameHeader());
|
||||||
|
if (header == buffer_.end()) {
|
||||||
|
buffer_.clear();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
buffer_.erase(buffer_.begin(), header);
|
||||||
|
if (buffer_.size() < kFrameSize) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (buffer_[kFrameSize - 1] != frameTail()) {
|
||||||
|
buffer_.erase(buffer_.begin());
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::copy_n(buffer_.begin(), kFrameSize, frame->begin());
|
||||||
|
buffer_.erase(buffer_.begin(), buffer_.begin() + kFrameSize);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::vector<uint8_t> buffer_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace origincar_base_core
|
||||||
|
|
||||||
|
#endif // ORIGINCAR_BASE__SERIAL_FRAME_PARSER_HPP_
|
||||||
@@ -13,6 +13,23 @@ struct WallKalmanNoise
|
|||||||
double theta;
|
double theta;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
enum class WallScanTrust
|
||||||
|
{
|
||||||
|
Reject,
|
||||||
|
Weak,
|
||||||
|
Medium,
|
||||||
|
High,
|
||||||
|
};
|
||||||
|
|
||||||
|
struct WallScanUpdatePolicy
|
||||||
|
{
|
||||||
|
bool accept;
|
||||||
|
WallScanTrust trust;
|
||||||
|
WallKalmanNoise noise;
|
||||||
|
double innovation_distance;
|
||||||
|
std::string reason;
|
||||||
|
};
|
||||||
|
|
||||||
class WallKalmanFilter
|
class WallKalmanFilter
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
@@ -31,6 +48,11 @@ private:
|
|||||||
WallKalmanNoise process_noise_per_second_;
|
WallKalmanNoise process_noise_per_second_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose,
|
||||||
|
const Pose2D &scan_pose,
|
||||||
|
const LocalizationQuality &quality,
|
||||||
|
bool stationary);
|
||||||
|
|
||||||
} // namespace origincar_wall
|
} // namespace origincar_wall
|
||||||
|
|
||||||
#endif // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
|
#endif // ORIGINCAR_BASE_WALL_KALMAN_FILTER_HPP_
|
||||||
|
|||||||
Binary file not shown.
@@ -5,7 +5,11 @@ import launch_ros.actions
|
|||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
robot_parameters = [
|
robot_parameters = [
|
||||||
{'usart_port_name': '/dev/ttyACM0',
|
{'usart_port_name': '/dev/ttyACM0',
|
||||||
'serial_baud_rate': 115200,
|
'serial_baud_rate': 921600,
|
||||||
|
'serial_read_timeout_ms': 20,
|
||||||
|
'tx_period_ms': 20,
|
||||||
|
'cmd_watchdog_timeout_ms': 500,
|
||||||
|
'control_period_ms': 50,
|
||||||
'robot_frame_id': 'base_footprint',
|
'robot_frame_id': 'base_footprint',
|
||||||
'odom_frame_id': 'odom',
|
'odom_frame_id': 'odom',
|
||||||
'combined_odom_topic': 'odom_combined',
|
'combined_odom_topic': 'odom_combined',
|
||||||
|
|||||||
@@ -9,7 +9,7 @@ def generate_launch_description():
|
|||||||
|
|
||||||
robot_parameters = [
|
robot_parameters = [
|
||||||
{'usart_port_name': '/dev/ttyACM0',
|
{'usart_port_name': '/dev/ttyACM0',
|
||||||
'serial_baud_rate': 115200,
|
'serial_baud_rate': 921600,
|
||||||
'robot_frame_id': 'base_link',
|
'robot_frame_id': 'base_link',
|
||||||
'odom_frame_id': 'odom',
|
'odom_frame_id': 'odom',
|
||||||
'cmd_vel': 'cmd_vel',
|
'cmd_vel': 'cmd_vel',
|
||||||
|
|||||||
@@ -11,6 +11,7 @@
|
|||||||
|
|
||||||
<test_depend>ament_lint_auto</test_depend>
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
<test_depend>ament_lint_common</test_depend>
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
<test_depend>ament_cmake_gtest</test_depend>
|
||||||
<build_export_depend>tf2_geometry_msgs</build_export_depend>
|
<build_export_depend>tf2_geometry_msgs</build_export_depend>
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
242
src/origincar_base/src/log.cpp
Normal file
242
src/origincar_base/src/log.cpp
Normal file
@@ -0,0 +1,242 @@
|
|||||||
|
#include "origincar_base/log.hpp"
|
||||||
|
|
||||||
|
#include <cerrno>
|
||||||
|
#include <cctype>
|
||||||
|
#include <cmath>
|
||||||
|
#include <ctime>
|
||||||
|
#include <iomanip>
|
||||||
|
#include <sstream>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <sys/types.h>
|
||||||
|
|
||||||
|
namespace origincar_base_logging
|
||||||
|
{
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
std::string sanitizeNodeName(const std::string &node_name)
|
||||||
|
{
|
||||||
|
std::string result;
|
||||||
|
for (char ch : node_name)
|
||||||
|
{
|
||||||
|
const unsigned char value = static_cast<unsigned char>(ch);
|
||||||
|
if (std::isalnum(value) || ch == '_' || ch == '-')
|
||||||
|
{
|
||||||
|
result.push_back(ch);
|
||||||
|
}
|
||||||
|
else if (!result.empty() && result.back() != '_')
|
||||||
|
{
|
||||||
|
result.push_back('_');
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return result.empty() ? "node" : result;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool makeDirectory(const std::string &path)
|
||||||
|
{
|
||||||
|
if (path.empty())
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (::mkdir(path.c_str(), 0755) == 0 || errno == EEXIST)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool makeDirectories(const std::string &path)
|
||||||
|
{
|
||||||
|
if (path.empty())
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string current;
|
||||||
|
for (std::size_t index = 0; index < path.size(); ++index)
|
||||||
|
{
|
||||||
|
current.push_back(path[index]);
|
||||||
|
if ((path[index] == '/' && current.size() > 1) || index + 1 == path.size())
|
||||||
|
{
|
||||||
|
if (current.size() > 1 && current.back() == '/')
|
||||||
|
{
|
||||||
|
current.pop_back();
|
||||||
|
}
|
||||||
|
if (!current.empty() && !makeDirectory(current))
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (index + 1 < path.size() && path[index] == '/')
|
||||||
|
{
|
||||||
|
current.push_back('/');
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string formatSystemTime()
|
||||||
|
{
|
||||||
|
const std::time_t now = std::time(nullptr);
|
||||||
|
std::tm local_time;
|
||||||
|
localtime_r(&now, &local_time);
|
||||||
|
std::ostringstream stream;
|
||||||
|
stream << std::put_time(&local_time, "%Y-%m-%d %H:%M:%S");
|
||||||
|
return stream.str();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string formatFileTimestamp()
|
||||||
|
{
|
||||||
|
const std::time_t now = std::time(nullptr);
|
||||||
|
std::tm local_time;
|
||||||
|
localtime_r(&now, &local_time);
|
||||||
|
std::ostringstream stream;
|
||||||
|
stream << std::put_time(&local_time, "%Y%m%d_%H%M%S");
|
||||||
|
return stream.str();
|
||||||
|
}
|
||||||
|
|
||||||
|
double elapsedMs(ScanOdomTimingLogger::TimePoint start, ScanOdomTimingLogger::TimePoint end)
|
||||||
|
{
|
||||||
|
return std::chrono::duration_cast<std::chrono::duration<double, std::milli>>(end - start).count();
|
||||||
|
}
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
ScanOdomTimingLogger::ScanOdomTimingLogger(
|
||||||
|
const std::string &node_name,
|
||||||
|
const std::string &base_log_dir,
|
||||||
|
TimePoint start_time)
|
||||||
|
: last_summary_time_(start_time),
|
||||||
|
log_path_(makeLogPath(node_name, base_log_dir)),
|
||||||
|
log_file_(log_path_.c_str(), std::ios::out | std::ios::app)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
uint64_t ScanOdomTimingLogger::startFrame(TimePoint now)
|
||||||
|
{
|
||||||
|
const uint64_t frame_id = next_frame_id_++;
|
||||||
|
active_frames_[frame_id] = now;
|
||||||
|
return frame_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ScanOdomTimingLogger::finishFrame(uint64_t frame_id, TimePoint now)
|
||||||
|
{
|
||||||
|
const auto frame = active_frames_.find(frame_id);
|
||||||
|
if (frame == active_frames_.end())
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const double duration_ms = elapsedMs(frame->second, now);
|
||||||
|
active_frames_.erase(frame);
|
||||||
|
stats_.count += 1;
|
||||||
|
stats_.total_ms += duration_ms;
|
||||||
|
stats_.last_ms = duration_ms;
|
||||||
|
stats_.max_ms = std::max(stats_.max_ms, duration_ms);
|
||||||
|
stats_.min_ms = std::min(stats_.min_ms, duration_ms);
|
||||||
|
writeFinishedFrame(frame_id, duration_ms);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ScanOdomTimingLogger::cancelFrame(uint64_t frame_id, const std::string &reason, TimePoint now)
|
||||||
|
{
|
||||||
|
const auto frame = active_frames_.find(frame_id);
|
||||||
|
if (frame == active_frames_.end())
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
const double elapsed_ms = elapsedMs(frame->second, now);
|
||||||
|
active_frames_.erase(frame);
|
||||||
|
writeCanceledFrame(frame_id, elapsed_ms, reason);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ScanOdomTimingLogger::makeSummaryLineIfDue(TimePoint now, std::string *line)
|
||||||
|
{
|
||||||
|
if (now - last_summary_time_ < std::chrono::seconds(1))
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
last_summary_time_ = now;
|
||||||
|
if (line)
|
||||||
|
{
|
||||||
|
*line = formatStatsLine("scan_to_odom timing");
|
||||||
|
}
|
||||||
|
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_;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ScanOdomTimingLogger::isOpen() const
|
||||||
|
{
|
||||||
|
return log_file_.is_open();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string ScanOdomTimingLogger::makeLogPath(
|
||||||
|
const std::string &node_name,
|
||||||
|
const std::string &base_log_dir) const
|
||||||
|
{
|
||||||
|
const std::string node_dir = base_log_dir + "/" + sanitizeNodeName(node_name);
|
||||||
|
makeDirectories(node_dir);
|
||||||
|
return node_dir + "/" + formatFileTimestamp() + ".log";
|
||||||
|
}
|
||||||
|
|
||||||
|
void ScanOdomTimingLogger::writeFinishedFrame(uint64_t frame_id, double duration_ms)
|
||||||
|
{
|
||||||
|
if (!log_file_.is_open())
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
log_file_ << formatSystemTime()
|
||||||
|
<< " frame_id=" << frame_id
|
||||||
|
<< " status=finished"
|
||||||
|
<< std::fixed << std::setprecision(3)
|
||||||
|
<< " duration_ms=" << duration_ms
|
||||||
|
<< " " << formatStatsLine("stats")
|
||||||
|
<< '\n';
|
||||||
|
log_file_.flush();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ScanOdomTimingLogger::writeCanceledFrame(uint64_t frame_id, double elapsed_ms, const std::string &reason)
|
||||||
|
{
|
||||||
|
if (!log_file_.is_open())
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
log_file_ << formatSystemTime()
|
||||||
|
<< " frame_id=" << frame_id
|
||||||
|
<< " status=canceled"
|
||||||
|
<< " reason=" << reason
|
||||||
|
<< std::fixed << std::setprecision(3)
|
||||||
|
<< " elapsed_ms=" << elapsed_ms
|
||||||
|
<< '\n';
|
||||||
|
log_file_.flush();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string ScanOdomTimingLogger::formatStatsLine(const std::string &prefix) const
|
||||||
|
{
|
||||||
|
const double average_ms = stats_.count == 0 ? 0.0 : stats_.total_ms / static_cast<double>(stats_.count);
|
||||||
|
const double min_ms = stats_.count == 0 ? 0.0 : stats_.min_ms;
|
||||||
|
std::ostringstream stream;
|
||||||
|
stream << prefix
|
||||||
|
<< " frames=" << stats_.count
|
||||||
|
<< std::fixed << std::setprecision(3)
|
||||||
|
<< " last_ms=" << stats_.last_ms
|
||||||
|
<< " avg_ms=" << average_ms
|
||||||
|
<< " max_ms=" << stats_.max_ms
|
||||||
|
<< " min_ms=" << min_ms;
|
||||||
|
return stream.str();
|
||||||
|
}
|
||||||
|
} // namespace origincar_base_logging
|
||||||
@@ -1,10 +1,12 @@
|
|||||||
#include "origincar_base/origincar_base.h"
|
#include "origincar_base/origincar_base.h"
|
||||||
|
#include "origincar_base/command_frame.hpp"
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
|
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
|
||||||
#include "origincar_msg/msg/data.hpp"
|
#include "origincar_msg/msg/data.hpp"
|
||||||
#include "robot_localization/srv/set_pose.hpp"
|
#include "robot_localization/srv/set_pose.hpp"
|
||||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
|
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
|
||||||
using std::placeholders::_1;
|
using std::placeholders::_1;
|
||||||
@@ -93,78 +95,26 @@ float origincar_base::Odom_Trans(uint8_t Data_High, uint8_t Data_Low)
|
|||||||
|
|
||||||
void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
|
void origincar_base::Akm_Cmd_Vel_Callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr akm_ctl)
|
||||||
{
|
{
|
||||||
short transition;
|
origincar_base_core::Command command;
|
||||||
std::cout << "linerx" << akm_ctl->drive.speed << std::endl;
|
command.linear_x = akm_ctl->drive.speed;
|
||||||
std::cout << "angular" << akm_ctl->drive.steering_angle << std::endl;
|
command.steering_angle = akm_ctl->drive.steering_angle;
|
||||||
|
command.ackermann = true;
|
||||||
Send_Data.tx[0] = FRAME_HEADER;
|
command_gate_->update(command, std::chrono::steady_clock::now());
|
||||||
Send_Data.tx[1] = 0;
|
|
||||||
Send_Data.tx[2] = 0;
|
|
||||||
|
|
||||||
transition = 0;
|
|
||||||
transition = akm_ctl->drive.speed * 1000;
|
|
||||||
Send_Data.tx[4] = transition;
|
|
||||||
Send_Data.tx[3] = transition >> 8;
|
|
||||||
|
|
||||||
transition = 0;
|
|
||||||
transition = akm_ctl->drive.steering_angle * 1000 / 2;
|
|
||||||
Send_Data.tx[8] = transition;
|
|
||||||
Send_Data.tx[7] = transition >> 8;
|
|
||||||
|
|
||||||
Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
|
|
||||||
Send_Data.tx[10] = FRAME_TAIL;
|
|
||||||
|
|
||||||
try
|
|
||||||
{
|
|
||||||
Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
|
|
||||||
}
|
|
||||||
catch (serial::IOException &e)
|
|
||||||
{
|
|
||||||
RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port"));
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
|
void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
|
||||||
{
|
{
|
||||||
// RCLCPP_INFO(this->get_logger(), "linarx: %.2f, angularz: %.2f ", twist_aux->linear.x, twist_aux->angular.z);
|
origincar_base_core::Command command;
|
||||||
std::cout << "linerx" << twist_aux->linear.x << std::endl;
|
command.linear_x = twist_aux->linear.x;
|
||||||
std::cout << "angular" << twist_aux->angular.z << std::endl;
|
command.linear_y = twist_aux->linear.y;
|
||||||
short transition;
|
command.angular_z = twist_aux->angular.z;
|
||||||
Send_Data.tx[0] = FRAME_HEADER;
|
command_gate_->update(command, std::chrono::steady_clock::now());
|
||||||
Send_Data.tx[1] = 0;
|
|
||||||
Send_Data.tx[2] = 0;
|
|
||||||
|
|
||||||
transition = 0;
|
|
||||||
transition = twist_aux->linear.x * 1000;
|
|
||||||
Send_Data.tx[4] = transition;
|
|
||||||
Send_Data.tx[3] = transition >> 8;
|
|
||||||
|
|
||||||
transition = 0;
|
|
||||||
transition = twist_aux->linear.y * 1000;
|
|
||||||
Send_Data.tx[6] = transition;
|
|
||||||
Send_Data.tx[5] = transition >> 8;
|
|
||||||
|
|
||||||
transition = 0;
|
|
||||||
transition = (twist_aux->angular.z) * 1000;
|
|
||||||
Send_Data.tx[8] = transition;
|
|
||||||
Send_Data.tx[7] = transition >> 8;
|
|
||||||
|
|
||||||
Send_Data.tx[9] = Check_Sum(9, SEND_DATA_CHECK);
|
|
||||||
Send_Data.tx[10] = FRAME_TAIL;
|
|
||||||
|
|
||||||
try
|
|
||||||
{
|
|
||||||
Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
|
|
||||||
}
|
|
||||||
catch (serial::IOException &e)
|
|
||||||
{
|
|
||||||
RCLCPP_ERROR(this->get_logger(), ("Unable to send data through serial port"));
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch)
|
void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr sign_switch)
|
||||||
{
|
{
|
||||||
(void)sign_switch;
|
(void)sign_switch;
|
||||||
|
std::lock_guard<std::mutex> lock(sensor_data_mutex_);
|
||||||
if (sign_switch->data == -1)
|
if (sign_switch->data == -1)
|
||||||
{
|
{
|
||||||
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
|
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
|
||||||
@@ -201,6 +151,7 @@ void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr
|
|||||||
|
|
||||||
void origincar_base::Publish_ImuSensor()
|
void origincar_base::Publish_ImuSensor()
|
||||||
{
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(sensor_data_mutex_);
|
||||||
tf2::Quaternion q;
|
tf2::Quaternion q;
|
||||||
q.setRPY(0.0, 0.0, Robot_Pos.Z);
|
q.setRPY(0.0, 0.0, Robot_Pos.Z);
|
||||||
|
|
||||||
@@ -226,6 +177,7 @@ void origincar_base::Publish_ImuSensor()
|
|||||||
|
|
||||||
void origincar_base::Publish_Odom()
|
void origincar_base::Publish_Odom()
|
||||||
{
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(sensor_data_mutex_);
|
||||||
tf2::Quaternion q;
|
tf2::Quaternion q;
|
||||||
q.setRPY(0, 0, Robot_Pos.Z);
|
q.setRPY(0, 0, Robot_Pos.Z);
|
||||||
geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q);
|
geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(q);
|
||||||
@@ -278,18 +230,36 @@ void origincar_base::Publish_Odom()
|
|||||||
tf_broadcaster_->sendTransform(t);
|
tf_broadcaster_->sendTransform(t);
|
||||||
}
|
}
|
||||||
odom_publisher->publish(odom);
|
odom_publisher->publish(odom);
|
||||||
|
if (has_pending_odom_timing_frame_)
|
||||||
|
{
|
||||||
|
scan_odom_timing_logger_.finishFrame(pending_odom_timing_frame_id_);
|
||||||
|
has_pending_odom_timing_frame_ = false;
|
||||||
|
}
|
||||||
|
Print_Timing_Log_If_Due();
|
||||||
robotpose_publisher->publish(robotpose);
|
robotpose_publisher->publish(robotpose);
|
||||||
robotvel_publisher->publish(robotvel);
|
robotvel_publisher->publish(robotvel);
|
||||||
}
|
}
|
||||||
|
|
||||||
void origincar_base::Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
|
void origincar_base::Scan_Callback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
|
||||||
{
|
{
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(wall_scan_mutex_);
|
||||||
|
if (has_latest_scan_timing_frame_ && !latest_scan_consumed_)
|
||||||
|
{
|
||||||
|
scan_odom_timing_logger_.cancelFrame(latest_scan_timing_frame_id_, "replaced_by_new_scan");
|
||||||
|
has_latest_scan_timing_frame_ = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const uint64_t timing_frame_id = scan_odom_timing_logger_.startFrame();
|
||||||
const auto scan_points = scanMsgToPoints(*scan, wall_scan_stride_);
|
const auto scan_points = scanMsgToPoints(*scan, wall_scan_stride_);
|
||||||
const auto pose_frame_points = ::origincar_wall::transformScanPointsToPoseFrame(scan_points, laser_pose_);
|
const auto pose_frame_points = ::origincar_wall::transformScanPointsToPoseFrame(scan_points, laser_pose_);
|
||||||
std::lock_guard<std::mutex> lock(wall_scan_mutex_);
|
std::lock_guard<std::mutex> lock(wall_scan_mutex_);
|
||||||
latest_scan_points_ = pose_frame_points;
|
latest_scan_points_ = pose_frame_points;
|
||||||
|
latest_scan_timing_frame_id_ = timing_frame_id;
|
||||||
has_latest_scan_ = true;
|
has_latest_scan_ = true;
|
||||||
latest_scan_consumed_ = false;
|
latest_scan_consumed_ = false;
|
||||||
|
has_latest_scan_timing_frame_ = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void origincar_base::Apply_Wall_Update()
|
void origincar_base::Apply_Wall_Update()
|
||||||
@@ -299,6 +269,8 @@ void origincar_base::Apply_Wall_Update()
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
std::vector<::origincar_wall::WallPoint> scan_points;
|
std::vector<::origincar_wall::WallPoint> scan_points;
|
||||||
|
uint64_t timing_frame_id = 0;
|
||||||
|
bool has_timing_frame = false;
|
||||||
{
|
{
|
||||||
std::lock_guard<std::mutex> lock(wall_scan_mutex_);
|
std::lock_guard<std::mutex> lock(wall_scan_mutex_);
|
||||||
if (!has_latest_scan_ || latest_scan_consumed_)
|
if (!has_latest_scan_ || latest_scan_consumed_)
|
||||||
@@ -306,8 +278,16 @@ void origincar_base::Apply_Wall_Update()
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
scan_points = latest_scan_points_;
|
scan_points = latest_scan_points_;
|
||||||
|
timing_frame_id = latest_scan_timing_frame_id_;
|
||||||
|
has_timing_frame = has_latest_scan_timing_frame_;
|
||||||
|
has_latest_scan_timing_frame_ = false;
|
||||||
latest_scan_consumed_ = true;
|
latest_scan_consumed_ = true;
|
||||||
}
|
}
|
||||||
|
if (has_timing_frame)
|
||||||
|
{
|
||||||
|
pending_odom_timing_frame_id_ = timing_frame_id;
|
||||||
|
has_pending_odom_timing_frame_ = true;
|
||||||
|
}
|
||||||
|
|
||||||
const auto result = ::origincar_wall::localizeFromScan(scan_points, wall_fit_config_, wall_filter_->pose());
|
const auto result = ::origincar_wall::localizeFromScan(scan_points, wall_fit_config_, wall_filter_->pose());
|
||||||
if (!result.ok)
|
if (!result.ok)
|
||||||
@@ -325,8 +305,22 @@ void origincar_base::Apply_Wall_Update()
|
|||||||
wall_filter_->correct(result.pose, noise);
|
wall_filter_->correct(result.pose, noise);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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))
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "%s", timing_summary.c_str());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void origincar_base::Publish_Voltage()
|
void origincar_base::Publish_Voltage()
|
||||||
{
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(sensor_data_mutex_);
|
||||||
std_msgs::msg::Float32 voltage_msgs;
|
std_msgs::msg::Float32 voltage_msgs;
|
||||||
static float Count_Voltage_Pub = 0;
|
static float Count_Voltage_Pub = 0;
|
||||||
|
|
||||||
@@ -340,6 +334,7 @@ void origincar_base::Publish_Voltage()
|
|||||||
|
|
||||||
void origincar_base::Publish_GyroDebug()
|
void origincar_base::Publish_GyroDebug()
|
||||||
{
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(sensor_data_mutex_);
|
||||||
origincar_msg::msg::Data gyro_debug;
|
origincar_msg::msg::Data gyro_debug;
|
||||||
gyro_debug.x = gyro_z_filtered_pre_bias_;
|
gyro_debug.x = gyro_z_filtered_pre_bias_;
|
||||||
gyro_debug.y = gyro_z_bias_model_;
|
gyro_debug.y = gyro_z_bias_model_;
|
||||||
@@ -373,14 +368,37 @@ bool origincar_base::Get_Sensor_Data()
|
|||||||
{
|
{
|
||||||
short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
|
short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
|
||||||
uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
|
uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
|
||||||
try
|
std::array<uint8_t, RECEIVE_DATA_SIZE> frame{};
|
||||||
{
|
if (!serial_frame_parser_.popFrame(&frame)) {
|
||||||
Stm32_Serial.read(Receive_Data_Pr, sizeof(Receive_Data_Pr));
|
try
|
||||||
}
|
{
|
||||||
catch (const serial::SerialException &e)
|
std::string incoming;
|
||||||
{
|
{
|
||||||
return false;
|
std::lock_guard<std::mutex> serial_lock(serial_mutex_);
|
||||||
|
const auto available = Stm32_Serial.available();
|
||||||
|
if (available == 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
incoming = Stm32_Serial.read(std::min<std::size_t>(available, 256));
|
||||||
|
}
|
||||||
|
if (incoming.empty()) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
serial_frame_parser_.append(
|
||||||
|
reinterpret_cast<const uint8_t *>(incoming.data()), incoming.size());
|
||||||
|
}
|
||||||
|
catch (const std::exception &e)
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR_THROTTLE(
|
||||||
|
this->get_logger(), *this->get_clock(), 2000,
|
||||||
|
"Unable to read STM32 serial data: %s", e.what());
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (!serial_frame_parser_.popFrame(&frame)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
std::copy(frame.begin(), frame.end(), Receive_Data_Pr);
|
||||||
for (j = 0; j < 24; j++)
|
for (j = 0; j < 24; j++)
|
||||||
{
|
{
|
||||||
if (Receive_Data_Pr[j] == FRAME_HEADER)
|
if (Receive_Data_Pr[j] == FRAME_HEADER)
|
||||||
@@ -403,6 +421,7 @@ bool origincar_base::Get_Sensor_Data()
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
std::lock_guard<std::mutex> sensor_lock(sensor_data_mutex_);
|
||||||
Receive_Data.Frame_Header = Receive_Data.rx[0];
|
Receive_Data.Frame_Header = Receive_Data.rx[0];
|
||||||
Receive_Data.Frame_Tail = Receive_Data.rx[23];
|
Receive_Data.Frame_Tail = Receive_Data.rx[23];
|
||||||
if (Receive_Data.Frame_Header == FRAME_HEADER)
|
if (Receive_Data.Frame_Header == FRAME_HEADER)
|
||||||
@@ -491,6 +510,7 @@ bool origincar_base::Get_Sensor_Data()
|
|||||||
transition_16 |= Receive_Data.rx[21];
|
transition_16 |= Receive_Data.rx[21];
|
||||||
Power_voltage = transition_16 / 1000 + (transition_16 % 1000) * 0.001;
|
Power_voltage = transition_16 / 1000 + (transition_16 % 1000) * 0.001;
|
||||||
|
|
||||||
|
sensor_sequence_.fetch_add(1, std::memory_order_release);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -501,36 +521,91 @@ bool origincar_base::Get_Sensor_Data()
|
|||||||
|
|
||||||
void origincar_base::Control()
|
void origincar_base::Control()
|
||||||
{
|
{
|
||||||
rclcpp::Time current_time, last_time;
|
sensor_thread_stop_.store(false, std::memory_order_release);
|
||||||
current_time = rclcpp::Node::now();
|
sensor_thread_ = std::thread(&origincar_base::Sensor_Receive_Loop, this);
|
||||||
last_time = rclcpp::Node::now();
|
rclcpp::spin(this->get_node_base_interface());
|
||||||
while (rclcpp::ok())
|
sensor_thread_stop_.store(true, std::memory_order_release);
|
||||||
{
|
if (sensor_thread_.joinable()) {
|
||||||
current_time = rclcpp::Node::now();
|
sensor_thread_.join();
|
||||||
Sampling_Time = (current_time - last_time).seconds();
|
}
|
||||||
if (true == Get_Sensor_Data())
|
}
|
||||||
{
|
|
||||||
rclcpp::spin_some(this->get_node_base_interface());
|
void origincar_base::Sensor_Receive_Loop()
|
||||||
if (wall_filter_)
|
{
|
||||||
{
|
while (rclcpp::ok() && !sensor_thread_stop_.load(std::memory_order_acquire)) {
|
||||||
wall_filter_->predict(1.03 * Robot_Vel.X, 1.01 * Robot_Vel.Y, Robot_Vel.Z, Sampling_Time);
|
if (!Get_Sensor_Data()) {
|
||||||
Apply_Wall_Update();
|
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||||
const auto fused_pose = wall_filter_->pose();
|
|
||||||
Robot_Pos.X = static_cast<float>(fused_pose.x);
|
|
||||||
Robot_Pos.Y = static_cast<float>(fused_pose.y);
|
|
||||||
Robot_Pos.Z = static_cast<float>(fused_pose.theta);
|
|
||||||
}
|
|
||||||
Publish_ImuSensor();
|
|
||||||
Publish_GyroDebug();
|
|
||||||
Publish_Voltage();
|
|
||||||
Publish_Odom();
|
|
||||||
}
|
}
|
||||||
last_time = current_time;
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void origincar_base::Control_Timer_Callback()
|
||||||
|
{
|
||||||
|
const auto sequence = sensor_sequence_.load(std::memory_order_acquire);
|
||||||
|
if (sequence == 0 || sequence == last_processed_sensor_sequence_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const auto current_time = rclcpp::Node::now();
|
||||||
|
Sampling_Time = (current_time - _Last_Time).seconds();
|
||||||
|
if (Sampling_Time <= 0.0f || Sampling_Time > 0.5f) {
|
||||||
|
Sampling_Time = static_cast<float>(control_period_ms_) / 1000.0f;
|
||||||
|
}
|
||||||
|
_Last_Time = current_time;
|
||||||
|
|
||||||
|
Vel_Pos_Data velocity;
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(sensor_data_mutex_);
|
||||||
|
velocity = Robot_Vel;
|
||||||
|
}
|
||||||
|
if (wall_filter_)
|
||||||
|
{
|
||||||
|
wall_filter_->predict(
|
||||||
|
1.03 * velocity.X, 1.01 * velocity.Y, velocity.Z, Sampling_Time);
|
||||||
|
Apply_Wall_Update();
|
||||||
|
const auto fused_pose = wall_filter_->pose();
|
||||||
|
std::lock_guard<std::mutex> lock(sensor_data_mutex_);
|
||||||
|
Robot_Pos.X = static_cast<float>(fused_pose.x);
|
||||||
|
Robot_Pos.Y = static_cast<float>(fused_pose.y);
|
||||||
|
Robot_Pos.Z = static_cast<float>(fused_pose.theta);
|
||||||
|
}
|
||||||
|
|
||||||
|
Publish_ImuSensor();
|
||||||
|
Publish_GyroDebug();
|
||||||
|
Publish_Voltage();
|
||||||
|
Publish_Odom();
|
||||||
|
last_processed_sensor_sequence_ = sequence;
|
||||||
|
}
|
||||||
|
|
||||||
|
void origincar_base::Tx_Timer_Callback()
|
||||||
|
{
|
||||||
|
if (!command_gate_) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
Send_Command(command_gate_->commandAt(std::chrono::steady_clock::now()));
|
||||||
|
}
|
||||||
|
|
||||||
|
void origincar_base::Send_Command(const origincar_base_core::Command & command)
|
||||||
|
{
|
||||||
|
const auto frame = origincar_base_core::encodeCommandFrame(command);
|
||||||
|
try
|
||||||
|
{
|
||||||
|
std::lock_guard<std::mutex> lock(serial_mutex_);
|
||||||
|
if (Stm32_Serial.isOpen()) {
|
||||||
|
Stm32_Serial.write(frame.data(), frame.size());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
catch (const std::exception &e)
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR_THROTTLE(
|
||||||
|
this->get_logger(), *this->get_clock(), 2000,
|
||||||
|
"Unable to send STM32 command: %s", e.what());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
origincar_base::origincar_base()
|
origincar_base::origincar_base()
|
||||||
: rclcpp::Node("origincar_base")
|
: rclcpp::Node("origincar_base"),
|
||||||
|
scan_odom_timing_logger_(this->get_name())
|
||||||
{
|
{
|
||||||
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
|
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
|
||||||
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
|
memset(&Robot_Vel, 0, sizeof(Robot_Vel));
|
||||||
@@ -550,11 +625,15 @@ origincar_base::origincar_base()
|
|||||||
gyro_z_low_pass_initialized_ = false;
|
gyro_z_low_pass_initialized_ = false;
|
||||||
has_latest_scan_ = false;
|
has_latest_scan_ = false;
|
||||||
latest_scan_consumed_ = true;
|
latest_scan_consumed_ = true;
|
||||||
|
latest_scan_timing_frame_id_ = 0;
|
||||||
|
pending_odom_timing_frame_id_ = 0;
|
||||||
|
has_latest_scan_timing_frame_ = false;
|
||||||
|
has_pending_odom_timing_frame_ = false;
|
||||||
|
|
||||||
int serial_baud_rate = 115200;
|
int serial_baud_rate = 921600;
|
||||||
|
|
||||||
this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
|
this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
|
||||||
this->declare_parameter<int>("serial_baud_rate", 115200);
|
this->declare_parameter<int>("serial_baud_rate", 921600);
|
||||||
this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
|
this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
|
||||||
this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
|
this->declare_parameter<std::string>("akm_cmd_vel", "ackermann_cmd");
|
||||||
this->declare_parameter<std::string>("odom_frame_id", "odom");
|
this->declare_parameter<std::string>("odom_frame_id", "odom");
|
||||||
@@ -564,11 +643,16 @@ origincar_base::origincar_base()
|
|||||||
this->declare_parameter<std::string>("scan_topic", "/scan");
|
this->declare_parameter<std::string>("scan_topic", "/scan");
|
||||||
this->declare_parameter<std::string>("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
|
this->declare_parameter<std::string>("wall_config_path", "/home/sunrise/yiliao_ws/src/origincar_base/config/wall_fit.json");
|
||||||
this->declare_parameter<bool>("publish_tf", true);
|
this->declare_parameter<bool>("publish_tf", true);
|
||||||
|
this->declare_parameter<bool>("scan_odom_timing_to_terminal", false);
|
||||||
this->declare_parameter<int>("wall_scan_stride", 2);
|
this->declare_parameter<int>("wall_scan_stride", 2);
|
||||||
this->declare_parameter<double>("laser_x", 0.0);
|
this->declare_parameter<double>("laser_x", 0.0);
|
||||||
this->declare_parameter<double>("laser_y", 0.0);
|
this->declare_parameter<double>("laser_y", 0.0);
|
||||||
this->declare_parameter<double>("laser_yaw", 0.0);
|
this->declare_parameter<double>("laser_yaw", 0.0);
|
||||||
this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);
|
this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);
|
||||||
|
this->declare_parameter<int>("cmd_watchdog_timeout_ms", 150);
|
||||||
|
this->declare_parameter<int>("tx_period_ms", 20);
|
||||||
|
this->declare_parameter<int>("serial_read_timeout_ms", 20);
|
||||||
|
this->declare_parameter<int>("control_period_ms", 50);
|
||||||
|
|
||||||
// Odom covariance parameters (tunable via YAML)
|
// Odom covariance parameters (tunable via YAML)
|
||||||
this->declare_parameter<double>("odom_pose_cov_x", 0.01);
|
this->declare_parameter<double>("odom_pose_cov_x", 0.01);
|
||||||
@@ -586,6 +670,7 @@ origincar_base::origincar_base()
|
|||||||
this->get_parameter("scan_topic", scan_topic_);
|
this->get_parameter("scan_topic", scan_topic_);
|
||||||
this->get_parameter("wall_config_path", wall_config_path_);
|
this->get_parameter("wall_config_path", wall_config_path_);
|
||||||
this->get_parameter("publish_tf", publish_tf_);
|
this->get_parameter("publish_tf", publish_tf_);
|
||||||
|
this->get_parameter("scan_odom_timing_to_terminal", scan_odom_timing_to_terminal_);
|
||||||
this->get_parameter("wall_scan_stride", wall_scan_stride_);
|
this->get_parameter("wall_scan_stride", wall_scan_stride_);
|
||||||
laser_pose_ = {
|
laser_pose_ = {
|
||||||
this->get_parameter("laser_x").as_double(),
|
this->get_parameter("laser_x").as_double(),
|
||||||
@@ -605,6 +690,16 @@ origincar_base::origincar_base()
|
|||||||
this->get_parameter("odom_pose_cov_x", odom_pose_cov_x_);
|
this->get_parameter("odom_pose_cov_x", odom_pose_cov_x_);
|
||||||
this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_);
|
this->get_parameter("odom_pose_cov_y", odom_pose_cov_y_);
|
||||||
this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_);
|
this->get_parameter("odom_pose_cov_yaw", odom_pose_cov_yaw_);
|
||||||
|
this->get_parameter("cmd_watchdog_timeout_ms", cmd_watchdog_timeout_ms_);
|
||||||
|
this->get_parameter("tx_period_ms", tx_period_ms_);
|
||||||
|
this->get_parameter("serial_read_timeout_ms", serial_read_timeout_ms_);
|
||||||
|
this->get_parameter("control_period_ms", control_period_ms_);
|
||||||
|
cmd_watchdog_timeout_ms_ = std::max(1, cmd_watchdog_timeout_ms_);
|
||||||
|
tx_period_ms_ = std::max(1, tx_period_ms_);
|
||||||
|
serial_read_timeout_ms_ = std::max(1, serial_read_timeout_ms_);
|
||||||
|
control_period_ms_ = std::max(1, control_period_ms_);
|
||||||
|
command_gate_ = std::make_unique<origincar_base_core::CommandGate>(
|
||||||
|
std::chrono::milliseconds(cmd_watchdog_timeout_ms_));
|
||||||
|
|
||||||
wall_fit_config_ = ::origincar_wall::loadWallFitConfig(wall_config_path_);
|
wall_fit_config_ = ::origincar_wall::loadWallFitConfig(wall_config_path_);
|
||||||
const auto initial_pose = ::origincar_wall::Pose2D{
|
const auto initial_pose = ::origincar_wall::Pose2D{
|
||||||
@@ -613,6 +708,7 @@ origincar_base::origincar_base()
|
|||||||
0.0,
|
0.0,
|
||||||
};
|
};
|
||||||
wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose);
|
wall_filter_ = std::make_unique<::origincar_wall::WallKalmanFilter>(initial_pose);
|
||||||
|
scan_odom_timing_logger_.setPrintToTerminal(scan_odom_timing_to_terminal_);
|
||||||
Robot_Pos.X = static_cast<float>(initial_pose.x);
|
Robot_Pos.X = static_cast<float>(initial_pose.x);
|
||||||
Robot_Pos.Y = static_cast<float>(initial_pose.y);
|
Robot_Pos.Y = static_cast<float>(initial_pose.y);
|
||||||
Robot_Pos.Z = static_cast<float>(initial_pose.theta);
|
Robot_Pos.Z = static_cast<float>(initial_pose.theta);
|
||||||
@@ -648,7 +744,7 @@ origincar_base::origincar_base()
|
|||||||
{
|
{
|
||||||
Stm32_Serial.setPort(usart_port_name);
|
Stm32_Serial.setPort(usart_port_name);
|
||||||
Stm32_Serial.setBaudrate(serial_baud_rate);
|
Stm32_Serial.setBaudrate(serial_baud_rate);
|
||||||
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
|
serial::Timeout _time = serial::Timeout::simpleTimeout(serial_read_timeout_ms_);
|
||||||
Stm32_Serial.setTimeout(_time);
|
Stm32_Serial.setTimeout(_time);
|
||||||
Stm32_Serial.open();
|
Stm32_Serial.open();
|
||||||
}
|
}
|
||||||
@@ -660,55 +756,35 @@ origincar_base::origincar_base()
|
|||||||
{
|
{
|
||||||
RCLCPP_INFO(this->get_logger(), "origincar_base serial port opened");
|
RCLCPP_INFO(this->get_logger(), "origincar_base serial port opened");
|
||||||
}
|
}
|
||||||
|
if (scan_odom_timing_logger_.isOpen())
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(this->get_logger(), "scan_to_odom timing log: %s", scan_odom_timing_logger_.logPath().c_str());
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(this->get_logger(), "scan_to_odom timing log file could not be opened: %s", scan_odom_timing_logger_.logPath().c_str());
|
||||||
|
}
|
||||||
|
_Last_Time = rclcpp::Node::now();
|
||||||
|
tx_timer = create_wall_timer(
|
||||||
|
std::chrono::milliseconds(tx_period_ms_),
|
||||||
|
std::bind(&origincar_base::Tx_Timer_Callback, this));
|
||||||
|
odom_timer = create_wall_timer(
|
||||||
|
std::chrono::milliseconds(control_period_ms_),
|
||||||
|
std::bind(&origincar_base::Control_Timer_Callback, this));
|
||||||
}
|
}
|
||||||
|
|
||||||
void sigintHandler(int sig)
|
void sigintHandler(int sig)
|
||||||
{
|
{
|
||||||
sig = sig;
|
(void)sig;
|
||||||
printf("OriginBot shutdown...\n");
|
|
||||||
serial::Serial Stm32_Serial;
|
|
||||||
Stm32_Serial.setPort("/dev/ttyACM0");
|
|
||||||
Stm32_Serial.setBaudrate(115200);
|
|
||||||
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
|
|
||||||
Stm32_Serial.setTimeout(_time);
|
|
||||||
Stm32_Serial.open();
|
|
||||||
SEND_DATA Send_Data;
|
|
||||||
if (Stm32_Serial.isOpen())
|
|
||||||
{
|
|
||||||
Send_Data.tx[0] = FRAME_HEADER;
|
|
||||||
Send_Data.tx[1] = 0;
|
|
||||||
Send_Data.tx[2] = 0;
|
|
||||||
|
|
||||||
Send_Data.tx[4] = 0;
|
|
||||||
Send_Data.tx[3] = 0;
|
|
||||||
|
|
||||||
Send_Data.tx[6] = 0;
|
|
||||||
Send_Data.tx[5] = 0;
|
|
||||||
|
|
||||||
Send_Data.tx[7] = 0;
|
|
||||||
Send_Data.tx[8] = 0;
|
|
||||||
int check_sum = 0;
|
|
||||||
for (int k = 0; k < 9; k++)
|
|
||||||
{
|
|
||||||
check_sum = check_sum ^ Send_Data.tx[k];
|
|
||||||
}
|
|
||||||
Send_Data.tx[9] = check_sum;
|
|
||||||
Send_Data.tx[10] = FRAME_TAIL;
|
|
||||||
|
|
||||||
try
|
|
||||||
{
|
|
||||||
Stm32_Serial.write(Send_Data.tx, sizeof(Send_Data.tx));
|
|
||||||
}
|
|
||||||
catch (serial::IOException &e)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// Shutdown ROS2 and release resources.
|
|
||||||
rclcpp::shutdown();
|
rclcpp::shutdown();
|
||||||
}
|
}
|
||||||
|
|
||||||
origincar_base::~origincar_base()
|
origincar_base::~origincar_base()
|
||||||
{
|
{
|
||||||
|
sensor_thread_stop_.store(true, std::memory_order_release);
|
||||||
|
if (sensor_thread_.joinable()) {
|
||||||
|
sensor_thread_.join();
|
||||||
|
}
|
||||||
|
Send_Command(origincar_base_core::Command{});
|
||||||
RCLCPP_INFO(this->get_logger(), "Shutting down");
|
RCLCPP_INFO(this->get_logger(), "Shutting down");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -373,7 +373,7 @@ origincar_base::origincar_base()
|
|||||||
Robot_Pos.X = 0.54;
|
Robot_Pos.X = 0.54;
|
||||||
Robot_Pos.Y = 0.2;
|
Robot_Pos.Y = 0.2;
|
||||||
|
|
||||||
int serial_baud_rate = 115200;
|
int serial_baud_rate = 921600;
|
||||||
|
|
||||||
this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
|
this->declare_parameter<std::string>("usart_port_name", "/dev/ttyCH343USB0");
|
||||||
this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
|
this->declare_parameter<std::string>("cmd_vel", "cmd_vel");
|
||||||
@@ -436,7 +436,7 @@ void sigintHandler(int sig)
|
|||||||
printf("OriginBot shutdown...\n");
|
printf("OriginBot shutdown...\n");
|
||||||
serial::Serial Stm32_Serial;
|
serial::Serial Stm32_Serial;
|
||||||
Stm32_Serial.setPort("/dev/ttyACM0");
|
Stm32_Serial.setPort("/dev/ttyACM0");
|
||||||
Stm32_Serial.setBaudrate(115200);
|
Stm32_Serial.setBaudrate(921600);
|
||||||
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
|
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
|
||||||
Stm32_Serial.setTimeout(_time);
|
Stm32_Serial.setTimeout(_time);
|
||||||
Stm32_Serial.open();
|
Stm32_Serial.open();
|
||||||
|
|||||||
@@ -6,6 +6,24 @@
|
|||||||
namespace origincar_wall
|
namespace origincar_wall
|
||||||
{
|
{
|
||||||
|
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
WallKalmanNoise highTrustNoise()
|
||||||
|
{
|
||||||
|
return {0.015, 0.015, 0.025};
|
||||||
|
}
|
||||||
|
|
||||||
|
WallKalmanNoise mediumTrustNoise()
|
||||||
|
{
|
||||||
|
return {0.045, 0.045, 0.060};
|
||||||
|
}
|
||||||
|
|
||||||
|
WallKalmanNoise weakTrustNoise()
|
||||||
|
{
|
||||||
|
return {0.10, 0.10, 0.12};
|
||||||
|
}
|
||||||
|
} // namespace
|
||||||
|
|
||||||
WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose)
|
WallKalmanFilter::WallKalmanFilter(const Pose2D &initial_pose)
|
||||||
: pose_(initial_pose),
|
: pose_(initial_pose),
|
||||||
covariance_{0.10, 0.10, 0.10},
|
covariance_{0.10, 0.10, 0.10},
|
||||||
@@ -63,4 +81,75 @@ WallKalmanNoise WallKalmanFilter::covariance() const
|
|||||||
return covariance_;
|
return covariance_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
WallScanUpdatePolicy chooseWallScanUpdatePolicy(const Pose2D &filter_pose,
|
||||||
|
const Pose2D &scan_pose,
|
||||||
|
const LocalizationQuality &quality,
|
||||||
|
bool stationary)
|
||||||
|
{
|
||||||
|
WallScanUpdatePolicy policy{false, WallScanTrust::Reject, weakTrustNoise(), 0.0, ""};
|
||||||
|
policy.innovation_distance = std::hypot(scan_pose.x - filter_pose.x, scan_pose.y - filter_pose.y);
|
||||||
|
|
||||||
|
if (!quality.has_mean_error)
|
||||||
|
{
|
||||||
|
policy.reason = "missing mean error";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
if (quality.wall_count < 3)
|
||||||
|
{
|
||||||
|
policy.reason = "too few walls";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
if (quality.mean_error > 0.15)
|
||||||
|
{
|
||||||
|
policy.reason = "mean error too high";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (stationary && policy.innovation_distance > 0.04)
|
||||||
|
{
|
||||||
|
policy.reason = "stationary innovation too large";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
if (!stationary && policy.innovation_distance > 0.15)
|
||||||
|
{
|
||||||
|
policy.reason = "motion innovation too large";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (quality.wall_count == 4 && quality.mean_error < 0.05)
|
||||||
|
{
|
||||||
|
policy.accept = true;
|
||||||
|
policy.trust = stationary ? WallScanTrust::Medium : WallScanTrust::High;
|
||||||
|
policy.noise = stationary ? mediumTrustNoise() : highTrustNoise();
|
||||||
|
policy.reason = stationary ? "stationary high trust downgraded to medium" : "high trust";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (quality.wall_count >= 3 && quality.mean_error < 0.10)
|
||||||
|
{
|
||||||
|
policy.accept = true;
|
||||||
|
policy.trust = stationary ? WallScanTrust::Weak : WallScanTrust::Medium;
|
||||||
|
policy.noise = stationary ? weakTrustNoise() : mediumTrustNoise();
|
||||||
|
policy.reason = stationary ? "stationary medium trust downgraded to weak" : "medium trust";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (quality.wall_count >= 3 && quality.mean_error <= 0.15)
|
||||||
|
{
|
||||||
|
if (stationary)
|
||||||
|
{
|
||||||
|
policy.reason = "stationary weak trust rejected";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
policy.accept = true;
|
||||||
|
policy.trust = WallScanTrust::Weak;
|
||||||
|
policy.noise = weakTrustNoise();
|
||||||
|
policy.reason = "weak trust";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
|
||||||
|
policy.reason = "rejected by policy";
|
||||||
|
return policy;
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace origincar_wall
|
} // namespace origincar_wall
|
||||||
|
|||||||
64
src/origincar_base/test/command_frame_test.cpp
Normal file
64
src/origincar_base/test/command_frame_test.cpp
Normal file
@@ -0,0 +1,64 @@
|
|||||||
|
#include <array>
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include "origincar_base/command_frame.hpp"
|
||||||
|
|
||||||
|
using origincar_base_core::Command;
|
||||||
|
using origincar_base_core::encodeCommandFrame;
|
||||||
|
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
uint8_t checksum(const std::array<uint8_t, 11> & frame)
|
||||||
|
{
|
||||||
|
uint8_t value = 0;
|
||||||
|
for (std::size_t index = 0; index < 9; ++index) {
|
||||||
|
value ^= frame[index];
|
||||||
|
}
|
||||||
|
return value;
|
||||||
|
}
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
TEST(CommandFrameTest, EncodesTwistCommandUsingExistingProtocol)
|
||||||
|
{
|
||||||
|
const auto frame = encodeCommandFrame(Command{0.25, -0.1, 0.4});
|
||||||
|
|
||||||
|
EXPECT_EQ(frame[0], 0x7B);
|
||||||
|
EXPECT_EQ(frame[3], 0x00);
|
||||||
|
EXPECT_EQ(frame[4], 0xFA);
|
||||||
|
EXPECT_EQ(frame[5], 0xFF);
|
||||||
|
EXPECT_EQ(frame[6], 0x9C);
|
||||||
|
EXPECT_EQ(frame[7], 0x01);
|
||||||
|
EXPECT_EQ(frame[8], 0x90);
|
||||||
|
EXPECT_EQ(frame[9], checksum(frame));
|
||||||
|
EXPECT_EQ(frame[10], 0x7D);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(CommandFrameTest, EncodesAckermannSteeringWithFirmwareScale)
|
||||||
|
{
|
||||||
|
Command command;
|
||||||
|
command.linear_x = 0.25;
|
||||||
|
command.steering_angle = 0.4;
|
||||||
|
command.ackermann = true;
|
||||||
|
|
||||||
|
const auto frame = encodeCommandFrame(command);
|
||||||
|
|
||||||
|
EXPECT_EQ(frame[3], 0x00);
|
||||||
|
EXPECT_EQ(frame[4], 0xFA);
|
||||||
|
EXPECT_EQ(frame[5], 0x00);
|
||||||
|
EXPECT_EQ(frame[6], 0x00);
|
||||||
|
EXPECT_EQ(frame[7], 0x00);
|
||||||
|
EXPECT_EQ(frame[8], 0xC8);
|
||||||
|
EXPECT_EQ(frame[9], checksum(frame));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(CommandFrameTest, ZeroCommandEncodesZeroPayload)
|
||||||
|
{
|
||||||
|
const auto frame = encodeCommandFrame(Command{});
|
||||||
|
|
||||||
|
for (std::size_t index = 1; index <= 8; ++index) {
|
||||||
|
EXPECT_EQ(frame[index], 0x00);
|
||||||
|
}
|
||||||
|
EXPECT_EQ(frame[9], checksum(frame));
|
||||||
|
}
|
||||||
42
src/origincar_base/test/command_gate_test.cpp
Normal file
42
src/origincar_base/test/command_gate_test.cpp
Normal file
@@ -0,0 +1,42 @@
|
|||||||
|
#include <chrono>
|
||||||
|
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include "origincar_base/command_gate.hpp"
|
||||||
|
|
||||||
|
using origincar_base_core::Command;
|
||||||
|
using origincar_base_core::CommandGate;
|
||||||
|
|
||||||
|
TEST(CommandGateTest, ReturnsZeroBeforeFirstCommand)
|
||||||
|
{
|
||||||
|
const auto t0 = std::chrono::steady_clock::time_point{};
|
||||||
|
CommandGate gate(std::chrono::milliseconds(150));
|
||||||
|
|
||||||
|
EXPECT_EQ(gate.commandAt(t0), (Command{}));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(CommandGateTest, KeepsLatestCommandUntilWatchdogExpires)
|
||||||
|
{
|
||||||
|
const auto t0 = std::chrono::steady_clock::time_point{};
|
||||||
|
CommandGate gate(std::chrono::milliseconds(150));
|
||||||
|
const Command command{0.25, 0.0, 0.4};
|
||||||
|
|
||||||
|
gate.update(command, t0);
|
||||||
|
|
||||||
|
EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(149)), command);
|
||||||
|
EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(150)), (Command{}));
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(CommandGateTest, NewCommandRefreshesWatchdog)
|
||||||
|
{
|
||||||
|
const auto t0 = std::chrono::steady_clock::time_point{};
|
||||||
|
CommandGate gate(std::chrono::milliseconds(150));
|
||||||
|
const Command first{0.1, 0.0, 0.1};
|
||||||
|
const Command second{0.3, 0.0, -0.2};
|
||||||
|
|
||||||
|
gate.update(first, t0);
|
||||||
|
gate.update(second, t0 + std::chrono::milliseconds(100));
|
||||||
|
|
||||||
|
EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(249)), second);
|
||||||
|
EXPECT_EQ(gate.commandAt(t0 + std::chrono::milliseconds(250)), (Command{}));
|
||||||
|
}
|
||||||
113
src/origincar_base/test/scan_odom_timing_logger_test.cpp
Normal file
113
src/origincar_base/test/scan_odom_timing_logger_test.cpp
Normal file
@@ -0,0 +1,113 @@
|
|||||||
|
#include "origincar_base/log.hpp"
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <fstream>
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <string>
|
||||||
|
#include <unistd.h>
|
||||||
|
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
using Clock = std::chrono::steady_clock;
|
||||||
|
|
||||||
|
void require(bool condition, const std::string &message)
|
||||||
|
{
|
||||||
|
if (!condition)
|
||||||
|
{
|
||||||
|
throw std::runtime_error(message);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void requireContains(const std::string &text, const std::string &expected, const std::string &message)
|
||||||
|
{
|
||||||
|
if (text.find(expected) == std::string::npos)
|
||||||
|
{
|
||||||
|
throw std::runtime_error(message + " missing=" + expected + " text=" + text);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string readFile(const std::string &path)
|
||||||
|
{
|
||||||
|
std::ifstream input(path.c_str());
|
||||||
|
return std::string((std::istreambuf_iterator<char>(input)), std::istreambuf_iterator<char>());
|
||||||
|
}
|
||||||
|
|
||||||
|
std::string makeTempDir()
|
||||||
|
{
|
||||||
|
const std::string path = "/tmp/origincar_timing_logger_test_" + std::to_string(getpid());
|
||||||
|
std::string command = "rm -rf " + path + " && mkdir -p " + path;
|
||||||
|
require(std::system(command.c_str()) == 0, "create temp log dir");
|
||||||
|
return path;
|
||||||
|
}
|
||||||
|
|
||||||
|
void testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate()
|
||||||
|
{
|
||||||
|
const auto root = makeTempDir();
|
||||||
|
const auto start = Clock::time_point(std::chrono::seconds(0));
|
||||||
|
origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root, start);
|
||||||
|
|
||||||
|
const auto first = logger.startFrame(start);
|
||||||
|
require(logger.finishFrame(first, start + std::chrono::milliseconds(15)), "finish first frame");
|
||||||
|
std::string contents = readFile(logger.logPath());
|
||||||
|
requireContains(contents, "frame_id=1", "first frame id");
|
||||||
|
requireContains(contents, "duration_ms=15.000", "first frame duration");
|
||||||
|
requireContains(contents, "avg_ms=15.000", "first frame average");
|
||||||
|
requireContains(contents, "max_ms=15.000", "first frame max");
|
||||||
|
requireContains(contents, "min_ms=15.000", "first frame min");
|
||||||
|
|
||||||
|
const auto second = logger.startFrame(start + std::chrono::milliseconds(20));
|
||||||
|
require(logger.finishFrame(second, start + std::chrono::milliseconds(50)), "finish second frame");
|
||||||
|
contents = readFile(logger.logPath());
|
||||||
|
requireContains(contents, "frame_id=2", "second frame id");
|
||||||
|
requireContains(contents, "duration_ms=30.000", "second frame duration");
|
||||||
|
requireContains(contents, "avg_ms=22.500", "second frame average");
|
||||||
|
requireContains(contents, "max_ms=30.000", "second frame max");
|
||||||
|
requireContains(contents, "min_ms=15.000", "second frame min");
|
||||||
|
}
|
||||||
|
|
||||||
|
void testSummaryPrintIsThrottledToOneHz()
|
||||||
|
{
|
||||||
|
const auto root = makeTempDir();
|
||||||
|
const auto start = Clock::time_point(std::chrono::seconds(0));
|
||||||
|
origincar_base_logging::ScanOdomTimingLogger logger("origincar_base", root, start);
|
||||||
|
std::string summary;
|
||||||
|
|
||||||
|
const auto frame = logger.startFrame(start);
|
||||||
|
require(logger.finishFrame(frame, start + std::chrono::milliseconds(10)), "finish frame");
|
||||||
|
require(!logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(999), &summary),
|
||||||
|
"summary should not be due before one second");
|
||||||
|
require(logger.makeSummaryLineIfDue(start + std::chrono::milliseconds(1000), &summary),
|
||||||
|
"summary should be due at one second");
|
||||||
|
requireContains(summary, "frames=1", "summary frame count");
|
||||||
|
requireContains(summary, "avg_ms=10.000", "summary average");
|
||||||
|
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()
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
testFinishedFramesAreWrittenImmediatelyAndStatsAccumulate();
|
||||||
|
testSummaryPrintIsThrottledToOneHz();
|
||||||
|
testTerminalOutputDefaultsToOff();
|
||||||
|
}
|
||||||
|
catch (const std::exception &error)
|
||||||
|
{
|
||||||
|
std::cerr << "scan_odom_timing_logger_test failed: " << error.what() << std::endl;
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
66
src/origincar_base/test/serial_frame_parser_test.cpp
Normal file
66
src/origincar_base/test/serial_frame_parser_test.cpp
Normal file
@@ -0,0 +1,66 @@
|
|||||||
|
#include <array>
|
||||||
|
#include <cstdint>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
|
||||||
|
#include "origincar_base/serial_frame_parser.hpp"
|
||||||
|
|
||||||
|
using origincar_base_core::SerialFrameParser;
|
||||||
|
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
std::array<uint8_t, SerialFrameParser::kFrameSize> makeFrame(uint8_t seed)
|
||||||
|
{
|
||||||
|
std::array<uint8_t, SerialFrameParser::kFrameSize> frame{};
|
||||||
|
frame[0] = 0x7B;
|
||||||
|
frame[23] = 0x7D;
|
||||||
|
for (std::size_t index = 1; index < 23; ++index) {
|
||||||
|
frame[index] = static_cast<uint8_t>(seed + index);
|
||||||
|
}
|
||||||
|
return frame;
|
||||||
|
}
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
TEST(SerialFrameParserTest, ReassemblesFrameAcrossPartialReads)
|
||||||
|
{
|
||||||
|
SerialFrameParser parser;
|
||||||
|
const auto expected = makeFrame(10);
|
||||||
|
std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
|
||||||
|
|
||||||
|
parser.append(expected.data(), 7);
|
||||||
|
EXPECT_FALSE(parser.popFrame(&actual));
|
||||||
|
|
||||||
|
parser.append(expected.data() + 7, expected.size() - 7);
|
||||||
|
ASSERT_TRUE(parser.popFrame(&actual));
|
||||||
|
EXPECT_EQ(actual, expected);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SerialFrameParserTest, DiscardsNoiseBeforeValidFrame)
|
||||||
|
{
|
||||||
|
SerialFrameParser parser;
|
||||||
|
const auto expected = makeFrame(20);
|
||||||
|
const std::vector<uint8_t> noise{0x00, 0x01, 0x7D, 0x55};
|
||||||
|
std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
|
||||||
|
|
||||||
|
parser.append(noise.data(), noise.size());
|
||||||
|
parser.append(expected.data(), expected.size());
|
||||||
|
|
||||||
|
ASSERT_TRUE(parser.popFrame(&actual));
|
||||||
|
EXPECT_EQ(actual, expected);
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(SerialFrameParserTest, RejectsInvalidTailAndContinuesSearching)
|
||||||
|
{
|
||||||
|
SerialFrameParser parser;
|
||||||
|
auto invalid = makeFrame(30);
|
||||||
|
invalid[23] = 0x00;
|
||||||
|
const auto expected = makeFrame(40);
|
||||||
|
std::array<uint8_t, SerialFrameParser::kFrameSize> actual{};
|
||||||
|
|
||||||
|
parser.append(invalid.data(), invalid.size());
|
||||||
|
parser.append(expected.data(), expected.size());
|
||||||
|
|
||||||
|
ASSERT_TRUE(parser.popFrame(&actual));
|
||||||
|
EXPECT_EQ(actual, expected);
|
||||||
|
}
|
||||||
@@ -60,6 +60,69 @@ void testYawCorrectionUsesWrappedInnovation()
|
|||||||
|
|
||||||
require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary");
|
require(std::fabs(filter.pose().theta) > 175.0 * kPi / 180.0, "yaw should stay near wrap boundary");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void testScanPolicyGradesByWallCountAndMeanError()
|
||||||
|
{
|
||||||
|
const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
|
||||||
|
const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0};
|
||||||
|
|
||||||
|
const auto high = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, scan_pose, {4, 0.04, true}, false);
|
||||||
|
require(high.accept, "4 walls with low error should be accepted");
|
||||||
|
require(high.trust == origincar_wall::WallScanTrust::High, "4 walls with low error should be high trust");
|
||||||
|
requireNear(high.noise.x, 0.015, 1e-12, "high trust x noise");
|
||||||
|
|
||||||
|
const auto medium = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, scan_pose, {3, 0.08, true}, false);
|
||||||
|
require(medium.accept, "3 walls with medium error should be accepted");
|
||||||
|
require(medium.trust == origincar_wall::WallScanTrust::Medium, "3 walls with medium error should be medium trust");
|
||||||
|
requireNear(medium.noise.x, 0.045, 1e-12, "medium trust x noise");
|
||||||
|
|
||||||
|
const auto weak = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, scan_pose, {3, 0.12, true}, false);
|
||||||
|
require(weak.accept, "3 walls with weak error should be accepted as weak");
|
||||||
|
require(weak.trust == origincar_wall::WallScanTrust::Weak, "3 walls with weak error should be weak trust");
|
||||||
|
requireNear(weak.noise.x, 0.10, 1e-12, "weak trust x noise");
|
||||||
|
}
|
||||||
|
|
||||||
|
void testScanPolicyRejectsPoorOrJumpingMeasurements()
|
||||||
|
{
|
||||||
|
const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
|
||||||
|
|
||||||
|
const auto too_few_walls = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, {1.01, 1.0, 0.0}, {2, 0.04, true}, false);
|
||||||
|
require(!too_few_walls.accept, "fewer than 3 walls should be rejected");
|
||||||
|
|
||||||
|
const auto too_noisy = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, {1.01, 1.0, 0.0}, {4, 0.16, true}, false);
|
||||||
|
require(!too_noisy.accept, "mean error above 0.15 should be rejected");
|
||||||
|
|
||||||
|
const auto stationary_jump = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, {1.06, 1.0, 0.0}, {4, 0.03, true}, true);
|
||||||
|
require(!stationary_jump.accept, "stationary scan jump above 0.04m should be rejected");
|
||||||
|
}
|
||||||
|
|
||||||
|
void testScanPolicyDowngradesWhileStationary()
|
||||||
|
{
|
||||||
|
const origincar_wall::Pose2D filter_pose{1.0, 1.0, 0.0};
|
||||||
|
const origincar_wall::Pose2D scan_pose{1.02, 1.0, 0.0};
|
||||||
|
|
||||||
|
const auto downgraded_high = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, scan_pose, {4, 0.04, true}, true);
|
||||||
|
require(downgraded_high.accept, "stationary high trust scan should still be accepted");
|
||||||
|
require(downgraded_high.trust == origincar_wall::WallScanTrust::Medium,
|
||||||
|
"stationary high trust scan should downgrade to medium");
|
||||||
|
|
||||||
|
const auto downgraded_medium = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, scan_pose, {3, 0.08, true}, true);
|
||||||
|
require(downgraded_medium.accept, "stationary medium trust scan should still be accepted");
|
||||||
|
require(downgraded_medium.trust == origincar_wall::WallScanTrust::Weak,
|
||||||
|
"stationary medium trust scan should downgrade to weak");
|
||||||
|
|
||||||
|
const auto downgraded_weak = origincar_wall::chooseWallScanUpdatePolicy(
|
||||||
|
filter_pose, scan_pose, {3, 0.12, true}, true);
|
||||||
|
require(!downgraded_weak.accept, "stationary weak trust scan should be rejected");
|
||||||
|
}
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
int main()
|
int main()
|
||||||
@@ -69,6 +132,9 @@ int main()
|
|||||||
testPredictUsesWheelVelocityAndImuYawRate();
|
testPredictUsesWheelVelocityAndImuYawRate();
|
||||||
testScanUpdateTrustsLowNoiseMoreThanHighNoise();
|
testScanUpdateTrustsLowNoiseMoreThanHighNoise();
|
||||||
testYawCorrectionUsesWrappedInnovation();
|
testYawCorrectionUsesWrappedInnovation();
|
||||||
|
testScanPolicyGradesByWallCountAndMeanError();
|
||||||
|
testScanPolicyRejectsPoorOrJumpingMeasurements();
|
||||||
|
testScanPolicyDowngradesWhileStationary();
|
||||||
}
|
}
|
||||||
catch (const std::exception &error)
|
catch (const std::exception &error)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -19,7 +19,7 @@ must be replaced by a map created from the real environment before driving.
|
|||||||
2. The LSLIDAR model, interface and serial device match the selected parameter
|
2. The LSLIDAR model, interface and serial device match the selected parameter
|
||||||
file. The example N10 configuration uses `/dev/ttyCH343USB0`.
|
file. The example N10 configuration uses `/dev/ttyCH343USB0`.
|
||||||
3. The STM32 serial device and baud rate are correct. The default is
|
3. The STM32 serial device and baud rate are correct. The default is
|
||||||
`/dev/ttyACM0` and 115200.
|
`/dev/ttyACM0` and 921600.
|
||||||
4. The robot is Ackermann configured and the measured minimum turning radius is
|
4. The robot is Ackermann configured and the measured minimum turning radius is
|
||||||
close to 0.40 m.
|
close to 0.40 m.
|
||||||
5. The initial robot pose in the map is known and the start cell is free.
|
5. The initial robot pose in the map is known and the start cell is free.
|
||||||
@@ -59,7 +59,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
|
|||||||
lidar_model:=N10 \
|
lidar_model:=N10 \
|
||||||
start_lidar:=true \
|
start_lidar:=true \
|
||||||
serial_port:=/dev/ttyACM0 \
|
serial_port:=/dev/ttyACM0 \
|
||||||
serial_baud:=115200 \
|
serial_baud:=921600 \
|
||||||
enable_motion:=false
|
enable_motion:=false
|
||||||
```
|
```
|
||||||
|
|
||||||
|
|||||||
@@ -331,7 +331,7 @@ def generate_launch_description():
|
|||||||
DeclareLaunchArgument("start_base", default_value="true"),
|
DeclareLaunchArgument("start_base", default_value="true"),
|
||||||
DeclareLaunchArgument("start_robot_state_publisher", default_value="true"),
|
DeclareLaunchArgument("start_robot_state_publisher", default_value="true"),
|
||||||
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
||||||
DeclareLaunchArgument("serial_baud", default_value="115200"),
|
DeclareLaunchArgument("serial_baud", default_value="921600"),
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"enable_motion",
|
"enable_motion",
|
||||||
default_value="false",
|
default_value="false",
|
||||||
|
|||||||
@@ -273,7 +273,7 @@ def generate_launch_description():
|
|||||||
"lidar_params",
|
"lidar_params",
|
||||||
description="Required: LSLIDAR YAML matching the actual sensor/interface.",
|
description="Required: LSLIDAR YAML matching the actual sensor/interface.",
|
||||||
),
|
),
|
||||||
DeclareLaunchArgument("lidar_serial_port", default_value="/dev/ttyCH343USB0"),
|
DeclareLaunchArgument("lidar_serial_port", default_value="/dev/radar"),
|
||||||
DeclareLaunchArgument("lidar_model", default_value="N10"),
|
DeclareLaunchArgument("lidar_model", default_value="N10"),
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"start_lidar",
|
"start_lidar",
|
||||||
@@ -281,7 +281,7 @@ def generate_launch_description():
|
|||||||
description="Start LSLIDAR. Set false when /scan is provided externally.",
|
description="Start LSLIDAR. Set false when /scan is provided externally.",
|
||||||
),
|
),
|
||||||
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
|
||||||
DeclareLaunchArgument("serial_baud", default_value="115200"),
|
DeclareLaunchArgument("serial_baud", default_value="921600"),
|
||||||
DeclareLaunchArgument(
|
DeclareLaunchArgument(
|
||||||
"enable_motion",
|
"enable_motion",
|
||||||
default_value="false",
|
default_value="false",
|
||||||
|
|||||||
@@ -70,7 +70,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
|
|||||||
lidar_model:=N10 \
|
lidar_model:=N10 \
|
||||||
start_lidar:=true \
|
start_lidar:=true \
|
||||||
serial_port:=/dev/实际底盘串口 \
|
serial_port:=/dev/实际底盘串口 \
|
||||||
serial_baud:=115200 \
|
serial_baud:=921600 \
|
||||||
enable_motion:=false
|
enable_motion:=false
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -84,7 +84,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
|
|||||||
lidar_model:=N10 \
|
lidar_model:=N10 \
|
||||||
start_lidar:=true \
|
start_lidar:=true \
|
||||||
serial_port:=/dev/实际底盘串口 \
|
serial_port:=/dev/实际底盘串口 \
|
||||||
serial_baud:=115200 \
|
serial_baud:=921600 \
|
||||||
enable_motion:=false
|
enable_motion:=false
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -183,7 +183,7 @@ ros2 launch planner real_hybrid_astar.launch.py \
|
|||||||
lidar_model:=N10 \
|
lidar_model:=N10 \
|
||||||
start_lidar:=true \
|
start_lidar:=true \
|
||||||
serial_port:=/dev/实际底盘串口 \
|
serial_port:=/dev/实际底盘串口 \
|
||||||
serial_baud:=115200 \
|
serial_baud:=921600 \
|
||||||
wheelbase:=0.143 \
|
wheelbase:=0.143 \
|
||||||
max_steering_angle:=0.60 \
|
max_steering_angle:=0.60 \
|
||||||
enable_motion:=true
|
enable_motion:=true
|
||||||
|
|||||||
@@ -25,12 +25,23 @@ target_link_libraries(qr_detect
|
|||||||
ament_target_dependencies(qr_detect
|
ament_target_dependencies(qr_detect
|
||||||
rclcpp std_msgs sensor_msgs origincar_msg)
|
rclcpp std_msgs sensor_msgs origincar_msg)
|
||||||
|
|
||||||
install(TARGETS qr_detect
|
add_executable(qr_dete_depth src/qr_dete_depth.cpp)
|
||||||
|
target_include_directories(qr_dete_depth PUBLIC
|
||||||
|
${OpenCV_INCLUDE_DIRS} ${ZBar_INCLUDE_DIRS})
|
||||||
|
target_link_libraries(qr_dete_depth
|
||||||
|
${OpenCV_LIBS} ${ZBar_LIBRARIES})
|
||||||
|
ament_target_dependencies(qr_dete_depth
|
||||||
|
rclcpp std_msgs sensor_msgs origincar_msg)
|
||||||
|
|
||||||
|
install(TARGETS qr_detect qr_dete_depth
|
||||||
DESTINATION lib/${PROJECT_NAME})
|
DESTINATION lib/${PROJECT_NAME})
|
||||||
|
|
||||||
install(DIRECTORY launch/
|
install(DIRECTORY launch/
|
||||||
DESTINATION share/${PROJECT_NAME}/launch)
|
DESTINATION share/${PROJECT_NAME}/launch)
|
||||||
|
|
||||||
|
install(DIRECTORY config/
|
||||||
|
DESTINATION share/${PROJECT_NAME}/config)
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
if(BUILD_TESTING)
|
||||||
find_package(ament_cmake_gtest REQUIRED)
|
find_package(ament_cmake_gtest REQUIRED)
|
||||||
find_package(ament_lint_auto REQUIRED)
|
find_package(ament_lint_auto REQUIRED)
|
||||||
@@ -43,6 +54,15 @@ if(BUILD_TESTING)
|
|||||||
ament_target_dependencies(test_qr_detect
|
ament_target_dependencies(test_qr_detect
|
||||||
rclcpp std_msgs sensor_msgs origincar_msg)
|
rclcpp std_msgs sensor_msgs origincar_msg)
|
||||||
endif()
|
endif()
|
||||||
|
ament_add_gtest(test_qr_dete_depth test/test_qr_dete_depth.cpp)
|
||||||
|
if(TARGET test_qr_dete_depth)
|
||||||
|
target_include_directories(test_qr_dete_depth PUBLIC
|
||||||
|
${OpenCV_INCLUDE_DIRS} ${ZBar_INCLUDE_DIRS})
|
||||||
|
target_link_libraries(test_qr_dete_depth
|
||||||
|
${OpenCV_LIBS} ${ZBar_LIBRARIES})
|
||||||
|
ament_target_dependencies(test_qr_dete_depth
|
||||||
|
rclcpp std_msgs sensor_msgs origincar_msg)
|
||||||
|
endif()
|
||||||
set(ament_cmake_copyright_FOUND TRUE)
|
set(ament_cmake_copyright_FOUND TRUE)
|
||||||
set(ament_cmake_cpplint_FOUND TRUE)
|
set(ament_cmake_cpplint_FOUND TRUE)
|
||||||
ament_lint_auto_find_test_dependencies()
|
ament_lint_auto_find_test_dependencies()
|
||||||
|
|||||||
5
src/qr_detection/config/qr_dete_depth.yaml
Normal file
5
src/qr_detection/config/qr_dete_depth.yaml
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
qr_dete_depth:
|
||||||
|
ros__parameters:
|
||||||
|
image_topic: /aurora/rgb/image_raw
|
||||||
|
use_buffer: true
|
||||||
|
tts_service: /tts/speak
|
||||||
31
src/qr_detection/launch/qr_dete_depth.launch.py
Normal file
31
src/qr_detection/launch/qr_dete_depth.launch.py
Normal file
@@ -0,0 +1,31 @@
|
|||||||
|
import os
|
||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
default_params_file = os.path.join(
|
||||||
|
get_package_share_directory("qr_detection"),
|
||||||
|
"config",
|
||||||
|
"qr_dete_depth.yaml",
|
||||||
|
)
|
||||||
|
params_file = LaunchConfiguration("params_file")
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"params_file",
|
||||||
|
default_value=default_params_file,
|
||||||
|
description="Path to qr_dete_depth parameter YAML file",
|
||||||
|
),
|
||||||
|
Node(
|
||||||
|
package="qr_detection",
|
||||||
|
executable="qr_dete_depth",
|
||||||
|
name="qr_dete_depth",
|
||||||
|
output="screen",
|
||||||
|
parameters=[params_file],
|
||||||
|
),
|
||||||
|
])
|
||||||
@@ -26,8 +26,8 @@ def generate_launch_description():
|
|||||||
|
|
||||||
qr_detection_node = Node(
|
qr_detection_node = Node(
|
||||||
package='qr_detection',
|
package='qr_detection',
|
||||||
executable='qr_dete_node',
|
executable='qr_detect',
|
||||||
name='qr_dete_node',
|
name='qr_detect',
|
||||||
output='screen'
|
output='screen'
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|||||||
@@ -1,173 +1,208 @@
|
|||||||
#include <memory>
|
#include "origincar_msg/srv/speak.hpp"
|
||||||
#include <iostream>
|
|
||||||
#include <opencv2/opencv.hpp>
|
|
||||||
#include <zbar.h>
|
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
#include "sensor_msgs/msg/image.hpp"
|
||||||
#include "std_msgs/msg/int32.hpp"
|
#include "std_msgs/msg/int32.hpp"
|
||||||
#include "std_msgs/msg/string.hpp"
|
#include "std_msgs/msg/string.hpp"
|
||||||
#include "sensor_msgs/msg/image.hpp"
|
|
||||||
|
|
||||||
class MinimalHbmemSubscriber : public rclcpp::Node
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <zbar.h>
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <memory>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
class QrDeteDepthNode : public rclcpp::Node
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
MinimalHbmemSubscriber()
|
explicit QrDeteDepthNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
|
||||||
: Node("qr_detection"), detect_qr_code_(true) // 默认检测二维码
|
: Node("qr_dete_depth", options), enabled_(true)
|
||||||
{
|
{
|
||||||
use_buffer = this->declare_parameter("use_buffer", true); // 声明参数，默认使用缓存
|
image_topic_ = declare_parameter<std::string>("image_topic", "/aurora/rgb/image_raw");
|
||||||
// 订阅 /aurora/rgb/image_raw（sensor_msgs::msg::Image 格式）
|
use_buffer_ = declare_parameter<bool>("use_buffer", true);
|
||||||
subscription_image_ =
|
tts_service_ = declare_parameter<std::string>("tts_service", "/tts/speak");
|
||||||
this->create_subscription<sensor_msgs::msg::Image>(
|
|
||||||
"/aurora/rgb/image_raw",
|
|
||||||
10,
|
|
||||||
std::bind(&MinimalHbmemSubscriber::image_callback, this, std::placeholders::_1));
|
|
||||||
|
|
||||||
// 创建订阅器，订阅 'sign4return' 话题
|
image_sub_ = create_subscription<sensor_msgs::msg::Image>(
|
||||||
// subscription_sign_ =
|
image_topic_, rclcpp::SensorDataQoS(),
|
||||||
// this->create_subscription<std_msgs::msg::Int32>(
|
std::bind(&QrDeteDepthNode::image_callback, this, std::placeholders::_1));
|
||||||
// "sign4return",
|
|
||||||
// 10,
|
|
||||||
// std::bind(&MinimalHbmemSubscriber::sign_callback, this, std::placeholders::_1));
|
|
||||||
// 创建发布器，发布 'sign4return' 话题
|
|
||||||
sign_publisher_ = this->create_publisher<std_msgs::msg::Int32>("sign4return", 10);
|
|
||||||
|
|
||||||
// 创建 publisher，topic 为 "qr_results"
|
const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
|
||||||
publisher_ =
|
sign_sub_ = create_subscription<std_msgs::msg::Int32>(
|
||||||
this->create_publisher<std_msgs::msg::String>("qr_results", 10);
|
"sign4return", sign_qos,
|
||||||
|
std::bind(&QrDeteDepthNode::sign_callback, this, std::placeholders::_1));
|
||||||
|
|
||||||
|
result_pub_ = create_publisher<std_msgs::msg::String>("qr_results", 10);
|
||||||
|
tts_client_ = create_client<origincar_msg::srv::Speak>(tts_service_);
|
||||||
|
|
||||||
|
RCLCPP_INFO(
|
||||||
|
get_logger(), "QR depth detect ready, topic=%s, use_buffer=%s, tts_service=%s",
|
||||||
|
image_topic_.c_str(), use_buffer_ ? "true" : "false", tts_service_.c_str());
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// 图像回调函数，处理 /aurora/rgb/image_raw 话题
|
|
||||||
void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
|
void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
|
||||||
{
|
{
|
||||||
if (!detect_qr_code_)
|
if (!enabled_) {
|
||||||
{
|
|
||||||
RCLCPP_INFO(this->get_logger(), "QR detection is disabled");
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// 创建 Clock 对象并获取当前时间
|
log_frame_delay(msg);
|
||||||
auto clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
|
|
||||||
auto now = clock->now();
|
|
||||||
|
|
||||||
// 转换消息时间戳为 ROS 时间
|
|
||||||
auto msg_time = rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec);
|
|
||||||
|
|
||||||
// 计算延时（单位：微秒）
|
|
||||||
auto duration = now - msg_time;
|
|
||||||
auto delay_us = duration.nanoseconds() / 1000; // 转换为微秒
|
|
||||||
|
|
||||||
// 打印延迟
|
|
||||||
RCLCPP_INFO(this->get_logger(), "frame_id: %s, time cost %ldus",
|
|
||||||
msg->header.frame_id.c_str(), delay_us);
|
|
||||||
|
|
||||||
// 将 sensor_msgs::Image 转换为 OpenCV 格式（Aurora RGB 为 bgr8 编码，需转为灰度给 ZBar）
|
|
||||||
if (msg->encoding != "bgr8") {
|
|
||||||
RCLCPP_ERROR(this->get_logger(), "Expected bgr8 encoding, got %s", msg->encoding.c_str());
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
cv::Mat rgb_image(msg->height, msg->width, CV_8UC3,
|
|
||||||
const_cast<uint8_t*>(msg->data.data()));
|
|
||||||
cv::Mat gray_image;
|
cv::Mat gray_image;
|
||||||
cv::cvtColor(rgb_image, gray_image, cv::COLOR_BGR2GRAY);
|
if (!image_to_gray(msg, gray_image)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// 初始化 ZBar 扫描器
|
scan_and_publish(gray_image);
|
||||||
|
}
|
||||||
|
|
||||||
|
void log_frame_delay(const sensor_msgs::msg::Image::SharedPtr msg)
|
||||||
|
{
|
||||||
|
const auto now = get_clock()->now();
|
||||||
|
const auto msg_time = rclcpp::Time(msg->header.stamp);
|
||||||
|
const auto delay_us = (now - msg_time).nanoseconds() / 1000;
|
||||||
|
|
||||||
|
RCLCPP_INFO(
|
||||||
|
get_logger(), "frame_id: %s, time cost %ldus",
|
||||||
|
msg->header.frame_id.c_str(), delay_us);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool image_to_gray(const sensor_msgs::msg::Image::SharedPtr msg, cv::Mat & gray_image)
|
||||||
|
{
|
||||||
|
if (msg->encoding != "bgr8") {
|
||||||
|
static int warn_count = 0;
|
||||||
|
if (warn_count++ < 3) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "Expected bgr8 encoding, got %s", msg->encoding.c_str());
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
cv::Mat bgr_image(
|
||||||
|
msg->height, msg->width, CV_8UC3,
|
||||||
|
const_cast<uint8_t *>(msg->data.data()));
|
||||||
|
cv::cvtColor(bgr_image, gray_image, cv::COLOR_BGR2GRAY);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void scan_and_publish(cv::Mat & gray_image)
|
||||||
|
{
|
||||||
zbar::ImageScanner scanner;
|
zbar::ImageScanner scanner;
|
||||||
scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
|
scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);
|
||||||
|
|
||||||
// 将 OpenCV 图像数据包装成 ZBar 图像
|
zbar::Image zbar_image(
|
||||||
zbar::Image zbar_image(gray_image.cols, gray_image.rows, "Y800",
|
gray_image.cols, gray_image.rows, "Y800",
|
||||||
gray_image.data, gray_image.cols * gray_image.rows);
|
gray_image.data, gray_image.cols * gray_image.rows);
|
||||||
|
|
||||||
// 扫描图像中的条形码和二维码
|
|
||||||
std::string qr_results;
|
std::string qr_results;
|
||||||
int n = scanner.scan(zbar_image);
|
const int n = scanner.scan(zbar_image);
|
||||||
if (n > 0) {
|
if (n > 0) {
|
||||||
for (auto symbol = zbar_image.symbol_begin();
|
for (auto symbol = zbar_image.symbol_begin();
|
||||||
symbol != zbar_image.symbol_end(); ++symbol) {
|
symbol != zbar_image.symbol_end(); ++symbol)
|
||||||
|
{
|
||||||
qr_results += symbol->get_data();
|
qr_results += symbol->get_data();
|
||||||
RCLCPP_INFO(this->get_logger(), "Decoded %s symbol \"%s\"",
|
RCLCPP_INFO(
|
||||||
symbol->get_type_name().c_str(), symbol->get_data().c_str());
|
get_logger(), "Decoded %s symbol \"%s\"",
|
||||||
|
symbol->get_type_name().c_str(), symbol->get_data().c_str());
|
||||||
}
|
}
|
||||||
qr_results_buff_ = qr_results; // 更新缓存
|
qr_results_buff_ = qr_results;
|
||||||
|
|
||||||
// 检测字符合法性
|
|
||||||
// if (ResultLegal(qr_results)){
|
|
||||||
// // 发布sign4return话题
|
|
||||||
// auto message = std_msgs::msg::Int32();
|
|
||||||
// message.data = 5; // 5表示检测到二维码
|
|
||||||
// sign_publisher_->publish(message);
|
|
||||||
// RCLCPP_INFO(this->get_logger(), "QR Results Legal");
|
|
||||||
// }
|
|
||||||
// else{
|
|
||||||
// RCLCPP_INFO(this->get_logger(), "\033[31m QR Results Illegal \033[00m");
|
|
||||||
// }
|
|
||||||
|
|
||||||
// 发布sign4return话题
|
|
||||||
auto message = std_msgs::msg::Int32();
|
|
||||||
message.data = 5; // 5表示检测到二维码
|
|
||||||
sign_publisher_->publish(message);
|
|
||||||
} else {
|
} else {
|
||||||
qr_results = "QR Code not detected";
|
qr_results = "QR Code not detected";
|
||||||
RCLCPP_INFO(this->get_logger(), "QR Code not detected");
|
RCLCPP_INFO(get_logger(), "QR Code not detected");
|
||||||
}
|
}
|
||||||
|
|
||||||
// 发布 QR 结果
|
const std::string result_text = use_buffer_ ? qr_results_buff_ : qr_results;
|
||||||
auto message = std_msgs::msg::String();
|
if (result_text.empty()) {
|
||||||
if (use_buffer){
|
return;
|
||||||
message.data = qr_results_buff_;
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
message.data = qr_results;
|
|
||||||
}
|
}
|
||||||
publisher_->publish(message);
|
|
||||||
|
publish_result(result_text);
|
||||||
}
|
}
|
||||||
|
|
||||||
// 消息回调函数，处理 sign4return 话题
|
void publish_result(const std::string & qr_data)
|
||||||
// void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
|
{
|
||||||
// {
|
auto out = std_msgs::msg::String();
|
||||||
// if (msg->data == 0)
|
if (is_numeric(qr_data)) {
|
||||||
// {
|
out.data = qr_data + " " + (((qr_data.back() - '0') % 2 == 1) ? "顺时针" : "逆时针");
|
||||||
// detect_qr_code_ = true; // 启动二维码检测
|
} else {
|
||||||
// RCLCPP_INFO(this->get_logger(), "QR detection started");
|
out.data = qr_data;
|
||||||
// }
|
}
|
||||||
// else if (msg->data == 5)
|
|
||||||
// {
|
|
||||||
// detect_qr_code_ = false; // 停止二维码检测
|
|
||||||
// RCLCPP_INFO(this->get_logger(), "QR detection stopped");
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
|
|
||||||
// 识别结果合法性检测
|
if (out.data != last_) {
|
||||||
bool ResultLegal(std::string& input){
|
RCLCPP_INFO(get_logger(), "QR: %s", out.data.c_str());
|
||||||
// 合法字符串集合
|
last_ = out.data;
|
||||||
static const std::unordered_set<std::string> kValidStrings = {
|
}
|
||||||
"1", "2", "顺时针", "逆时针", "顺", "逆"
|
|
||||||
};
|
result_pub_->publish(out);
|
||||||
return kValidStrings.find(input) != kValidStrings.end();
|
speak_once(out.data);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool is_numeric(const std::string & input) const
|
||||||
|
{
|
||||||
|
return !input.empty() && input.find_first_not_of("0123456789") == std::string::npos;
|
||||||
|
}
|
||||||
|
|
||||||
// /aurora/rgb/image_raw 订阅器
|
void speak_once(const std::string & text)
|
||||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_image_;
|
{
|
||||||
// sign4return 订阅器
|
if (text == last_spoken_) {
|
||||||
// rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr subscription_sign_;
|
return;
|
||||||
// sign4return 发布器
|
}
|
||||||
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr sign_publisher_;
|
last_spoken_ = text;
|
||||||
// QR code results 发布器
|
|
||||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
|
|
||||||
|
|
||||||
// 参数
|
if (!tts_client_->service_is_ready()) {
|
||||||
bool use_buffer;
|
static int warn_count = 0;
|
||||||
// 二维码检测标志位
|
if (warn_count++ < 3) {
|
||||||
bool detect_qr_code_;
|
RCLCPP_WARN(get_logger(), "TTS service %s not available", tts_service_.c_str());
|
||||||
// 二维码检测结果缓存
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
auto request = std::make_shared<origincar_msg::srv::Speak::Request>();
|
||||||
|
request->text = text;
|
||||||
|
tts_client_->async_send_request(
|
||||||
|
request,
|
||||||
|
[this](rclcpp::Client<origincar_msg::srv::Speak>::SharedFuture future) {
|
||||||
|
try {
|
||||||
|
const auto response = future.get();
|
||||||
|
if (!response->success) {
|
||||||
|
RCLCPP_WARN(get_logger(), "TTS failed: %s", response->message.c_str());
|
||||||
|
}
|
||||||
|
} catch (const std::exception & e) {
|
||||||
|
RCLCPP_ERROR(get_logger(), "TTS call error: %s", e.what());
|
||||||
|
}
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
void sign_callback(const std_msgs::msg::Int32::SharedPtr msg)
|
||||||
|
{
|
||||||
|
if (msg->data == 0) {
|
||||||
|
enabled_ = true;
|
||||||
|
last_.clear();
|
||||||
|
last_spoken_.clear();
|
||||||
|
qr_results_buff_.clear();
|
||||||
|
RCLCPP_INFO(get_logger(), "ON");
|
||||||
|
} else if (msg->data == 5) {
|
||||||
|
enabled_ = false;
|
||||||
|
RCLCPP_INFO(get_logger(), "OFF");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
|
||||||
|
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sign_sub_;
|
||||||
|
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr result_pub_;
|
||||||
|
rclcpp::Client<origincar_msg::srv::Speak>::SharedPtr tts_client_;
|
||||||
|
|
||||||
|
bool enabled_;
|
||||||
|
bool use_buffer_;
|
||||||
|
std::string image_topic_;
|
||||||
|
std::string tts_service_;
|
||||||
|
std::string last_;
|
||||||
|
std::string last_spoken_;
|
||||||
std::string qr_results_buff_;
|
std::string qr_results_buff_;
|
||||||
};
|
};
|
||||||
|
|
||||||
int main(int argc, char * argv[])
|
int main(int argc, char * argv[])
|
||||||
{
|
{
|
||||||
rclcpp::init(argc, argv);
|
rclcpp::init(argc, argv);
|
||||||
rclcpp::spin(std::make_shared<MinimalHbmemSubscriber>());
|
rclcpp::spin(std::make_shared<QrDeteDepthNode>());
|
||||||
rclcpp::shutdown();
|
rclcpp::shutdown();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -49,8 +49,9 @@ public:
|
|||||||
auto_select_image_subscription();
|
auto_select_image_subscription();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
|
||||||
sign_sub_ = create_subscription<std_msgs::msg::Int32>(
|
sign_sub_ = create_subscription<std_msgs::msg::Int32>(
|
||||||
"sign4return", 10,
|
"sign4return", sign_qos,
|
||||||
std::bind(&QrDetectNode::sign_cb, this, std::placeholders::_1));
|
std::bind(&QrDetectNode::sign_cb, this, std::placeholders::_1));
|
||||||
|
|
||||||
pub_ = create_publisher<std_msgs::msg::String>("qr_results", 10);
|
pub_ = create_publisher<std_msgs::msg::String>("qr_results", 10);
|
||||||
@@ -243,8 +244,15 @@ private:
|
|||||||
|
|
||||||
void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
|
void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
|
||||||
{
|
{
|
||||||
if (msg->data == 0) { enabled_ = true; RCLCPP_INFO(get_logger(), "ON"); }
|
if (msg->data == 0) {
|
||||||
else if (msg->data == 5) { enabled_ = false; RCLCPP_INFO(get_logger(), "OFF"); }
|
enabled_ = true;
|
||||||
|
last_.clear();
|
||||||
|
last_spoken_.clear();
|
||||||
|
RCLCPP_INFO(get_logger(), "ON");
|
||||||
|
} else if (msg->data == 5) {
|
||||||
|
enabled_ = false;
|
||||||
|
RCLCPP_INFO(get_logger(), "OFF");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
|
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
|
||||||
|
|||||||
@@ -28,8 +28,9 @@ public:
|
|||||||
std::bind(&QrHbmemNode::callback, this, std::placeholders::_1));
|
std::bind(&QrHbmemNode::callback, this, std::placeholders::_1));
|
||||||
|
|
||||||
// sign4return 启停控制
|
// sign4return 启停控制
|
||||||
|
const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
|
||||||
sign_sub_ = this->create_subscription<std_msgs::msg::Int32>(
|
sign_sub_ = this->create_subscription<std_msgs::msg::Int32>(
|
||||||
"sign4return", 10,
|
"sign4return", sign_qos,
|
||||||
std::bind(&QrHbmemNode::sign_cb, this, std::placeholders::_1));
|
std::bind(&QrHbmemNode::sign_cb, this, std::placeholders::_1));
|
||||||
|
|
||||||
// 结果发布
|
// 结果发布
|
||||||
@@ -74,8 +75,14 @@ private:
|
|||||||
|
|
||||||
void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
|
void sign_cb(const std_msgs::msg::Int32::SharedPtr msg)
|
||||||
{
|
{
|
||||||
if (msg->data == 0) { detect_enabled_ = true; RCLCPP_INFO(this->get_logger(), "ON"); }
|
if (msg->data == 0) {
|
||||||
else if (msg->data == 5) { detect_enabled_ = false; RCLCPP_INFO(this->get_logger(), "OFF"); }
|
detect_enabled_ = true;
|
||||||
|
last_found_.clear();
|
||||||
|
RCLCPP_INFO(this->get_logger(), "ON");
|
||||||
|
} else if (msg->data == 5) {
|
||||||
|
detect_enabled_ = false;
|
||||||
|
RCLCPP_INFO(this->get_logger(), "OFF");
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
rclcpp::SubscriptionHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr hbmem_sub_;
|
rclcpp::SubscriptionHbmem<hbm_img_msgs::msg::HbmMsg1080P>::SharedPtr hbmem_sub_;
|
||||||
|
|||||||
@@ -55,14 +55,15 @@ public:
|
|||||||
std::bind(&QrUsbCameraNode::image_callback, this, std::placeholders::_1));
|
std::bind(&QrUsbCameraNode::image_callback, this, std::placeholders::_1));
|
||||||
|
|
||||||
// ── 订阅 sign4return (远程控制开关) ──
|
// ── 订阅 sign4return (远程控制开关) ──
|
||||||
|
const auto sign_qos = rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
|
||||||
sign_sub_ = create_subscription<std_msgs::msg::Int32>(
|
sign_sub_ = create_subscription<std_msgs::msg::Int32>(
|
||||||
"sign4return", 10,
|
"sign4return", sign_qos,
|
||||||
std::bind(&QrUsbCameraNode::sign_callback, this, std::placeholders::_1));
|
std::bind(&QrUsbCameraNode::sign_callback, this, std::placeholders::_1));
|
||||||
|
|
||||||
// ── 发布者 ──
|
// ── 发布者 ──
|
||||||
qr_pub_ = create_publisher<qr_detection::msg::QrDetection>("qr_detection", 10);
|
qr_pub_ = create_publisher<qr_detection::msg::QrDetection>("qr_detection", 10);
|
||||||
qr_text_pub_ = create_publisher<std_msgs::msg::String>("qr_text", 10);
|
qr_text_pub_ = create_publisher<std_msgs::msg::String>("qr_text", 10);
|
||||||
sign_pub_ = create_publisher<std_msgs::msg::Int32>("sign4return", 10);
|
sign_pub_ = create_publisher<std_msgs::msg::Int32>("sign4return", sign_qos);
|
||||||
|
|
||||||
// ── 定时器: 检查缓冲区超时 ──
|
// ── 定时器: 检查缓冲区超时 ──
|
||||||
buffer_timer_ = create_wall_timer(
|
buffer_timer_ = create_wall_timer(
|
||||||
@@ -202,6 +203,11 @@ private:
|
|||||||
{
|
{
|
||||||
if (msg->data == 0) {
|
if (msg->data == 0) {
|
||||||
detect_enabled_ = true;
|
detect_enabled_ = true;
|
||||||
|
qr_found_ = false;
|
||||||
|
qr_lost_count_ = 0;
|
||||||
|
prev_qr_text_.clear();
|
||||||
|
buffer_text_.clear();
|
||||||
|
buffer_time_acc_ = 0.0;
|
||||||
RCLCPP_INFO(get_logger(), "QR detection ENABLED");
|
RCLCPP_INFO(get_logger(), "QR detection ENABLED");
|
||||||
} else if (msg->data == 5) {
|
} else if (msg->data == 5) {
|
||||||
detect_enabled_ = false;
|
detect_enabled_ = false;
|
||||||
|
|||||||
71
src/qr_detection/test/test_qr_dete_depth.cpp
Normal file
71
src/qr_detection/test/test_qr_dete_depth.cpp
Normal file
@@ -0,0 +1,71 @@
|
|||||||
|
#include <algorithm>
|
||||||
|
#include <chrono>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
#include <origincar_msg/srv/speak.hpp>
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <sensor_msgs/msg/image.hpp>
|
||||||
|
#include <std_msgs/msg/int32.hpp>
|
||||||
|
#include <std_msgs/msg/string.hpp>
|
||||||
|
|
||||||
|
#define private public
|
||||||
|
#define main qr_dete_depth_node_main
|
||||||
|
#include "../src/qr_dete_depth.cpp"
|
||||||
|
#undef main
|
||||||
|
#undef private
|
||||||
|
|
||||||
|
namespace
|
||||||
|
{
|
||||||
|
|
||||||
|
bool has_endpoint_type(
|
||||||
|
const std::vector<rclcpp::TopicEndpointInfo> & endpoints,
|
||||||
|
const std::string & topic_type)
|
||||||
|
{
|
||||||
|
return std::any_of(
|
||||||
|
endpoints.begin(), endpoints.end(),
|
||||||
|
[&](const rclcpp::TopicEndpointInfo & endpoint) {
|
||||||
|
return endpoint.topic_type() == topic_type;
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
TEST(QrDeteDepthNode, exposesQrResultAndControlEndpoints)
|
||||||
|
{
|
||||||
|
if (!rclcpp::ok()) {
|
||||||
|
rclcpp::init(0, nullptr);
|
||||||
|
}
|
||||||
|
|
||||||
|
rclcpp::NodeOptions options;
|
||||||
|
options.append_parameter_override("image_topic", "/depth_qr_test_image");
|
||||||
|
auto qr_node = std::make_shared<QrDeteDepthNode>(options);
|
||||||
|
auto graph_node = std::make_shared<rclcpp::Node>("qr_dete_depth_graph_test");
|
||||||
|
rclcpp::executors::SingleThreadedExecutor executor;
|
||||||
|
executor.add_node(qr_node);
|
||||||
|
executor.add_node(graph_node);
|
||||||
|
|
||||||
|
std::vector<rclcpp::TopicEndpointInfo> image_subscribers;
|
||||||
|
std::vector<rclcpp::TopicEndpointInfo> sign_subscribers;
|
||||||
|
std::vector<rclcpp::TopicEndpointInfo> result_publishers;
|
||||||
|
const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(3);
|
||||||
|
while (std::chrono::steady_clock::now() < deadline) {
|
||||||
|
executor.spin_some();
|
||||||
|
image_subscribers = graph_node->get_subscriptions_info_by_topic("/depth_qr_test_image");
|
||||||
|
sign_subscribers = graph_node->get_subscriptions_info_by_topic("/sign4return");
|
||||||
|
result_publishers = graph_node->get_publishers_info_by_topic("/qr_results");
|
||||||
|
if (has_endpoint_type(image_subscribers, "sensor_msgs/msg/Image") &&
|
||||||
|
has_endpoint_type(sign_subscribers, "std_msgs/msg/Int32") &&
|
||||||
|
has_endpoint_type(result_publishers, "std_msgs/msg/String"))
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
rclcpp::sleep_for(std::chrono::milliseconds(100));
|
||||||
|
}
|
||||||
|
|
||||||
|
EXPECT_TRUE(has_endpoint_type(image_subscribers, "sensor_msgs/msg/Image"));
|
||||||
|
EXPECT_TRUE(has_endpoint_type(sign_subscribers, "std_msgs/msg/Int32"));
|
||||||
|
EXPECT_TRUE(has_endpoint_type(result_publishers, "std_msgs/msg/String"));
|
||||||
|
ASSERT_NE(qr_node->tts_client_, nullptr);
|
||||||
|
EXPECT_STREQ(qr_node->tts_client_->get_service_name(), "/tts/speak");
|
||||||
|
}
|
||||||
41
src/racing_control/AGENTS.md
Normal file
41
src/racing_control/AGENTS.md
Normal file
@@ -0,0 +1,41 @@
|
|||||||
|
# 项目说明
|
||||||
|
|
||||||
|
## 项目用途
|
||||||
|
|
||||||
|
- 这是 RDKx5 赛车机器人的 ROS2 Humble 工作区。
|
||||||
|
- `src/racing_control` 是比赛总调度包。它只负责调度已有的感知、导航、VLM、TTS 和备用轨迹保护节点，不在包内重新实现这些功能。
|
||||||
|
|
||||||
|
## 环境准备
|
||||||
|
|
||||||
|
- 通过 `ssh sunrise@192.168.10.210` 连接机器人。
|
||||||
|
- 构建或运行前先 source ROS 和工作区：
|
||||||
|
`source /opt/ros/humble/setup.bash && source /home/sunrise/yiliao_ws/install/setup.bash`。
|
||||||
|
- 部分相机/Hobot 流程可能还需要 source `/opt/tros/humble/setup.bash`。
|
||||||
|
|
||||||
|
## 构建与测试
|
||||||
|
|
||||||
|
- 只构建比赛总控：
|
||||||
|
`cd /home/sunrise/yiliao_ws && source /opt/ros/humble/setup.bash && source install/setup.bash && colcon build --packages-select racing_control --cmake-args -DBUILD_TESTING=ON`。
|
||||||
|
- 只测试比赛总控：
|
||||||
|
`cd /home/sunrise/yiliao_ws && source /opt/ros/humble/setup.bash && source install/setup.bash && colcon test --packages-select racing_control && colcon test-result --verbose --test-result-base build/racing_control`。
|
||||||
|
- `ament_xmllint` 会从 `download.ros.org` 读取 package schema；如果出现临时资源或网络错误，先重跑 xmllint 测试，不要急着修改 XML。
|
||||||
|
|
||||||
|
## 架构
|
||||||
|
|
||||||
|
- `/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`，属于备用链路；默认比赛流程不依赖它。
|
||||||
|
- 比赛总控使用 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`。
|
||||||
|
|
||||||
|
## 外部系统
|
||||||
|
|
||||||
|
- `vlm_detect` 会调用由 launch 参数配置的外部 VLM 服务，该服务可能运行在另一台机器上。
|
||||||
|
- `vlm_detect` 同时通过 `origincar_msg/srv/Speak` 提供 `/tts/speak` 语音服务。
|
||||||
|
|
||||||
|
## 已知注意事项
|
||||||
|
|
||||||
|
- 不要让多个节点同时发布 `/cmd_vel`，避免与 Nav2 controller 输出互相抢控制权。
|
||||||
|
- 实际比赛点位依赖现场标定；不要把代码或 yaml 默认值当作真实赛场坐标。
|
||||||
|
- 机器人底盘也订阅 `/sign4return`；总调度节点不要发布未明确约定的负数复位指令。
|
||||||
@@ -1,17 +1,70 @@
|
|||||||
cmake_minimum_required(VERSION 3.8)
|
cmake_minimum_required(VERSION 3.8)
|
||||||
project(racing_control)
|
project(racing_control)
|
||||||
|
|
||||||
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
|
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||||
|
|
||||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
# find dependencies
|
# find dependencies
|
||||||
find_package(ament_cmake REQUIRED)
|
find_package(ament_cmake REQUIRED)
|
||||||
|
find_package(geometry_msgs REQUIRED)
|
||||||
|
find_package(nav_msgs REQUIRED)
|
||||||
|
find_package(nav2_msgs REQUIRED)
|
||||||
|
find_package(rclcpp REQUIRED)
|
||||||
|
find_package(rclcpp_action REQUIRED)
|
||||||
|
find_package(sensor_msgs REQUIRED)
|
||||||
|
find_package(std_msgs REQUIRED)
|
||||||
|
|
||||||
|
add_library(racing_control_core
|
||||||
|
src/candidate_waypoint_selector.cpp
|
||||||
|
src/final_home_fallback.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(racing_control_core PUBLIC
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
ament_target_dependencies(racing_control_core
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
)
|
||||||
|
|
||||||
|
add_executable(racing_control
|
||||||
|
src/racing_control.cpp
|
||||||
|
src/candidate_waypoint_selector.cpp
|
||||||
|
src/final_home_fallback.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(racing_control PUBLIC
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
$<INSTALL_INTERFACE:include>
|
||||||
|
)
|
||||||
|
ament_target_dependencies(racing_control
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
nav2_msgs
|
||||||
|
rclcpp
|
||||||
|
rclcpp_action
|
||||||
|
sensor_msgs
|
||||||
|
std_msgs
|
||||||
|
)
|
||||||
|
|
||||||
|
install(TARGETS
|
||||||
|
racing_control_core
|
||||||
|
racing_control
|
||||||
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
install(DIRECTORY include/ DESTINATION include)
|
||||||
|
install(DIRECTORY config launch
|
||||||
|
DESTINATION share/${PROJECT_NAME}
|
||||||
|
)
|
||||||
# uncomment the following section in order to fill in
|
# uncomment the following section in order to fill in
|
||||||
# further dependencies manually.
|
# further dependencies manually.
|
||||||
# find_package(<dependency> REQUIRED)
|
# find_package(<dependency> REQUIRED)
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
if(BUILD_TESTING)
|
||||||
|
find_package(ament_cmake_gtest REQUIRED)
|
||||||
find_package(ament_lint_auto REQUIRED)
|
find_package(ament_lint_auto REQUIRED)
|
||||||
# the following line skips the linter which checks for copyrights
|
# the following line skips the linter which checks for copyrights
|
||||||
# comment the line when a copyright and license is added to all source files
|
# comment the line when a copyright and license is added to all source files
|
||||||
@@ -20,6 +73,23 @@ if(BUILD_TESTING)
|
|||||||
# comment the line when this package is in a git repo and when
|
# comment the line when this package is in a git repo and when
|
||||||
# a copyright and license is added to all source files
|
# a copyright and license is added to all source files
|
||||||
set(ament_cmake_cpplint_FOUND TRUE)
|
set(ament_cmake_cpplint_FOUND TRUE)
|
||||||
|
|
||||||
|
ament_add_gtest(test_racing_control_helpers
|
||||||
|
test/test_racing_control_helpers.cpp
|
||||||
|
)
|
||||||
|
target_include_directories(test_racing_control_helpers PRIVATE
|
||||||
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
)
|
||||||
|
ament_target_dependencies(test_racing_control_helpers
|
||||||
|
geometry_msgs
|
||||||
|
nav_msgs
|
||||||
|
rclcpp
|
||||||
|
)
|
||||||
|
target_sources(test_racing_control_helpers PRIVATE
|
||||||
|
src/candidate_waypoint_selector.cpp
|
||||||
|
src/final_home_fallback.cpp
|
||||||
|
)
|
||||||
|
|
||||||
ament_lint_auto_find_test_dependencies()
|
ament_lint_auto_find_test_dependencies()
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
|
|||||||
125
src/racing_control/config/racing_control.yaml
Normal file
125
src/racing_control/config/racing_control.yaml
Normal file
@@ -0,0 +1,125 @@
|
|||||||
|
racing_control:
|
||||||
|
ros__parameters:
|
||||||
|
# Startup
|
||||||
|
auto_start: false
|
||||||
|
frame_id: odom
|
||||||
|
use_post_qr_pose: false
|
||||||
|
split_qr_to_vlm_segment: true
|
||||||
|
enable_vlm_image_relay: false
|
||||||
|
enable_dynamic_replanning: false
|
||||||
|
enable_recovery: true
|
||||||
|
enable_candidate_waypoint_selection: true
|
||||||
|
vlm_image_input_topic: /image
|
||||||
|
vlm_image_output_topic: /vlm_image
|
||||||
|
|
||||||
|
# Shared coordination topics
|
||||||
|
sign_topic: /sign4return
|
||||||
|
qr_result_topic: /qr_results
|
||||||
|
vlm_result_topic: /vlm_result
|
||||||
|
odom_topic: /odom_combined
|
||||||
|
recovery_cmd_vel_topic: /cmd_vel
|
||||||
|
global_costmap_topic: /global_costmap/costmap
|
||||||
|
trajectory_guard_input_topic: /trajectory_guard/input_path
|
||||||
|
|
||||||
|
# Nav2 actions and plugin IDs
|
||||||
|
navigate_action: /navigate_to_pose
|
||||||
|
compute_path_action: /compute_path_through_poses
|
||||||
|
follow_path_action: /follow_path
|
||||||
|
planner_id: GridBased
|
||||||
|
controller_id: FollowPath
|
||||||
|
goal_checker_id: ""
|
||||||
|
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: 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
|
||||||
|
# Used only when the initial ComputePathThroughPoses request fails.
|
||||||
|
planning_failure_backup_speed: -0.1
|
||||||
|
planning_failure_backup_distance: 0.03
|
||||||
|
planning_failure_backup_timeout_sec: 0.3
|
||||||
|
recovery_clear_wait_sec: 1.0
|
||||||
|
recovery_search_radius_m: 1.0
|
||||||
|
recovery_rear_clear_distance_m: 0.5
|
||||||
|
# 最终回家保护：普通 Nav2/恢复失败后，低速直控回 home，同时定时探测 Nav2 是否恢复。
|
||||||
|
enable_final_home_fallback: true
|
||||||
|
final_home_distance_tolerance: 0.2
|
||||||
|
final_home_yaw_tolerance: 0.3
|
||||||
|
final_home_forward_speed: 0.12
|
||||||
|
final_home_yaw_gain: 0.5
|
||||||
|
final_home_max_turn_speed: 0.7
|
||||||
|
final_home_nav_retry_interval_sec: 0.8
|
||||||
|
global_costmap_clear_service: global_costmap/clear_entirely_global_costmap
|
||||||
|
local_costmap_clear_service: local_costmap/clear_entirely_local_costmap
|
||||||
|
max_recovery_attempts: 2
|
||||||
|
circle_goal_tolerance: 0.50
|
||||||
|
|
||||||
|
# VLM capture:
|
||||||
|
# stop - stop at the VLM waypoint, trigger capture, then continue immediately.
|
||||||
|
# pass_through - keep following the full route and trigger capture while passing nearby.
|
||||||
|
vlm_capture_mode: stop
|
||||||
|
pass_through_vlm_trigger_radius: 0.35
|
||||||
|
|
||||||
|
# /sign4return command values used by existing packages
|
||||||
|
sign_qr_enable: 0
|
||||||
|
sign_qr_disable: 5
|
||||||
|
sign_vlm_trigger: 9
|
||||||
|
sign_profile_normal: 10
|
||||||
|
sign_profile_task2: 11
|
||||||
|
vlm_trigger_repeat_count: 5
|
||||||
|
vlm_trigger_interval_sec: 0.5
|
||||||
|
|
||||||
|
# Candidate waypoint JSONs reuse the saved-point format.
|
||||||
|
# home is intentionally never replaced.
|
||||||
|
candidate_waypoint_json_dir: /home/sunrise/yiliao_ws/src/racing_control/config/waypoints
|
||||||
|
qr_candidate_group_name: qr
|
||||||
|
entry_candidate_group_name: entry
|
||||||
|
# Task-two route candidates are selected by point name only:
|
||||||
|
# clockwise waypoint N -> goal_NNN_1
|
||||||
|
# counterclockwise waypoint N -> goal_NNN_2
|
||||||
|
# The VLM capture point is still selected by vlm_waypoint_number.
|
||||||
|
|
||||||
|
# Pose parameters are flat x/y/yaw-radians triples in frame_id.
|
||||||
|
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.
|
||||||
|
# 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:
|
||||||
|
- 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:
|
||||||
|
- 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]
|
||||||
106
src/racing_control/config/waypoints/main.json
Normal file
106
src/racing_control/config/waypoints/main.json
Normal file
@@ -0,0 +1,106 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-08-07T14:42:49.402Z",
|
||||||
|
"count": 2,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "qr",
|
||||||
|
"captured_at": "2026-08-07T14:42:33.118Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 65.22491304455252,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.333107888975101,
|
||||||
|
"y": 1.028867429995691,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.5389539275964809,
|
||||||
|
"w": 0.8423352443821446
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 65.22491304455252
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "entry",
|
||||||
|
"captured_at": "2026-08-07T14:42:45.961Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 87.09617569507752,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 2.504811372926262,
|
||||||
|
"y": 2.2798075935366624,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.6889631335573172,
|
||||||
|
"w": 0.7247963856138373
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 87.09617569507752
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/racing_control/config/waypoints/main_1.json
Normal file
155
src/racing_control/config/waypoints/main_1.json
Normal file
@@ -0,0 +1,155 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-08-07T14:44:10.775Z",
|
||||||
|
"count": 3,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_001_1",
|
||||||
|
"captured_at": "2026-08-07T14:43:55.328Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 89.33764149250207,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 0.7534956931109804,
|
||||||
|
"y": 3.766501234923627,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.7030077953706314,
|
||||||
|
"w": 0.7111821423855668
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 89.33764149250207
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_002_1",
|
||||||
|
"captured_at": "2026-08-07T14:44:02.099Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 2.2906028734862383,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 3.8567885105264077,
|
||||||
|
"y": 4.329424195458402,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.019987949834902125,
|
||||||
|
"w": 0.9998002209748693
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 2.2906028734862383
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_003_1",
|
||||||
|
"captured_at": "2026-08-07T14:44:09.250Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": -92.25458353863968,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 2.5144339572664096,
|
||||||
|
"y": 2.4337692660037766,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": -0.720881318872454,
|
||||||
|
"w": 0.6930585286256215
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": -92.25458353863968
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
155
src/racing_control/config/waypoints/main_2.json
Normal file
155
src/racing_control/config/waypoints/main_2.json
Normal file
@@ -0,0 +1,155 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-08-07T14:43:29.485Z",
|
||||||
|
"count": 3,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "goal_001_2",
|
||||||
|
"captured_at": "2026-08-07T14:43:14.866Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 88.52360610794565,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.2128251001861265,
|
||||||
|
"y": 3.7376334819031842,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.6979380047775942,
|
||||||
|
"w": 0.7161581818893582
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 88.52360610794565
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_002_2",
|
||||||
|
"captured_at": "2026-08-07T14:43:20.968Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 178.26427158937722,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 1.1720794040064113,
|
||||||
|
"y": 4.319801449605877,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.99988528505826,
|
||||||
|
"w": 0.015146508639358486
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 178.26427158937722
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "goal_003_2",
|
||||||
|
"captured_at": "2026-08-07T14:43:28.294Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": -88.21009502116203,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 2.4951887885861144,
|
||||||
|
"y": 2.318297930897253,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": -0.6959760577153663,
|
||||||
|
"w": 0.7180649880665239
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": -88.21009502116203
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
204
src/racing_control/config/waypoints/rest.json
Normal file
204
src/racing_control/config/waypoints/rest.json
Normal file
@@ -0,0 +1,204 @@
|
|||||||
|
{
|
||||||
|
"topic": "/odom_combined",
|
||||||
|
"message_type": "nav_msgs/msg/Odometry",
|
||||||
|
"saved_at": "2026-08-12T07:47:27.220Z",
|
||||||
|
"count": 4,
|
||||||
|
"points": [
|
||||||
|
{
|
||||||
|
"name": "qr",
|
||||||
|
"captured_at": "2026-08-12T07:13:09.418Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 74.93151184050782,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.507415254237288,
|
||||||
|
"y": 0.7213320974576272,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.6082871552778868,
|
||||||
|
"w": 0.7937170381968224
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 74.93151184050782
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "qr",
|
||||||
|
"captured_at": "2026-08-12T07:13:22.097Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 59.82647997035565,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 4.375,
|
||||||
|
"y": 1.2403998940677967,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.4986880501001949,
|
||||||
|
"w": 0.8667815345790804
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 59.82647997035565
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "entry",
|
||||||
|
"captured_at": "2026-08-12T07:13:32.736Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 87.99044618697887,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 2.791313559322034,
|
||||||
|
"y": 2.50099311440678,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.6945983947098336,
|
||||||
|
"w": 0.7193977134148553
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 87.99044618697887
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"name": "entry",
|
||||||
|
"captured_at": "2026-08-12T07:13:41.872Z",
|
||||||
|
"received_at": null,
|
||||||
|
"yaw_degrees": 87.79740183823421,
|
||||||
|
"odom": {
|
||||||
|
"header": {
|
||||||
|
"frame_id": "odom",
|
||||||
|
"stamp": {
|
||||||
|
"sec": 0,
|
||||||
|
"nanosec": 0
|
||||||
|
},
|
||||||
|
"stamp_iso": null
|
||||||
|
},
|
||||||
|
"child_frame_id": "base_link",
|
||||||
|
"pose": {
|
||||||
|
"pose": {
|
||||||
|
"position": {
|
||||||
|
"x": 2.3146186440677967,
|
||||||
|
"y": 2.8028998940677967,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"orientation": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0.6933854908702647,
|
||||||
|
"w": 0.7205668331602574
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"twist": {
|
||||||
|
"twist": {
|
||||||
|
"linear": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
},
|
||||||
|
"angular": {
|
||||||
|
"x": 0,
|
||||||
|
"y": 0,
|
||||||
|
"z": 0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"covariance": []
|
||||||
|
},
|
||||||
|
"yaw_degrees": 87.79740183823421
|
||||||
|
}
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user