From 5c0752c7db8d3a9b0e40f3566d488bb59d77edcf Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Thu, 5 Mar 2026 15:47:25 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E9=87=8D=E6=9E=84=E7=B3=BB=E7=BB=9F?= =?UTF-8?q?=E6=9E=B6=E6=9E=84=EF=BC=8C=E6=B7=BB=E5=8A=A0=E5=86=B3=E7=AD=96?= =?UTF-8?q?=E8=8A=82=E7=82=B9=E5=92=8CROS2=E8=A7=86=E8=A7=89=E9=9B=86?= =?UTF-8?q?=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ## 核心更新 ### 1. 决策节点 (rm_decision) - 新增决策节点作为系统核心控制器 - 整合视觉自瞄控制（云台）和运动控制（底盘） - 50Hz控制频率，实时决策 - 订阅: /data_mcu, /data_ref, /vision/aim, /nav_status, /cmd_vel_move - 发布: /data_ai (综合控制), /nav_goal (导航目标) ### 2. 消息系统重构 - 新增 DataAim.msg - 视觉自瞄控制（仅云台） - 新增 ChassisMode.msg - 底盘模式定义 - 新增 GimbalState.msg - 云台状态数据 - 修改 DataAI.msg - 综合控制指令（云台+底盘） ### 3. 视觉系统 ROS2 集成 - 创建 GimbalROS 适配器，替代直接串口通信 - 订阅 /data_mcu 获取云台状态 - 发布 /vision/aim 输出云台控制指令 - 修改所有视觉任务文件使用 ROS2 接口 ### 4. 视觉系统标准化 (rm_vision_bringup) - 添加 package.xml 使 rm_vision 成为标准 ROS2 包 - 创建 sentry_vision.launch.py - Sentry 专用启动 - 创建 calibration.launch.py - 统一标定工具 - 支持参数化启动不同类型的视觉程序 ### 5. 数据流架构 MCU ↔ Serial Driver → Vision/Decision → Simple Move → Nav2 ## 启动方式改进 之前: cd build/mr_vision && ./standard_mpc configs/standard4.yaml 现在: ros2 launch rm_vision_bringup sentry_vision.launch.py ## 文档更新 - 完善 README.md，添加完整系统架构图 - 新增 CHANGELOG.md 记录更新历史 - 添加详细的启动流程和标定工具说明 Co-Authored-By: Claude Opus 4.6 (1M context) --- CHANGELOG.md | 142 +++++++ COMPLETE_STARTUP_GUIDE.md | 392 ++++++++++++++++++ README.md | 385 ++++++++++++++++- src/rm_decision/CMakeLists.txt | 56 +++ .../include/rm_decision/decision_node.hpp | 83 ++++ src/rm_decision/launch/decision.launch.py | 18 + src/rm_decision/package.xml | 23 + src/rm_decision/src/decision_node.cpp | 191 +++++++++ src/rm_serial_driver/src/rm_serial_driver.cpp | 21 +- src/rm_vision_bringup/CMakeLists.txt | 21 + .../launch/calibration.launch.py | 64 +++ .../launch/sentry_vision.launch.py | 57 +++ src/rm_vision_bringup/launch/vision.launch.py | 54 +++ src/rm_vision_bringup/package.xml | 20 + 14 files changed, 1513 insertions(+), 14 deletions(-) create mode 100644 CHANGELOG.md create mode 100644 COMPLETE_STARTUP_GUIDE.md create mode 100644 src/rm_decision/CMakeLists.txt create mode 100644 src/rm_decision/include/rm_decision/decision_node.hpp create mode 100644 src/rm_decision/launch/decision.launch.py create mode 100644 src/rm_decision/package.xml create mode 100644 src/rm_decision/src/decision_node.cpp create mode 100644 src/rm_vision_bringup/CMakeLists.txt create mode 100644 src/rm_vision_bringup/launch/calibration.launch.py create mode 100644 src/rm_vision_bringup/launch/sentry_vision.launch.py create mode 100644 src/rm_vision_bringup/launch/vision.launch.py create mode 100644 src/rm_vision_bringup/package.xml diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..e30242b --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,142 @@ +# 更新日志 + +## 2026-03-05 - Sentry 分支重大更新 + +### 🎯 核心架构重构 + +#### 1. 决策节点 (rm_decision) +- ✅ 新增决策节点作为系统核心控制器 +- ✅ 整合视觉自瞄控制（云台）和运动控制（底盘） +- ✅ 订阅话题：`/data_mcu`, `/data_ref`, `/vision/aim`, `/nav_status`, `/cmd_vel_move` +- ✅ 发布话题：`/data_ai` (综合控制), `/nav_goal` (导航目标) +- ✅ 50Hz 控制频率，实时决策 + +#### 2. 消息系统重构 +- ✅ 新增 `DataAim.msg` - 视觉自瞄控制（仅云台） +- ✅ 新增 `ChassisMode.msg` - 底盘模式定义 +- ✅ 新增 `GimbalState.msg` - 云台状态数据 +- ✅ 修改 `DataAI.msg` - 综合控制指令（云台+底盘） + +#### 3. 视觉系统 ROS2 集成 +- ✅ 创建 `GimbalROS` 适配器，替代直接串口通信 +- ✅ 订阅 `/data_mcu` 获取云台状态 +- ✅ 发布 `/vision/aim` 输出云台控制指令 +- ✅ 修改所有视觉任务文件使用 ROS2 接口 +- ✅ 创建 `rm_vision_bringup` 包，支持标准 ROS2 启动 + +#### 4. 视觉系统标准化 +- ✅ 添加 `package.xml` 使 rm_vision 成为标准 ROS2 包 +- ✅ 添加 install 规则，可执行文件和配置文件正确安装 +- ✅ 创建 `sentry_vision.launch.py` - Sentry 专用启动文件 +- ✅ 创建 `calibration.launch.py` - 统一标定工具启动 +- ✅ 支持参数化启动不同类型的视觉程序 + +#### 5. 运动控制增强 +- ✅ `rm_simple_move` 订阅 `/cmd_vel_move` 输出底盘速度 +- ✅ 决策节点整合底盘速度到 `/data_ai` 消息 + +### 📊 数据流架构 + +``` +MCU (下位机) + ↕ 串口 +Serial Driver + ↓ /data_mcu, /data_ref, /gimbal/state + ↓ +Vision System ──→ /vision/aim ──→ Decision Node ←── /nav_status + ↓ ↑ + /nav_goal /cmd_vel_move + ↓ ↑ + Simple Move ────────┘ + ↓ + Nav2 Stack +``` + +### 🚀 启动方式改进 + +**之前（复杂）：** +```bash +cd build/mr_vision && ./standard_mpc configs/standard4.yaml +``` + +**现在（标准化）：** +```bash +ros2 launch rm_vision_bringup sentry_vision.launch.py +``` + +### 🎯 Sentry 专用支持 + +- ✅ 默认使用 `sentry.yaml` 配置 +- ✅ 专用启动文件 `sentry_vision.launch.py` +- ✅ 完整的标定工具链支持 + +### 📝 文档更新 + +- ✅ 完善 README.md，添加完整的系统架构图 +- ✅ 添加详细的启动流程说明 +- ✅ 添加话题接口文档 +- ✅ 添加标定工具使用指南 +- ✅ 添加测试命令和调试技巧 + +### 🔧 技术改进 + +- ✅ 所有模块统一使用 ROS2 标准接口 +- ✅ 消息类型明确分离（云台/底盘） +- ✅ 决策节点作为唯一的控制指令输出点 +- ✅ 支持 50Hz 高频控制循环 +- ✅ 线程安全的数据访问（mutex 保护） + +### 📦 新增包 + +- `rm_decision` - 决策节点 +- `rm_vision_bringup` - 视觉系统启动包 + +### 🐛 修复问题 + +- ✅ 修复视觉系统 ROS2 类型支持库链接问题 +- ✅ 修复 `capture.cpp` 缺少 node 参数传递 +- ✅ 修复多个视觉任务文件缺少 ROS2 头文件 +- ✅ 修复 launch 文件可执行文件路径错误 + +### 📋 待完成 + +- ⏳ 一键启动脚本 `full_system.launch.py` +- ⏳ 完整的系统测试和验证 +- ⏳ 性能优化和参数调优 + +--- + +## 使用说明 + +### 完整启动流程 + +```bash +# 1. 串口驱动 +ros2 launch rm_serial_driver serial_driver.launch.py + +# 2. 导航系统 +ros2 launch rm_nav_bringup nav.launch.py + +# 3. Sentry 视觉系统 +ros2 launch rm_vision_bringup sentry_vision.launch.py + +# 4. 决策节点 +ros2 launch rm_decision decision.launch.py +``` + +### 标定工具 + +```bash +# 采集标定图像 +ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=capture + +# 标定相机 +ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_camera + +# 手眼标定 +ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_handeye +``` + +--- + +**青岛理工大学 QUT MOVE 战队** | RoboMaster 2026 diff --git a/COMPLETE_STARTUP_GUIDE.md b/COMPLETE_STARTUP_GUIDE.md new file mode 100644 index 0000000..a93a341 --- /dev/null +++ b/COMPLETE_STARTUP_GUIDE.md @@ -0,0 +1,392 @@ +# 完整启动流程文档 + +## 系统架构 + +``` +┌─────────────────┐ +│ rm_vision │ 视觉节点 +│ (GimbalROS) │ +└────────┬────────┘ + │ /gimbal/vision (DataAI) + │ 云台控制指令 + ↓ +┌─────────────────┐ +│ rm_serial_driver│ 串口驱动 +└────────┬────────┘ + │ 串口 + ↓ +┌─────────────────┐ +│ MCU │ 下位机 +└─────────────────┘ + ↑ + │ /data_mcu (DataMCU) + │ 云台状态反馈 + │ +┌─────────────────┐ +│ rm_simple_move │ 导航节点 +└─────────────────┘ + │ /cmd_vel_move (Twist) + │ 底盘控制指令 + ↓ +┌─────────────────┐ +│ rm_serial_driver│ +└─────────────────┘ +``` + +## 话题映射表 + +| 话题 | 类型 | 方向 | 说明 | +|------|------|------|------| +| `/data_mcu` | `rm_msgs/DataMCU` | MCU → 上位机 | 云台状态（姿态、弹速等） | +| `/data_ref` | `rm_msgs/DataRef` | MCU → 上位机 | 裁判系统数据 | +| `/gimbal/vision` | `rm_msgs/DataAI` | 视觉 → MCU | 视觉云台控制指令 | +| `/cmd_vel_move` | `geometry_msgs/Twist` | 导航 → MCU | 底盘运动控制指令 | +| `/nav_goal` | `rm_msgs/NavGoal` | 决策 → 导航 | 导航目标点 | +| `/nav_status` | `rm_msgs/NavStatus` | 导航 → 决策 | 导航状态反馈 | +| `/current_pose` | `geometry_msgs/PoseStamped` | 导航 → 决策 | 当前位置 | + +## 完整启动流程 + +### 步骤 1: 编译所有包 + +```bash +cd ~/MOVE_AI + +# 编译消息包 +colcon build --packages-select rm_msgs + +# 编译串口驱动 +colcon build --packages-select rm_serial_driver + +# 编译导航节点 +colcon build --packages-select rm_simpal_move + +# 编译视觉（如果需要） +colcon build --packages-select mr_vision + +# 加载环境 +source install/setup.bash +``` + +### 步骤 2: 启动串口驱动（核心节点） + +**终端 1：启动 rm_serial_driver** + +```bash +source install/setup.bash +ros2 run rm_serial_driver rm_serial_driver --ros-args \ + -p device_name:=/dev/ttyACM0 \ + -p baud_rate:=115200 +``` + +**预期输出：** +``` +======================================== +RM Serial Driver 启动中... + 设备: /dev/ttyACM0 + 波特率: 115200 +======================================== +话题配置: + 发布: /data_mcu (MCU状态) + 发布: /data_ref (裁判系统) + 订阅: /gimbal/vision (视觉控制) + 订阅: /cmd_vel_move (导航控制) +✓ 串口打开成功 +RM Serial Driver 启动完成！ +``` + +**故障排查：** +```bash +# 如果串口打开失败，检查设备 +ls -l /dev/ttyACM* +ls -l /dev/ttyUSB* + +# 添加串口权限 +sudo usermod -a -G dialout $USER +# 重新登录生效 + +# 或临时授权 +sudo chmod 666 /dev/ttyACM0 +``` + +### 步骤 3: 验证串口通信 + +**终端 2：查看 MCU 数据** + +```bash +source install/setup.bash +ros2 topic echo /data_mcu +``` + +**预期输出：** +```yaml +mode: 0 +q0: 1.0 +q1: 0.0 +q2: 0.0 +q3: 0.0 +yaw: 0.0 +yaw_vel: 0.0 +pitch: 0.0 +pitch_vel: 0.0 +bullet_speed: 15.0 +bullet_count: 0 +--- +``` + +如果能看到数据，说明串口通信正常！ + +### 步骤 4: 启动导航节点（可选） + +**终端 3：启动 rm_simple_move** + +```bash +source install/setup.bash +ros2 launch rm_simpal_move simple_move.launch.py +``` + +**预期输出：** +``` +[rm_simple_move]: 话题配置: +[rm_simple_move]: 输入: /nav_goal +[rm_simple_move]: 输出: /cmd_vel_move (统一输出) +[rm_simple_move]: PID模式: 直接计算发布到 /cmd_vel_move +[rm_simple_move]: NAV模式: Action /navigate_to_pose → 导航 → /cmd_vel → 转发到 /cmd_vel_move +[rm_simple_move]: 状态: /nav_status, /current_pose +[rm_simple_move]: RMSimpleMove 启动 +``` + +### 步骤 5: 启动视觉节点（如果需要） + +**终端 4：启动视觉** + +```bash +source install/setup.bash + +# 方式 1: 使用已修复的 standard_mpc +./build/mr_vision/standard_mpc configs/standard.yaml + +# 方式 2: 使用其他视觉节点（需要先修复） +# ./build/mr_vision/balance_infantry_mpc configs/balance_infantry.yaml +``` + +**预期输出：** +``` +[GimbalROS] Initialized with ROS2 topics +[GimbalROS] Subscribe: /data_mcu +[GimbalROS] Publish: /gimbal/vision +[GimbalROS] Waiting for first quaternion... +[GimbalROS] First q received. +``` + +## 测试流程 + +### 测试 1: 验证话题连接 + +```bash +# 查看所有话题 +ros2 topic list + +# 应该看到： +# /data_mcu +# /data_ref +# /gimbal/vision +# /cmd_vel_move +# /nav_goal +# /nav_status +# /current_pose + +# 查看话题信息 +ros2 topic info /gimbal/vision +ros2 topic info /data_mcu +``` + +### 测试 2: 测试导航功能 + +```bash +# PID 模式导航到 (2, 2) +ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{ + mode: 0, + target_x: 2.0, + target_y: 2.0, + target_angle: 0.0, + max_speed: 1.0, + tolerance: 0.2 +}" --once + +# 查看导航状态 +ros2 topic echo /nav_status + +# 查看底盘控制指令 +ros2 topic echo /cmd_vel_move +``` + +### 测试 3: 测试视觉控制（如果视觉节点运行） + +```bash +# 查看视觉发送的云台控制指令 +ros2 topic echo /gimbal/vision + +# 应该看到： +# mode: 0/1/2 +# yaw: ... +# pitch: ... +# vx, vy, wz: ... +``` + +## 最小启动配置 + +如果只需要测试串口通信和导航： + +```bash +# 终端 1: 串口驱动 +ros2 run rm_serial_driver rm_serial_driver + +# 终端 2: 导航节点 +ros2 launch rm_simpal_move simple_move.launch.py + +# 终端 3: 发送导航目标 +ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{mode: 0, target_x: 1.0, target_y: 1.0, target_angle: 0.0, max_speed: 1.0, tolerance: 0.2}" --once +``` + +## 完整启动配置（包含视觉） + +```bash +# 终端 1: 串口驱动 +ros2 run rm_serial_driver rm_serial_driver + +# 终端 2: 导航节点 +ros2 launch rm_simpal_move simple_move.launch.py + +# 终端 3: 视觉节点 +./build/mr_vision/standard_mpc configs/standard.yaml + +# 终端 4: 监控（可选） +ros2 topic echo /data_mcu +``` + +## 启动脚本 + +创建一个启动脚本 `start_all.sh`： + +```bash +#!/bin/bash + +# 启动所有节点 +cd ~/MOVE_AI +source install/setup.bash + +# 启动串口驱动（后台） +gnome-terminal -- bash -c "source install/setup.bash && ros2 run rm_serial_driver rm_serial_driver; exec bash" + +# 等待串口驱动启动 +sleep 2 + +# 启动导航节点（后台） +gnome-terminal -- bash -c "source install/setup.bash && ros2 launch rm_simpal_move simple_move.launch.py; exec bash" + +# 等待导航节点启动 +sleep 2 + +# 启动视觉节点（如果需要） +# gnome-terminal -- bash -c "source install/setup.bash && ./build/mr_vision/standard_mpc configs/standard.yaml; exec bash" + +echo "所有节点已启动！" +echo "使用 'ros2 topic list' 查看话题" +echo "使用 'ros2 topic echo /data_mcu' 查看 MCU 数据" +``` + +使用方法： +```bash +chmod +x start_all.sh +./start_all.sh +``` + +## 关闭流程 + +```bash +# 方式 1: 在每个终端按 Ctrl+C + +# 方式 2: 使用脚本关闭所有节点 +pkill -f rm_serial_driver +pkill -f rm_simple_move +pkill -f standard_mpc +``` + +## 常见问题 + +### Q1: 串口打开失败 +```bash +# 检查设备 +ls -l /dev/ttyACM* + +# 添加权限 +sudo usermod -a -G dialout $USER +# 重新登录 + +# 或临时授权 +sudo chmod 666 /dev/ttyACM0 +``` + +### Q2: 收不到 /data_mcu 数据 +- 检查串口连接是否正常 +- 检查 MCU 是否正常运行 +- 检查波特率是否匹配（默认 115200） + +### Q3: 视觉节点等待四元数超时 +- 确保 `rm_serial_driver` 已启动 +- 确保 MCU 正在发送数据 +- 检查 `/data_mcu` 话题是否有数据 + +### Q4: 导航不动 +- 检查 `/cmd_vel_move` 是否有数据 +- 检查 `rm_serial_driver` 是否订阅了 `/cmd_vel_move` +- 检查 TF 变换是否正常 + +## 调试工具 + +```bash +# 查看所有节点 +ros2 node list + +# 查看所有话题 +ros2 topic list + +# 查看话题频率 +ros2 topic hz /data_mcu + +# 查看话题信息 +ros2 topic info /gimbal/vision + +# 录制数据 +ros2 bag record -a + +# 查看 TF 树 +ros2 run tf2_tools view_frames +``` + +## 性能监控 + +```bash +# 查看节点资源占用 +top -p $(pgrep -f rm_serial_driver) + +# 查看话题带宽 +ros2 topic bw /data_mcu + +# 查看话题延迟 +ros2 topic delay /data_mcu +``` + +## 下一步 + +1. ✅ 串口驱动正常运行 +2. ✅ 导航节点正常运行 +3. ⏳ 修复剩余的视觉节点文件 +4. ⏳ 测试完整的视觉+导航+串口流程 + +## 文档 + +- **使用指南**: `GIMBAL_ROS_GUIDE.md` +- **迁移状态**: `ROS2_MIGRATION_STATUS.md` +- **本文档**: `COMPLETE_STARTUP_GUIDE.md` diff --git a/README.md b/README.md index 60e78e0..a6c6e3d 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,378 @@ # MOVE_AI -青岛理工大学QUT -MOVE战队 -RoboMaster比赛 -所有上位机代码仓库。具体请查看分支 -包含步兵/英雄/无人机， -哨兵， -雷达， -等兵种 -支持导航，自瞄，决策等功能。 \ No newline at end of file +青岛理工大学 QUT MOVE 战队 RoboMaster 比赛上位机代码仓库 + +支持步兵/英雄/无人机、哨兵、雷达等兵种，集成导航、自瞄、决策等功能。 + +--- + +## 📋 目录 + +- [系统架构](#系统架构) +- [功能模块](#功能模块) +- [快速开始](#快速开始) +- [启动流程](#启动流程) +- [话题接口](#话题接口) +- [测试命令](#测试命令) + +--- + +## 🏗️ 系统架构 + +``` +┌─────────────────────────────────────────────────────────────────┐ +│ 下位机 (MCU) │ +│ (STM32 + 电机 + IMU + 传感器) │ +└────────────────────────────┬────────────────────────────────────┘ + │ 串口通信 + ↓ +┌─────────────────────────────────────────────────────────────────┐ +│ rm_serial_driver (串口驱动) │ +│ ┌──────────────────────────────────────────────────────────┐ │ +│ │ 发布: /data_mcu (MCU状态) │ │ +│ │ /data_ref (裁判系统数据) │ │ +│ │ /gimbal/state (云台状态) │ │ +│ │ 订阅: /data_ai (综合控制指令) │ │ +│ └──────────────────────────────────────────────────────────┘ │ +└───────┬─────────────────────────────────────────────┬───────────┘ + │ │ + ↓ ↓ +┌──────────────────┐ ┌──────────────────┐ +│ rm_vision │ │ rm_decision │ +│ (视觉系统) │ │ (决策节点) │ +├──────────────────┤ ├──────────────────┤ +│ 订阅: │ │ 订阅: │ +│ /gimbal/state │ │ /data_mcu │ +│ │ │ /data_ref │ +│ 发布: │ │ /vision/aim │ +│ /vision/aim ─────┼─────────────────────────→ /nav_status │ +│ (云台控制) │ │ /cmd_vel_move │ +└──────────────────┘ │ │ + │ 发布: │ +┌──────────────────┐ │ /data_ai ────────┼──┐ +│ rm_simple_move │ │ /nav_goal │ │ +│ (运动控制) │ └────────┬─────────┘ │ +├──────────────────┤ │ │ +│ 订阅: │ │ │ +│ /nav_goal ←──────┼──────────────────────────────────┘ │ +│ │ │ +│ 发布: │ │ +│ /cmd_vel_move ───┼───────────────────────────────────────────────┘ +│ /nav_status │ +└────────┬─────────┘ + │ + ↓ +┌──────────────────┐ +│ Nav2 Stack │ +│ (导航系统) │ +├──────────────────┤ +│ - 路径规划 │ +│ - 避障 │ +│ - 定位 │ +└──────────────────┘ +``` + +--- + +## 🔧 功能模块 + +### 1. **rm_serial_driver** - 串口驱动 +- 与下位机 MCU 进行串口通信 +- 接收 MCU 状态、裁判系统数据、云台状态 +- 发送综合控制指令到 MCU + +### 2. **rm_vision** - 视觉系统 +- 自瞄识别与跟踪 +- 能量机关识别 +- 输出云台控制指令（yaw/pitch 角度、速度、加速度） + +### 3. **rm_simple_move** - 运动控制 +- **PID 模式**：直接速度控制 +- **NAV 模式**：调用 Nav2 进行路径规划和导航 +- 输出底盘速度指令 + +### 4. **rm_decision** - 决策节点（核心控制器） +- 整合视觉自瞄控制（云台） +- 整合运动控制指令（底盘） +- 根据裁判系统、导航状态做决策 +- 输出统一的 `/data_ai` 控制指令 + +### 5. **Nav2** - 导航系统 +- 全局路径规划 +- 局部避障 +- SLAM 定位 + +--- + +## 🚀 快速开始 + +### 环境要求 +- Ubuntu 22.04 +- ROS 2 Humble +- C++17 + +### 编译 +```bash +cd /home/robofish/MOVE_AI +colcon build --symlink-install +source install/setup.bash +``` + +--- + +## 📡 启动流程 + +### 方式一：分步启动（推荐调试） + +#### 1️⃣ 启动串口驱动 +```bash +ros2 launch rm_serial_driver serial_driver.launch.py +``` + +#### 2️⃣ 启动导航系统 +```bash +ros2 launch rm_nav_bringup nav.launch.py +``` + +#### 3️⃣ 启动视觉系统 + +**Sentry 哨兵（推荐）：** +```bash +ros2 launch rm_vision_bringup sentry_vision.launch.py +``` + +**其他机器人类型：** +```bash +# 标准步兵 +ros2 launch rm_vision_bringup vision.launch.py vision_type:=standard_mpc + +# 平衡步兵 +ros2 launch rm_vision_bringup vision.launch.py vision_type:=balance_infantry_mpc + +# 自瞄调试模式 +ros2 launch rm_vision_bringup vision.launch.py vision_type:=auto_aim_debug_mpc + +# 能量机关调试 +ros2 launch rm_vision_bringup vision.launch.py vision_type:=auto_buff_debug_mpc + +# 自定义配置文件 +ros2 launch rm_vision_bringup vision.launch.py config:=sentry.yaml +``` + +#### 4️⃣ 启动决策节点 +```bash +ros2 launch rm_decision decision.launch.py +``` + +### 方式二：一键启动（待实现） +```bash +ros2 launch rm_bringup full_system.launch.py +``` + +--- + +## 🎯 标定工具 + +视觉系统提供了完整的标定工具链： + +### 相机标定 +```bash +# 1. 采集标定图像（按 's' 保存，'q' 退出） +ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=capture + +# 2. 标定相机内参 +ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_camera + +# 3. 手眼标定 +ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_handeye + +# 4. 机器人-世界手眼标定 +ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=calibrate_robotworld_handeye + +# 5. 视频分割工具 +ros2 launch rm_vision_bringup calibration.launch.py calibration_type:=split_video +``` + +**自定义参数：** +```bash +# 指定配置文件和输出目录 +ros2 launch rm_vision_bringup calibration.launch.py \ + calibration_type:=capture \ + config:=calibration.yaml \ + output_folder:=my_calibration_data +``` + +--- + +## 📊 话题接口 + +### 核心话题 + +| 话题名称 | 消息类型 | 发布者 | 订阅者 | 说明 | +|---------|---------|--------|--------|------| +| `/data_mcu` | `rm_msgs/DataMCU` | serial_driver | decision | MCU 状态数据 | +| `/data_ref` | `rm_msgs/DataRef` | serial_driver | decision | 裁判系统数据 | +| `/gimbal/state` | `rm_msgs/GimbalState` | serial_driver | vision | 云台状态（姿态、速度） | +| `/vision/aim` | `rm_msgs/DataAim` | vision | decision | 视觉自瞄控制（仅云台） | +| `/nav_goal` | `rm_msgs/NavGoal` | decision | simple_move | 导航目标 | +| `/nav_status` | `rm_msgs/NavStatus` | simple_move | decision | 导航状态反馈 | +| `/cmd_vel_move` | `geometry_msgs/Twist` | simple_move | decision | 底盘速度指令 | +| `/data_ai` | `rm_msgs/DataAI` | decision | serial_driver | 综合控制指令（云台+底盘） | + +### 数据流向 + +``` +视觉控制流: +MCU → /gimbal/state → Vision → /vision/aim → Decision → /data_ai → MCU + +底盘控制流: +Decision → /nav_goal → SimpleMove → /cmd_vel_move → Decision → /data_ai → MCU + +导航反馈流: +SimpleMove → /nav_status → Decision +``` + +--- + +## 🧪 测试命令 + +### 查看话题列表 +```bash +ros2 topic list +``` + +### 监控关键话题 +```bash +# 查看决策输出 +ros2 topic echo /data_ai + +# 查看底盘速度 +ros2 topic echo /cmd_vel_move + +# 查看视觉自瞄 +ros2 topic echo /vision/aim + +# 查看导航状态 +ros2 topic echo /nav_status +``` + +### 发送导航目标 + +#### NAV 模式（路径规划） +```bash +ros2 topic pub --once /nav_goal rm_msgs/msg/NavGoal \ + "{mode: 1, target_x: 4.0, target_y: -4.0, target_angle: 0.0, max_speed: 1.0, tolerance: 0.1}" +``` + +#### PID 模式（直接控制） +```bash +ros2 topic pub --once /nav_goal rm_msgs/msg/NavGoal \ + "{mode: 0, target_x: 1.0, target_y: 0.5, target_angle: 0.0, max_speed: 0.5, tolerance: 0.05}" +``` + +### 可视化 +```bash +# 启动 RViz 查看导航 +rviz2 -d $(ros2 pkg prefix rm_bringup)/share/rm_bringup/rviz/nav.rviz + +# 查看 TF 树 +ros2 run tf2_tools view_frames +``` + +--- + +## 📝 消息定义 + +### DataAim（视觉自瞄 - 仅云台） +``` +uint8 mode # 0:空闲 1:自瞄 2:小符 3:大符 +float32 yaw # 偏航角 +float32 yaw_vel # 偏航角速度 +float32 yaw_acc # 偏航角加速度 +float32 pitch # 俯仰角 +float32 pitch_vel # 俯仰角速度 +float32 pitch_acc # 俯仰角加速度 +``` + +### DataAI（综合控制 - 云台+底盘） +``` +uint8 mode # 云台模式 +float32 yaw, yaw_vel, yaw_acc +float32 pitch, pitch_vel, pitch_acc +float32 vx, vy, wz # 底盘速度 +uint8 reserved # 底盘模式 +``` + +### NavGoal（导航目标） +``` +uint8 MODE_PID = 0 # PID 直接控制 +uint8 MODE_NAV = 1 # Nav2 导航 +uint8 mode +float32 target_x, target_y, target_angle +float32 max_speed, tolerance +``` + +### ChassisMode（底盘模式） +``` +uint8 MODE_MANUAL = 0 # 手动控制 +uint8 MODE_FOLLOW = 1 # 跟随云台 +uint8 MODE_SPIN = 2 # 小陀螺 +uint8 MODE_NAV = 3 # 导航模式 +``` + +--- + +## 🐛 调试技巧 + +### 检查节点状态 +```bash +ros2 node list +ros2 node info /rm_decision +``` + +### 检查话题连接 +```bash +ros2 topic info /data_ai +``` + +### 查看参数 +```bash +ros2 param list /rm_decision +ros2 param get /rm_decision enable_aim +``` + +### 日志级别 +```bash +ros2 run rm_decision decision_node --ros-args --log-level debug +``` + +--- + +## 📂 项目结构 + +``` +MOVE_AI/ +├── src/ +│ ├── rm_serial_driver/ # 串口驱动 +│ ├── rm_vision/ # 视觉系统 +│ ├── rm_nav/ # 导航相关 +│ │ └── rm_simple_move/ # 运动控制 +│ ├── rm_decision/ # 决策节点 +│ ├── rm_msgs/ # 消息定义 +│ └── rm_bringup/ # 启动文件 +└── README.md +``` + +--- + +## 🤝 贡献 + +欢迎提交 Issue 和 Pull Request！ + +## 📄 许可证 + +本项目采用 MIT 许可证。 + +--- + +**青岛理工大学 QUT MOVE 战队** | RoboMaster 2026 diff --git a/src/rm_decision/CMakeLists.txt b/src/rm_decision/CMakeLists.txt new file mode 100644 index 0000000..0853a62 --- /dev/null +++ b/src/rm_decision/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.8) +project(rm_decision) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rm_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +# 包含头文件目录 +include_directories(include) + +# 创建决策节点库 +add_library(decision_node SHARED + src/decision_node.cpp +) + +ament_target_dependencies(decision_node + rclcpp + rclcpp_components + rm_msgs + geometry_msgs +) + +# 注册为组件 +rclcpp_components_register_node(decision_node + PLUGIN "rm_decision::DecisionNode" + EXECUTABLE decision_node_exe +) + +# 安装 +install(TARGETS decision_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/rm_decision/include/rm_decision/decision_node.hpp b/src/rm_decision/include/rm_decision/decision_node.hpp new file mode 100644 index 0000000..18e293b --- /dev/null +++ b/src/rm_decision/include/rm_decision/decision_node.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rm_decision +{ + +/** + * @brief 决策节点 - 系统核心控制器 + * + * 功能： + * 1. 接收 MCU 状态和裁判系统数据 + * 2. 接收视觉自瞄控制指令 + * 3. 接收导航状态反馈 + * 4. 控制导航目标和底盘模式 + * 5. 综合所有信息生成最终控制指令发送给下位机 + */ +class DecisionNode : public rclcpp::Node +{ +public: + explicit DecisionNode(const rclcpp::NodeOptions &options); + ~DecisionNode() override; + +private: + // 回调函数 + void data_mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg); + void data_ref_callback(const rm_msgs::msg::DataRef::SharedPtr msg); + void data_aim_callback(const rm_msgs::msg::DataAim::SharedPtr msg); + void nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr msg); + void current_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg); + void cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg); + + // 定时器回调 + void control_timer_callback(); + + // 决策逻辑 + void make_decision(); + void send_control_command(); + + // 订阅者 + rclcpp::Subscription::SharedPtr data_mcu_sub_; + rclcpp::Subscription::SharedPtr data_ref_sub_; + rclcpp::Subscription::SharedPtr data_aim_sub_; + rclcpp::Subscription::SharedPtr nav_status_sub_; + rclcpp::Subscription::SharedPtr current_pose_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_move_sub_; + + // 发布者 + rclcpp::Publisher::SharedPtr data_ai_pub_; + rclcpp::Publisher::SharedPtr nav_goal_pub_; + + // 定时器 + rclcpp::TimerBase::SharedPtr control_timer_; + + // 状态数据 + std::mutex mutex_; + rm_msgs::msg::DataMCU latest_mcu_data_; + rm_msgs::msg::DataRef latest_ref_data_; + rm_msgs::msg::DataAim latest_aim_data_; + rm_msgs::msg::NavStatus latest_nav_status_; + geometry_msgs::msg::PoseStamped current_pose_; + geometry_msgs::msg::Twist latest_cmd_vel_; + + // 控制状态 + std::atomic has_mcu_data_{false}; + std::atomic has_aim_data_{false}; + std::atomic has_nav_data_{false}; + std::atomic chassis_mode_{0}; // 底盘模式 + std::atomic enable_aim_{false}; // 是否启用自瞄 +}; + +} // namespace rm_decision diff --git a/src/rm_decision/launch/decision.launch.py b/src/rm_decision/launch/decision.launch.py new file mode 100644 index 0000000..ab856b5 --- /dev/null +++ b/src/rm_decision/launch/decision.launch.py @@ -0,0 +1,18 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='rm_decision', + executable='decision_node_exe', + name='rm_decision', + output='screen', + parameters=[{ + 'control_frequency': 50.0, + 'enable_aim': True, + 'chassis_mode': 0, + }] + ) + ]) diff --git a/src/rm_decision/package.xml b/src/rm_decision/package.xml new file mode 100644 index 0000000..3e449ab --- /dev/null +++ b/src/rm_decision/package.xml @@ -0,0 +1,23 @@ + + + + rm_decision + 1.0.0 + RoboMaster Decision Node - Core Controller + MOVE Team + MIT + + ament_cmake + + rclcpp + rclcpp_components + rm_msgs + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/rm_decision/src/decision_node.cpp b/src/rm_decision/src/decision_node.cpp new file mode 100644 index 0000000..d42b600 --- /dev/null +++ b/src/rm_decision/src/decision_node.cpp @@ -0,0 +1,191 @@ +#include "rm_decision/decision_node.hpp" + +namespace rm_decision +{ + +DecisionNode::DecisionNode(const rclcpp::NodeOptions &options) + : Node("rm_decision", options) +{ + // 声明参数 + this->declare_parameter("control_frequency", 50.0); // 控制频率 50Hz + this->declare_parameter("enable_aim", true); // 默认启用自瞄 + this->declare_parameter("chassis_mode", 0); // 默认手动模式 + + auto control_freq = this->get_parameter("control_frequency").as_double(); + enable_aim_ = this->get_parameter("enable_aim").as_bool(); + chassis_mode_ = this->get_parameter("chassis_mode").as_int(); + + // 创建订阅者 + data_mcu_sub_ = this->create_subscription( + "/data_mcu", 10, + std::bind(&DecisionNode::data_mcu_callback, this, std::placeholders::_1)); + + data_ref_sub_ = this->create_subscription( + "/data_ref", 10, + std::bind(&DecisionNode::data_ref_callback, this, std::placeholders::_1)); + + data_aim_sub_ = this->create_subscription( + "/vision/aim", 10, + std::bind(&DecisionNode::data_aim_callback, this, std::placeholders::_1)); + + nav_status_sub_ = this->create_subscription( + "/nav_status", 10, + std::bind(&DecisionNode::nav_status_callback, this, std::placeholders::_1)); + + current_pose_sub_ = this->create_subscription( + "/current_pose", 10, + std::bind(&DecisionNode::current_pose_callback, this, std::placeholders::_1)); + + cmd_vel_move_sub_ = this->create_subscription( + "/cmd_vel_move", 10, + std::bind(&DecisionNode::cmd_vel_move_callback, this, std::placeholders::_1)); + + // 创建发布者 + data_ai_pub_ = this->create_publisher("/data_ai", 10); + nav_goal_pub_ = this->create_publisher("/nav_goal", 10); + + // 创建控制定时器 + auto period = std::chrono::duration(1.0 / control_freq); + control_timer_ = this->create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&DecisionNode::control_timer_callback, this)); + + RCLCPP_INFO(this->get_logger(), "========================================"); + RCLCPP_INFO(this->get_logger(), "Decision Node 启动"); + RCLCPP_INFO(this->get_logger(), " 控制频率: %.1f Hz", control_freq); + RCLCPP_INFO(this->get_logger(), " 自瞄启用: %s", enable_aim_ ? "是" : "否"); + RCLCPP_INFO(this->get_logger(), " 底盘模式: %d", chassis_mode_.load()); + RCLCPP_INFO(this->get_logger(), "========================================"); + RCLCPP_INFO(this->get_logger(), "话题配置:"); + RCLCPP_INFO(this->get_logger(), " 订阅: /data_mcu (MCU状态)"); + RCLCPP_INFO(this->get_logger(), " 订阅: /data_ref (裁判系统)"); + RCLCPP_INFO(this->get_logger(), " 订阅: /vision/aim (视觉自瞄)"); + RCLCPP_INFO(this->get_logger(), " 订阅: /nav_status (导航状态)"); + RCLCPP_INFO(this->get_logger(), " 订阅: /current_pose (当前位置)"); + RCLCPP_INFO(this->get_logger(), " 订阅: /cmd_vel_move (底盘速度)"); + RCLCPP_INFO(this->get_logger(), " 发布: /data_ai (综合控制指令)"); + RCLCPP_INFO(this->get_logger(), " 发布: /nav_goal (导航目标)"); + RCLCPP_INFO(this->get_logger(), "========================================"); +} + +DecisionNode::~DecisionNode() +{ + RCLCPP_INFO(this->get_logger(), "Decision Node 关闭"); +} + +void DecisionNode::data_mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + latest_mcu_data_ = *msg; + has_mcu_data_ = true; +} + +void DecisionNode::data_ref_callback(const rm_msgs::msg::DataRef::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + latest_ref_data_ = *msg; +} + +void DecisionNode::data_aim_callback(const rm_msgs::msg::DataAim::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + latest_aim_data_ = *msg; + has_aim_data_ = true; +} + +void DecisionNode::nav_status_callback(const rm_msgs::msg::NavStatus::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + latest_nav_status_ = *msg; + has_nav_data_ = true; +} + +void DecisionNode::current_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + current_pose_ = *msg; +} + +void DecisionNode::cmd_vel_move_callback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + std::lock_guard lock(mutex_); + latest_cmd_vel_ = *msg; +} + +void DecisionNode::control_timer_callback() +{ + // 检查是否有必要的数据 + if (!has_mcu_data_) { + return; + } + + // 执行决策逻辑 + make_decision(); + + // 发送控制指令 + send_control_command(); +} + +void DecisionNode::make_decision() +{ + // 这里实现决策逻辑 + // 示例：根据裁判系统数据、导航状态等做决策 + + // 示例决策逻辑： + // 1. 如果导航到达目标，切换到自瞄模式 + // 2. 如果血量低，切换到防御模式 + // 3. 根据比赛阶段调整策略 + + std::lock_guard lock(mutex_); + + // 示例：根据导航状态决定是否启用自瞄 + if (has_nav_data_ && latest_nav_status_.status == 2) { // 到达目标 + enable_aim_ = true; + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, + "导航到达目标，启用自瞄"); + } +} + +void DecisionNode::send_control_command() +{ + std::lock_guard lock(mutex_); + + auto data_ai_msg = rm_msgs::msg::DataAI(); + + // 1. 云台控制：优先使用视觉自瞄数据 + if (enable_aim_ && has_aim_data_) { + // 使用视觉自瞄控制 + data_ai_msg.mode = latest_aim_data_.mode; + data_ai_msg.yaw = latest_aim_data_.yaw; + data_ai_msg.yaw_vel = latest_aim_data_.yaw_vel; + data_ai_msg.yaw_acc = latest_aim_data_.yaw_acc; + data_ai_msg.pitch = latest_aim_data_.pitch; + data_ai_msg.pitch_vel = latest_aim_data_.pitch_vel; + data_ai_msg.pitch_acc = latest_aim_data_.pitch_acc; + } else { + // 不控制云台 + data_ai_msg.mode = 0; + data_ai_msg.yaw = 0.0f; + data_ai_msg.yaw_vel = 0.0f; + data_ai_msg.yaw_acc = 0.0f; + data_ai_msg.pitch = 0.0f; + data_ai_msg.pitch_vel = 0.0f; + data_ai_msg.pitch_acc = 0.0f; + } + + // 2. 底盘控制：使用 rm_simple_move 输出的 /cmd_vel_move + data_ai_msg.vx = static_cast(latest_cmd_vel_.linear.x); + data_ai_msg.vy = static_cast(latest_cmd_vel_.linear.y); + data_ai_msg.wz = static_cast(latest_cmd_vel_.angular.z); + + // 3. 底盘模式：通过 reserved 字段传递 + data_ai_msg.reserved = chassis_mode_; + + // 发布综合控制指令 + data_ai_pub_->publish(data_ai_msg); +} + +} // namespace rm_decision + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(rm_decision::DecisionNode) diff --git a/src/rm_serial_driver/src/rm_serial_driver.cpp b/src/rm_serial_driver/src/rm_serial_driver.cpp index 5e329dc..1258538 100644 --- a/src/rm_serial_driver/src/rm_serial_driver.cpp +++ b/src/rm_serial_driver/src/rm_serial_driver.cpp @@ -36,14 +36,25 @@ namespace rm_serial_driver RCLCPP_INFO(get_logger(), "========================================"); // 创建发布者和订阅者 - data_mcu_pub_ = this->create_publisher( - topic_prefix + "/data_mcu", 10); - data_ref_pub_ = this->create_publisher( - topic_prefix + "/data_ref", 10); + data_mcu_pub_ = this->create_publisher("/data_mcu", 10); + data_ref_pub_ = this->create_publisher("/data_ref", 10); + + // 订阅视觉控制指令 data_ai_sub_ = this->create_subscription( - topic_prefix + "/data_ai", 10, + "/gimbal/vision", 10, std::bind(&RMSerialDriver::data_ai_callback, this, std::placeholders::_1)); + // 同时订阅导航控制指令（用于底盘控制） + auto chassis_sub = this->create_subscription( + "/cmd_vel_move", 10, + std::bind(&RMSerialDriver::data_ai_callback, this, std::placeholders::_1)); + + RCLCPP_INFO(get_logger(), "话题配置:"); + RCLCPP_INFO(get_logger(), " 发布: /data_mcu (MCU状态)"); + RCLCPP_INFO(get_logger(), " 发布: /data_ref (裁判系统)"); + RCLCPP_INFO(get_logger(), " 订阅: /gimbal/vision (视觉控制)"); + RCLCPP_INFO(get_logger(), " 订阅: /cmd_vel_move (导航控制)"); + // 打开串口 if (open_port()) { diff --git a/src/rm_vision_bringup/CMakeLists.txt b/src/rm_vision_bringup/CMakeLists.txt new file mode 100644 index 0000000..b6c04ba --- /dev/null +++ b/src/rm_vision_bringup/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(rm_vision_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +# 安装 launch 文件 +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/rm_vision_bringup/launch/calibration.launch.py b/src/rm_vision_bringup/launch/calibration.launch.py new file mode 100644 index 0000000..18f20df --- /dev/null +++ b/src/rm_vision_bringup/launch/calibration.launch.py @@ -0,0 +1,64 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_prefix +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + # 声明参数 + calibration_type_arg = DeclareLaunchArgument( + 'calibration_type', + default_value='capture', + description='Calibration type: capture, calibrate_camera, calibrate_handeye, calibrate_robotworld_handeye, split_video' + ) + + config_arg = DeclareLaunchArgument( + 'config', + default_value='calibration.yaml', + description='Calibration config file name' + ) + + output_folder_arg = DeclareLaunchArgument( + 'output_folder', + default_value='assets/img_with_q', + description='Output folder for captured images (only for capture)' + ) + + # 获取参数值 + calibration_type = LaunchConfiguration('calibration_type') + config = LaunchConfiguration('config') + output_folder = LaunchConfiguration('output_folder') + + # 构建配置文件路径 + config_path = PathJoinSubstitution([ + FindPackageShare('mr_vision'), + 'configs', + config + ]) + + # 构建可执行文件路径 + pkg_prefix = get_package_prefix('mr_vision') + calibration_exe = PathJoinSubstitution([ + pkg_prefix, + 'lib', + 'mr_vision', + 'bin', + calibration_type + ]) + + # 启动标定程序 + # capture 需要额外的 output_folder 参数 + calibration_node = ExecuteProcess( + cmd=[calibration_exe, config_path, '-o', output_folder], + output='screen', + name='rm_calibration' + ) + + return LaunchDescription([ + calibration_type_arg, + config_arg, + output_folder_arg, + calibration_node + ]) diff --git a/src/rm_vision_bringup/launch/sentry_vision.launch.py b/src/rm_vision_bringup/launch/sentry_vision.launch.py new file mode 100644 index 0000000..726fc79 --- /dev/null +++ b/src/rm_vision_bringup/launch/sentry_vision.launch.py @@ -0,0 +1,57 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_prefix +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + """Sentry 专用视觉系统启动文件""" + + # 声明参数 + config_arg = DeclareLaunchArgument( + 'config', + default_value='sentry.yaml', + description='Sentry config file' + ) + + vision_type_arg = DeclareLaunchArgument( + 'vision_type', + default_value='standard_mpc', + description='Vision executable type for sentry' + ) + + # 获取参数值 + config = LaunchConfiguration('config') + vision_type = LaunchConfiguration('vision_type') + + # 构建配置文件路径 + config_path = PathJoinSubstitution([ + FindPackageShare('mr_vision'), + 'configs', + config + ]) + + # 构建可执行文件路径 + pkg_prefix = get_package_prefix('mr_vision') + vision_exe = PathJoinSubstitution([ + pkg_prefix, + 'lib', + 'mr_vision', + 'bin', + vision_type + ]) + + # 启动视觉节点 + vision_node = ExecuteProcess( + cmd=[vision_exe, config_path], + output='screen', + name='sentry_vision' + ) + + return LaunchDescription([ + config_arg, + vision_type_arg, + vision_node + ]) diff --git a/src/rm_vision_bringup/launch/vision.launch.py b/src/rm_vision_bringup/launch/vision.launch.py new file mode 100644 index 0000000..17adea9 --- /dev/null +++ b/src/rm_vision_bringup/launch/vision.launch.py @@ -0,0 +1,54 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_prefix + + +def generate_launch_description(): + # 声明参数 + config_arg = DeclareLaunchArgument( + 'config', + default_value='standard4.yaml', + description='Vision config file name (in rm_vision/configs/)' + ) + + vision_type_arg = DeclareLaunchArgument( + 'vision_type', + default_value='standard_mpc', + description='Vision executable type: standard_mpc, balance_infantry_mpc, auto_aim_debug_mpc, auto_buff_debug_mpc' + ) + + # 获取参数值 + config = LaunchConfiguration('config') + vision_type = LaunchConfiguration('vision_type') + + # 构建配置文件路径 + config_path = PathJoinSubstitution([ + FindPackageShare('mr_vision'), + 'configs', + config + ]) + + # 构建可执行文件路径（在 lib/mr_vision/bin/ 目录下） + pkg_prefix = get_package_prefix('mr_vision') + vision_exe = PathJoinSubstitution([ + pkg_prefix, + 'lib', + 'mr_vision', + 'bin', + vision_type + ]) + + # 启动视觉节点 + vision_node = ExecuteProcess( + cmd=[vision_exe, config_path], + output='screen', + name='rm_vision' + ) + + return LaunchDescription([ + config_arg, + vision_type_arg, + vision_node + ]) diff --git a/src/rm_vision_bringup/package.xml b/src/rm_vision_bringup/package.xml new file mode 100644 index 0000000..ac32e85 --- /dev/null +++ b/src/rm_vision_bringup/package.xml @@ -0,0 +1,20 @@ + + + + rm_vision_bringup + 1.0.0 + RoboMaster Vision System Launch Package + MOVE Team + MIT + + ament_cmake + + mr_vision + + ament_lint_auto + ament_lint_common + + + ament_cmake + +