feat: 添加ROS2通信支持和哨兵MPC程序
- 添加ROS2条件编译支持，自动检测ROS2环境 - 创建GimbalROS类，使用rm_msgs进行ROS2通信 - 发布data_aim话题（DataAim消息） - 订阅data_mcu话题（DataMCU消息） - 新增sentry_mpc程序，使用ROS2通信的哨兵自瞄 - 新增capture_ros程序，使用ROS2通信的标定采集 - 更新README.md，添加完整的ROS2使用说明 - 移除旧的sentry相关程序，统一使用sentry_mpc Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
parent
16dfed6627
commit
0a6fd76f0d
@ -15,6 +15,22 @@ find_package(nlohmann_json REQUIRED)
|
||||
set(OpenVINO_DIR "/opt/intel/openvino_2024.6.0/runtime/cmake/")
|
||||
find_package(OpenVINO REQUIRED)
|
||||
|
||||
# 尝试查找ROS2
|
||||
find_package(ament_cmake QUIET)
|
||||
find_package(rclcpp QUIET)
|
||||
|
||||
if(ament_cmake_FOUND AND rclcpp_FOUND)
|
||||
message(STATUS "ROS2 found, enabling ROS2 support")
|
||||
set(USE_ROS2 ON)
|
||||
add_definitions(-DUSE_ROS2)
|
||||
|
||||
# 查找rm_msgs包
|
||||
find_package(rm_msgs REQUIRED)
|
||||
else()
|
||||
message(STATUS "ROS2 not found, building without ROS2 support")
|
||||
set(USE_ROS2 OFF)
|
||||
endif()
|
||||
|
||||
include_directories(${EIGEN3_INCLUDE_DIR})
|
||||
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||
include_directories(${PROJECT_SOURCE_DIR})
|
||||
@ -41,6 +57,13 @@ target_link_libraries(calibrate_handeye ${OpenCV_LIBS} fmt::fmt yaml-cpp compone
|
||||
target_link_libraries(calibrate_robotworld_handeye ${OpenCV_LIBS} fmt::fmt yaml-cpp component)
|
||||
target_link_libraries(split_video ${OpenCV_LIBS} fmt::fmt component)
|
||||
|
||||
# ROS2版本的capture
|
||||
if(USE_ROS2)
|
||||
add_executable(capture_ros calibration/capture_ros.cpp)
|
||||
target_link_libraries(capture_ros ${OpenCV_LIBS} fmt::fmt yaml-cpp component device rclcpp::rclcpp ${rm_msgs_TARGETS})
|
||||
ament_target_dependencies(capture_ros rclcpp rm_msgs)
|
||||
endif()
|
||||
|
||||
##################tests##################
|
||||
add_executable(auto_aim_test src/task/test/auto_aim_test.cpp)
|
||||
add_executable(auto_buff_test src/task/test/auto_buff_test.cpp)
|
||||
@ -117,3 +140,10 @@ target_link_libraries(balance_infantry ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann
|
||||
add_executable(balance_infantry_mpc src/task/balance_infantry_mpc.cpp)
|
||||
target_link_libraries(balance_infantry_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
|
||||
|
||||
################## ROS2 tasks ##################
|
||||
if(USE_ROS2)
|
||||
add_executable(sentry_mpc src/task/sentry_mpc.cpp)
|
||||
target_link_libraries(sentry_mpc ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device rclcpp::rclcpp ${rm_msgs_TARGETS})
|
||||
ament_target_dependencies(sentry_mpc rclcpp rm_msgs)
|
||||
endif()
|
||||
|
||||
|
||||
133
README.md
133
README.md
@ -25,6 +25,7 @@
|
||||
- 运算平台：Intel NUC（i7-1260P / i7-1165G7）
|
||||
- 相机：海康 MV-CS016-10UC + 6mm 镜头
|
||||
- 下位机：RoboMaster C 型开发板（STM32F407）/ 达妙 MC02（STM32H7）
|
||||
- ROS2 Humble（可选，用于哨兵ROS2通信）
|
||||
|
||||
## 依赖安装
|
||||
|
||||
@ -43,14 +44,45 @@
|
||||
nlohmann-json3-dev openssh-server screen
|
||||
```
|
||||
|
||||
3. ROS2 依赖（可选，用于哨兵）：
|
||||
```bash
|
||||
# 安装 ROS2 Humble
|
||||
sudo apt install ros-humble-desktop
|
||||
|
||||
# 编译 rm_msgs 包
|
||||
cd ~/rm_msgs
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build
|
||||
```
|
||||
|
||||
## 编译与运行
|
||||
|
||||
### 标准编译（不含ROS2）
|
||||
|
||||
```bash
|
||||
cmake -B build
|
||||
make -C build/ -j$(nproc)
|
||||
./build/auto_aim_test # 运行测试
|
||||
```
|
||||
|
||||
### ROS2编译（哨兵专用）
|
||||
|
||||
```bash
|
||||
# 设置ROS2环境
|
||||
source /opt/ros/humble/setup.bash
|
||||
source ~/rm_msgs/install/setup.bash
|
||||
|
||||
# 编译
|
||||
cmake -B build
|
||||
make -C build/ -j$(nproc)
|
||||
|
||||
# 运行ROS2版本程序
|
||||
./build/sentry_mpc configs/sentry.yaml
|
||||
./build/capture_ros configs/calibration.yaml -o assets/img_with_q
|
||||
```
|
||||
|
||||
**注意**：CMake会自动检测ROS2环境，如果检测到ROS2和rm_msgs包，会自动启用ROS2支持并编译相关程序。
|
||||
|
||||
## 可执行目标
|
||||
|
||||
### 主程序（task）
|
||||
@ -59,27 +91,31 @@ make -C build/ -j$(nproc)
|
||||
|------|------|----------|
|
||||
| `standard` | 步兵自瞄 | `configs/standard3.yaml` |
|
||||
| `standard_mpc` | 步兵自瞄（MPC 规划） | 需指定 |
|
||||
| `sentry_mpc` | 哨兵自瞄（ROS2通信） | 需指定 |
|
||||
| `uav` | 无人机自瞄 + 打符 | `configs/uav.yaml` |
|
||||
| `uav_debug` | 无人机调试（含可视化） | `configs/uav.yaml` |
|
||||
| `mt_standard` | 多线程步兵 | 需指定 |
|
||||
| `sentry` | 哨兵 | `configs/sentry.yaml` |
|
||||
| `sentry_multithread` | 哨兵多线程 | `configs/sentry.yaml` |
|
||||
| `sentry_debug` | 哨兵调试 | `configs/sentry.yaml` |
|
||||
| `sentry_bp` | 哨兵（带弹道预测） | `configs/sentry.yaml` |
|
||||
| `balance_infantry` | 平衡步兵 | 需指定 |
|
||||
| `balance_infantry_mpc` | 平衡步兵（MPC规划） | 需指定 |
|
||||
| `auto_aim_debug_mpc` | 自瞄 MPC 调试 | 需指定 |
|
||||
| `auto_buff_debug` | 打符调试 | 需指定 |
|
||||
| `auto_buff_debug_mpc` | 打符 MPC 调试 | 需指定 |
|
||||
| `mt_auto_aim_debug` | 多线程自瞄调试 | 需指定 |
|
||||
|
||||
**注意**：`sentry_mpc` 需要ROS2环境，只在检测到ROS2时才会编译。
|
||||
|
||||
### 标定工具（calibration）
|
||||
|
||||
| 目标 | 说明 |
|
||||
|------|------|
|
||||
| `capture` | 采集标定图像 |
|
||||
| `calibrate_camera` | 相机内参标定 |
|
||||
| `calibrate_handeye` | 手眼标定 |
|
||||
| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 |
|
||||
| `split_video` | 视频拆帧 |
|
||||
| 目标 | 说明 | 通信方式 |
|
||||
|------|------|----------|
|
||||
| `capture` | 采集标定图像 | 串口 |
|
||||
| `capture_ros` | 采集标定图像（ROS2） | ROS2话题 |
|
||||
| `calibrate_camera` | 相机内参标定 | - |
|
||||
| `calibrate_handeye` | 手眼标定 | - |
|
||||
| `calibrate_robotworld_handeye` | 机器人-世界手眼标定 | - |
|
||||
| `split_video` | 视频拆帧 | - |
|
||||
|
||||
**注意**：`capture_ros` 需要ROS2环境，只在检测到ROS2时才会编译。
|
||||
|
||||
### 测试用例（test）
|
||||
|
||||
@ -225,11 +261,78 @@ make -C build/ -j$(nproc)
|
||||
|
||||
### 4. ROS2 通信（哨兵专有）
|
||||
|
||||
**rm_msgs 自定义消息**
|
||||
|
||||
| 方向 | 话题 | 消息类型 | 内容 |
|
||||
|------|------|----------|------|
|
||||
| 发布 | `auto_aim_target_pos` | `std_msgs/String` | CSV: "x,y,z,w" |
|
||||
| 订阅 | `enemy_status` | `sp_msgs/EnemyStatusMsg` | 无敌敌人 ID 列表 |
|
||||
| 订阅 | `autoaim_target` | `sp_msgs/AutoaimTargetMsg` | 目标敌人 ID 列表 |
|
||||
| 发布 | `data_aim` | `rm_msgs/DataAim` | 视觉控制指令 |
|
||||
| 订阅 | `data_mcu` | `rm_msgs/DataMCU` | MCU状态数据 |
|
||||
|
||||
**DataAim 消息定义（视觉 → MCU）**
|
||||
|
||||
```
|
||||
uint8 mode # 0: 不控制, 1: 控制云台但不开火, 2: 控制云台且开火
|
||||
float32 yaw # 目标偏航角 (rad)
|
||||
float32 yaw_vel # 偏航角速度 (rad/s)
|
||||
float32 yaw_acc # 偏航角加速度 (rad/s²)
|
||||
float32 pitch # 目标俯仰角 (rad)
|
||||
float32 pitch_vel # 俯仰角速度 (rad/s)
|
||||
float32 pitch_acc # 俯仰角加速度 (rad/s²)
|
||||
```
|
||||
|
||||
**DataMCU 消息定义（MCU → 视觉）**
|
||||
|
||||
```
|
||||
uint8 mode # 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符
|
||||
float32 q0 # 四元数 w
|
||||
float32 q1 # 四元数 x
|
||||
float32 q2 # 四元数 y
|
||||
float32 q3 # 四元数 z
|
||||
float32 yaw # 偏航角 (rad)
|
||||
float32 yaw_vel # 偏航角速度 (rad/s)
|
||||
float32 pitch # 俯仰角 (rad)
|
||||
float32 pitch_vel # 俯仰角速度 (rad/s)
|
||||
float32 bullet_speed # 弹速 (m/s)
|
||||
uint16 bullet_count # 子弹累计发送次数
|
||||
```
|
||||
|
||||
**ROS2 程序使用说明**
|
||||
|
||||
1. 编译 rm_msgs 包：
|
||||
```bash
|
||||
cd ~/rm_msgs
|
||||
source /opt/ros/humble/setup.bash
|
||||
colcon build
|
||||
```
|
||||
|
||||
2. 编译视觉程序：
|
||||
```bash
|
||||
cd /home/robofish/MOVE_AI
|
||||
source /opt/ros/humble/setup.bash
|
||||
source ~/rm_msgs/install/setup.bash
|
||||
cmake -B build
|
||||
make -C build/ sentry_mpc capture_ros -j$(nproc)
|
||||
```
|
||||
|
||||
3. 运行程序：
|
||||
```bash
|
||||
# 运行哨兵自瞄
|
||||
source /opt/ros/humble/setup.bash
|
||||
source ~/rm_msgs/install/setup.bash
|
||||
./build/sentry_mpc configs/sentry.yaml
|
||||
|
||||
# 运行标定采集
|
||||
./build/capture_ros configs/calibration.yaml -o assets/img_with_q
|
||||
```
|
||||
|
||||
**ROS2 兼容性说明**
|
||||
|
||||
- 项目支持条件编译，自动检测ROS2环境
|
||||
- 如果未安装ROS2，只编译串口/CAN版本的程序
|
||||
- 如果安装了ROS2和rm_msgs，会额外编译ROS2版本：
|
||||
- `sentry_mpc`: 使用ROS2通信的哨兵自瞄程序
|
||||
- `capture_ros`: 使用ROS2通信的标定采集程序
|
||||
- ROS2版本与串口版本功能完全相同，只是通信方式不同
|
||||
|
||||
### 5. 协议总览
|
||||
|
||||
@ -238,4 +341,4 @@ make -C build/ -j$(nproc)
|
||||
| CBoard | CAN | 1Mbps | 8B | — | C 板 (STM32F407) |
|
||||
| Gimbal | 串口 | 可配置 | 29/43B | CRC16 | 达妙 MC02 (STM32H7) |
|
||||
| DM IMU | 串口 | 921600 | 57B | CRC16 | 达妙 IMU |
|
||||
| ROS2 | TCP/IP | — | 可变 | — | 导航系统 |
|
||||
| ROS2 | DDS | — | 可变 | DDS | 哨兵导航系统 |
|
||||
|
||||
164
calibration/capture_ros.cpp
Normal file
164
calibration/capture_ros.cpp
Normal file
@ -0,0 +1,164 @@
|
||||
#ifdef USE_ROS2
|
||||
|
||||
#include <fmt/core.h>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
#include <memory>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "calibration/board_pattern.hpp"
|
||||
#include "src/component/img_tools.hpp"
|
||||
#include "src/component/logger.hpp"
|
||||
#include "src/component/math_tools.hpp"
|
||||
#include "src/device/camera.hpp"
|
||||
#include "src/device/gimbal/gimbal_ros.hpp"
|
||||
|
||||
const std::string keys =
|
||||
"{help h usage ? | | 输出命令行参数说明}"
|
||||
"{@config-path c | configs/calibration.yaml | yaml配置文件路径 }"
|
||||
"{output-folder o | assets/img_with_q | 输出文件夹路径 }";
|
||||
|
||||
void write_q(const std::string q_path, const Eigen::Quaterniond & q)
|
||||
{
|
||||
std::ofstream q_file(q_path);
|
||||
Eigen::Vector4d xyzw = q.coeffs();
|
||||
// 输出顺序为wxyz
|
||||
q_file << fmt::format("{} {} {} {}", xyzw[3], xyzw[0], xyzw[1], xyzw[2]);
|
||||
q_file.close();
|
||||
}
|
||||
|
||||
void capture_loop(
|
||||
const std::string & config_path, const std::string & output_folder,
|
||||
std::shared_ptr<rclcpp::Node> node)
|
||||
{
|
||||
// 从配置文件加载标定板参数（支持 circles_grid 和 chessboard）
|
||||
auto yaml = YAML::LoadFile(config_path);
|
||||
auto board_pattern = calibration::load_board_pattern(yaml);
|
||||
|
||||
device::GimbalROS gimbal(config_path, node);
|
||||
device::Camera camera(config_path);
|
||||
cv::Mat img;
|
||||
std::chrono::steady_clock::time_point timestamp;
|
||||
|
||||
// ROS2 spin线程
|
||||
std::atomic<bool> quit = false;
|
||||
auto ros_thread = std::thread([&]() {
|
||||
while (!quit && rclcpp::ok()) {
|
||||
rclcpp::spin_some(node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
});
|
||||
|
||||
int count = 0;
|
||||
component::logger()->info("[CaptureROS] Started with ROS2 communication");
|
||||
component::logger()->info("[CaptureROS] Press 's' to save, 'q' to quit");
|
||||
|
||||
while (rclcpp::ok()) {
|
||||
camera.read(img, timestamp);
|
||||
Eigen::Quaterniond q = gimbal.q(timestamp);
|
||||
|
||||
// 在图像上显示欧拉角，用来判断imuabs系的xyz正方向，同时判断imu是否存在零漂
|
||||
auto img_with_ypr = img.clone();
|
||||
Eigen::Vector3d zyx = component::eulers(q, 2, 1, 0) * 57.3; // degree
|
||||
component::draw_text(img_with_ypr, fmt::format("Z {:.2f}", zyx[0]), {40, 40}, {0, 0, 255});
|
||||
component::draw_text(img_with_ypr, fmt::format("Y {:.2f}", zyx[1]), {40, 80}, {0, 0, 255});
|
||||
component::draw_text(img_with_ypr, fmt::format("X {:.2f}", zyx[2]), {40, 120}, {0, 0, 255});
|
||||
|
||||
std::vector<cv::Point2f> centers_2d;
|
||||
bool success;
|
||||
if (board_pattern.pattern_type == calibration::PatternType::chessboard) {
|
||||
// 棋盘格检测很慢，先在缩小图上快速检测，再映射回原图做亚像素精化
|
||||
cv::Mat small;
|
||||
double scale = 0.5;
|
||||
cv::resize(img, small, {}, scale, scale);
|
||||
std::vector<cv::Point2f> small_pts;
|
||||
success = calibration::find_pattern_points(small, board_pattern, small_pts);
|
||||
if (success) {
|
||||
for (auto & p : small_pts) { p.x /= scale; p.y /= scale; }
|
||||
cv::Mat gray;
|
||||
cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
|
||||
cv::cornerSubPix(
|
||||
gray, small_pts, cv::Size(11, 11), cv::Size(-1, -1),
|
||||
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 1e-3));
|
||||
centers_2d = std::move(small_pts);
|
||||
}
|
||||
} else {
|
||||
success = calibration::find_pattern_points(img, board_pattern, centers_2d);
|
||||
}
|
||||
cv::drawChessboardCorners(img_with_ypr, board_pattern.pattern_size, centers_2d, success);
|
||||
cv::resize(img_with_ypr, img_with_ypr, {}, 0.5, 0.5); // 显示时缩小图片尺寸
|
||||
|
||||
// 按"s"保存图片和对应四元数，按"q"退出程序
|
||||
cv::imshow("Press s to save, q to quit", img_with_ypr);
|
||||
auto key = cv::waitKey(1);
|
||||
if (key == 'q')
|
||||
break;
|
||||
else if (key != 's')
|
||||
continue;
|
||||
|
||||
// 保存图片和四元数
|
||||
count++;
|
||||
auto img_path = fmt::format("{}/{}.jpg", output_folder, count);
|
||||
auto q_path = fmt::format("{}/{}.txt", output_folder, count);
|
||||
cv::imwrite(img_path, img);
|
||||
write_q(q_path, q);
|
||||
component::logger()->info("[{}] Saved in {}", count, output_folder);
|
||||
}
|
||||
|
||||
quit = true;
|
||||
if (ros_thread.joinable()) ros_thread.join();
|
||||
|
||||
// 离开该作用域时，camera和gimbal会自动关闭
|
||||
}
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
// 读取命令行参数
|
||||
cv::CommandLineParser cli(argc, argv, keys);
|
||||
if (cli.has("help")) {
|
||||
cli.printMessage();
|
||||
return 0;
|
||||
}
|
||||
auto config_path = cli.get<std::string>(0);
|
||||
auto output_folder = cli.get<std::string>("output-folder");
|
||||
|
||||
// 初始化ROS2
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<rclcpp::Node>("capture_ros");
|
||||
|
||||
// 新建输出文件夹
|
||||
std::filesystem::create_directory(output_folder);
|
||||
|
||||
// 从配置文件读取标定板类型和尺寸
|
||||
auto yaml = YAML::LoadFile(config_path);
|
||||
auto board_pattern = calibration::load_board_pattern(yaml);
|
||||
component::logger()->info(
|
||||
"标定板类型: {}, 尺寸: {}列{}行",
|
||||
calibration::pattern_name(board_pattern.pattern_type),
|
||||
board_pattern.pattern_size.width, board_pattern.pattern_size.height);
|
||||
|
||||
// 主循环，保存图片和对应四元数
|
||||
capture_loop(config_path, output_folder, node);
|
||||
|
||||
component::logger()->warn("注意四元数输出顺序为wxyz");
|
||||
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main()
|
||||
{
|
||||
std::cerr << "Error: This program requires ROS2 support. Please build with ROS2 enabled." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
#endif // USE_ROS2
|
||||
@ -5,16 +5,23 @@ find_package(yaml-cpp REQUIRED)
|
||||
add_subdirectory(serial)
|
||||
|
||||
# 创建目标 device
|
||||
add_library(device STATIC
|
||||
hikrobot/hikrobot.cpp
|
||||
mindvision/mindvision.cpp
|
||||
usbcamera/usbcamera.cpp
|
||||
set(DEVICE_SOURCES
|
||||
hikrobot/hikrobot.cpp
|
||||
mindvision/mindvision.cpp
|
||||
usbcamera/usbcamera.cpp
|
||||
camera.cpp
|
||||
cboard.cpp
|
||||
dm_imu/dm_imu.cpp
|
||||
gimbal/gimbal.cpp
|
||||
)
|
||||
|
||||
# 如果启用ROS2，添加ROS2版本的gimbal
|
||||
if(USE_ROS2)
|
||||
list(APPEND DEVICE_SOURCES gimbal/gimbal_ros.cpp)
|
||||
endif()
|
||||
|
||||
add_library(device STATIC ${DEVICE_SOURCES})
|
||||
|
||||
# hikrobot
|
||||
target_include_directories(device PUBLIC hikrobot/include)
|
||||
if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64")
|
||||
@ -35,4 +42,10 @@ else()
|
||||
message(FATAL_ERROR "Unsupported architecture: ${CMAKE_HOST_SYSTEM_PROCESSOR}!")
|
||||
endif()
|
||||
|
||||
target_link_libraries(device MvCameraControl MVSDK usb-1.0 yaml-cpp serial)
|
||||
target_link_libraries(device MvCameraControl MVSDK usb-1.0 yaml-cpp serial)
|
||||
|
||||
# 如果启用ROS2，添加ROS2依赖
|
||||
if(USE_ROS2)
|
||||
target_include_directories(device PUBLIC ${rclcpp_INCLUDE_DIRS} ${rm_msgs_INCLUDE_DIRS})
|
||||
target_link_libraries(device ${rclcpp_LIBRARIES})
|
||||
endif()
|
||||
144
src/device/gimbal/gimbal_ros.cpp
Normal file
144
src/device/gimbal/gimbal_ros.cpp
Normal file
@ -0,0 +1,144 @@
|
||||
#ifdef USE_ROS2
|
||||
|
||||
#include "gimbal_ros.hpp"
|
||||
|
||||
#include "src/component/logger.hpp"
|
||||
#include "src/component/math_tools.hpp"
|
||||
#include "src/component/yaml.hpp"
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
GimbalROS::GimbalROS(const std::string & config_path, std::shared_ptr<rclcpp::Node> node)
|
||||
: node_(node)
|
||||
{
|
||||
// 创建发布者和订阅者
|
||||
aim_pub_ = node_->create_publisher<rm_msgs::msg::DataAim>("data_aim", 10);
|
||||
mcu_sub_ = node_->create_subscription<rm_msgs::msg::DataMCU>(
|
||||
"data_mcu", 10, std::bind(&GimbalROS::mcu_callback, this, std::placeholders::_1));
|
||||
|
||||
component::logger()->info("[GimbalROS] ROS2 communication initialized");
|
||||
component::logger()->info("[GimbalROS] Publishing to: data_aim");
|
||||
component::logger()->info("[GimbalROS] Subscribing to: data_mcu");
|
||||
}
|
||||
|
||||
GimbalROS::~GimbalROS()
|
||||
{
|
||||
component::logger()->info("[GimbalROS] Shutting down");
|
||||
}
|
||||
|
||||
GimbalMode GimbalROS::mode() const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return mode_;
|
||||
}
|
||||
|
||||
GimbalState GimbalROS::state() const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
return state_;
|
||||
}
|
||||
|
||||
std::string GimbalROS::str(GimbalMode mode) const
|
||||
{
|
||||
switch (mode) {
|
||||
case GimbalMode::IDLE:
|
||||
return "IDLE";
|
||||
case GimbalMode::AUTO_AIM:
|
||||
return "AUTO_AIM";
|
||||
case GimbalMode::SMALL_BUFF:
|
||||
return "SMALL_BUFF";
|
||||
case GimbalMode::BIG_BUFF:
|
||||
return "BIG_BUFF";
|
||||
default:
|
||||
return "INVALID";
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::Quaterniond GimbalROS::q(std::chrono::steady_clock::time_point t)
|
||||
{
|
||||
while (true) {
|
||||
auto [q_a, t_a] = queue_.pop();
|
||||
auto [q_b, t_b] = queue_.front();
|
||||
auto t_ab = component::delta_time(t_a, t_b);
|
||||
auto t_ac = component::delta_time(t_a, t);
|
||||
auto k = t_ac / t_ab;
|
||||
Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized();
|
||||
if (t < t_a) return q_c;
|
||||
if (!(t_a < t && t <= t_b)) continue;
|
||||
|
||||
return q_c;
|
||||
}
|
||||
}
|
||||
|
||||
void GimbalROS::send(
|
||||
bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
|
||||
float pitch_acc)
|
||||
{
|
||||
auto msg = rm_msgs::msg::DataAim();
|
||||
msg.mode = control ? (fire ? 2 : 1) : 0;
|
||||
msg.yaw = yaw;
|
||||
msg.yaw_vel = yaw_vel;
|
||||
msg.yaw_acc = yaw_acc;
|
||||
msg.pitch = pitch;
|
||||
msg.pitch_vel = pitch_vel;
|
||||
msg.pitch_acc = pitch_acc;
|
||||
|
||||
aim_pub_->publish(msg);
|
||||
}
|
||||
|
||||
void GimbalROS::mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg)
|
||||
{
|
||||
auto t = std::chrono::steady_clock::now();
|
||||
|
||||
// 验证四元数范数
|
||||
float q_norm = msg->q0 * msg->q0 + msg->q1 * msg->q1 + msg->q2 * msg->q2 + msg->q3 * msg->q3;
|
||||
if (q_norm < 0.9f || q_norm > 1.1f) {
|
||||
component::logger()->warn("[GimbalROS] Invalid quaternion norm {:.3f}, skipping", q_norm);
|
||||
return;
|
||||
}
|
||||
|
||||
// 验证模式
|
||||
if (msg->mode > 3) {
|
||||
component::logger()->warn("[GimbalROS] Invalid mode {}, skipping", msg->mode);
|
||||
return;
|
||||
}
|
||||
|
||||
// 存储四元数到队列
|
||||
Eigen::Quaterniond q(msg->q0, msg->q1, msg->q2, msg->q3);
|
||||
queue_.push({q, t});
|
||||
|
||||
// 更新状态
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
|
||||
state_.yaw = msg->yaw;
|
||||
state_.yaw_vel = msg->yaw_vel;
|
||||
state_.pitch = msg->pitch;
|
||||
state_.pitch_vel = msg->pitch_vel;
|
||||
state_.bullet_speed = msg->bullet_speed;
|
||||
state_.bullet_count = msg->bullet_count;
|
||||
|
||||
// 更新模式
|
||||
switch (msg->mode) {
|
||||
case 0:
|
||||
mode_ = GimbalMode::IDLE;
|
||||
break;
|
||||
case 1:
|
||||
mode_ = GimbalMode::AUTO_AIM;
|
||||
break;
|
||||
case 2:
|
||||
mode_ = GimbalMode::SMALL_BUFF;
|
||||
break;
|
||||
case 3:
|
||||
mode_ = GimbalMode::BIG_BUFF;
|
||||
break;
|
||||
default:
|
||||
mode_ = GimbalMode::IDLE;
|
||||
component::logger()->warn("[GimbalROS] Invalid mode: {}", msg->mode);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
|
||||
#endif // USE_ROS2
|
||||
58
src/device/gimbal/gimbal_ros.hpp
Normal file
58
src/device/gimbal/gimbal_ros.hpp
Normal file
@ -0,0 +1,58 @@
|
||||
#ifndef DEVICE__GIMBAL_ROS_HPP
|
||||
#define DEVICE__GIMBAL_ROS_HPP
|
||||
|
||||
#ifdef USE_ROS2
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rm_msgs/msg/data_aim.hpp"
|
||||
#include "rm_msgs/msg/data_mcu.hpp"
|
||||
#include "src/component/thread_safe_queue.hpp"
|
||||
#include "src/device/gimbal/gimbal.hpp"
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
class GimbalROS
|
||||
{
|
||||
public:
|
||||
GimbalROS(const std::string & config_path, std::shared_ptr<rclcpp::Node> node);
|
||||
|
||||
~GimbalROS();
|
||||
|
||||
GimbalMode mode() const;
|
||||
GimbalState state() const;
|
||||
std::string str(GimbalMode mode) const;
|
||||
Eigen::Quaterniond q(std::chrono::steady_clock::time_point t);
|
||||
|
||||
void send(
|
||||
bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
|
||||
float pitch_acc);
|
||||
|
||||
private:
|
||||
std::shared_ptr<rclcpp::Node> node_;
|
||||
|
||||
rclcpp::Publisher<rm_msgs::msg::DataAim>::SharedPtr aim_pub_;
|
||||
rclcpp::Subscription<rm_msgs::msg::DataMCU>::SharedPtr mcu_sub_;
|
||||
|
||||
mutable std::mutex mutex_;
|
||||
|
||||
GimbalMode mode_ = GimbalMode::IDLE;
|
||||
GimbalState state_;
|
||||
component::ThreadSafeQueue<std::tuple<Eigen::Quaterniond, std::chrono::steady_clock::time_point>>
|
||||
queue_{1000};
|
||||
|
||||
void mcu_callback(const rm_msgs::msg::DataMCU::SharedPtr msg);
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
||||
#endif // USE_ROS2
|
||||
|
||||
#endif // DEVICE__GIMBAL_ROS_HPP
|
||||
@ -1,106 +0,0 @@
|
||||
#include <fmt/core.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "src/device/camera.hpp"
|
||||
#include "src/device/cboard.hpp"
|
||||
#include "src/device/ros2/publish2nav.hpp"
|
||||
#include "src/device/ros2/ros2.hpp"
|
||||
#include "src/device/usbcamera/usbcamera.hpp"
|
||||
#include "src/module/auto_aim/aimer.hpp"
|
||||
#include "src/module/auto_aim/shooter.hpp"
|
||||
#include "src/module/auto_aim/solver.hpp"
|
||||
#include "src/module/auto_aim/tracker.hpp"
|
||||
#include "src/module/auto_aim/yolo.hpp"
|
||||
#include "src/module/omniperception/decider.hpp"
|
||||
#include "src/component/exiter.hpp"
|
||||
#include "src/component/img_tools.hpp"
|
||||
#include "src/component/logger.hpp"
|
||||
#include "src/component/math_tools.hpp"
|
||||
#include "src/component/plotter.hpp"
|
||||
#include "src/component/recorder.hpp"
|
||||
|
||||
using namespace std::chrono;
|
||||
|
||||
const std::string keys =
|
||||
"{help h usage ? | | 输出命令行参数说明}"
|
||||
"{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
component::Exiter exiter;
|
||||
component::Plotter plotter;
|
||||
component::Recorder recorder;
|
||||
|
||||
cv::CommandLineParser cli(argc, argv, keys);
|
||||
if (cli.has("help")) {
|
||||
cli.printMessage();
|
||||
return 0;
|
||||
}
|
||||
auto config_path = cli.get<std::string>(0);
|
||||
|
||||
device::ROS2 ros2;
|
||||
device::CBoard cboard(config_path);
|
||||
device::Camera camera(config_path);
|
||||
device::Camera back_camera("configs/camera.yaml");
|
||||
device::USBCamera usbcam1("video0", config_path);
|
||||
device::USBCamera usbcam2("video2", config_path);
|
||||
|
||||
auto_aim::YOLO yolo(config_path, false);
|
||||
auto_aim::Solver solver(config_path);
|
||||
auto_aim::Tracker tracker(config_path, solver);
|
||||
auto_aim::Aimer aimer(config_path);
|
||||
auto_aim::Shooter shooter(config_path);
|
||||
|
||||
omniperception::Decider decider(config_path);
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
std::chrono::steady_clock::time_point timestamp;
|
||||
device::Command last_command;
|
||||
|
||||
while (!exiter.exit()) {
|
||||
camera.read(img, timestamp);
|
||||
Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
|
||||
// recorder.record(img, q, timestamp);
|
||||
|
||||
/// 自瞄核心逻辑
|
||||
solver.set_R_gimbal2world(q);
|
||||
|
||||
Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
|
||||
|
||||
auto armors = yolo.detect(img);
|
||||
|
||||
decider.get_invincible_armor(ros2.subscribe_enemy_status());
|
||||
|
||||
decider.armor_filter(armors);
|
||||
|
||||
// decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target());
|
||||
|
||||
decider.set_priority(armors);
|
||||
|
||||
auto targets = tracker.track(armors, timestamp);
|
||||
|
||||
device::Command command{false, false, 0, 0};
|
||||
|
||||
/// 全向感知逻辑
|
||||
if (tracker.state() == "lost")
|
||||
command = decider.decide(yolo, gimbal_pos, usbcam1, usbcam2, back_camera);
|
||||
else
|
||||
command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode);
|
||||
|
||||
/// 发射逻辑
|
||||
command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);
|
||||
|
||||
cboard.send(command);
|
||||
|
||||
/// ROS2通信
|
||||
Eigen::Vector4d target_info = decider.get_target_info(armors, targets);
|
||||
|
||||
ros2.publish(target_info);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@ -1,104 +0,0 @@
|
||||
#include <fmt/core.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "src/device/camera.hpp"
|
||||
#include "src/device/cboard.hpp"
|
||||
#include "src/device/ros2/publish2nav.hpp"
|
||||
#include "src/device/ros2/ros2.hpp"
|
||||
#include "src/device/usbcamera/usbcamera.hpp"
|
||||
#include "src/module/auto_aim/aimer.hpp"
|
||||
#include "src/module/auto_aim/shooter.hpp"
|
||||
#include "src/module/auto_aim/solver.hpp"
|
||||
#include "src/module/auto_aim/tracker.hpp"
|
||||
#include "src/module/auto_aim/yolo.hpp"
|
||||
#include "src/module/omniperception/decider.hpp"
|
||||
#include "src/component/exiter.hpp"
|
||||
#include "src/component/img_tools.hpp"
|
||||
#include "src/component/logger.hpp"
|
||||
#include "src/component/math_tools.hpp"
|
||||
#include "src/component/plotter.hpp"
|
||||
#include "src/component/recorder.hpp"
|
||||
|
||||
using namespace std::chrono;
|
||||
|
||||
const std::string keys =
|
||||
"{help h usage ? | | 输出命令行参数说明}"
|
||||
"{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
component::Exiter exiter;
|
||||
component::Plotter plotter;
|
||||
component::Recorder recorder;
|
||||
|
||||
cv::CommandLineParser cli(argc, argv, keys);
|
||||
if (cli.has("help")) {
|
||||
cli.printMessage();
|
||||
return 0;
|
||||
}
|
||||
auto config_path = cli.get<std::string>(0);
|
||||
|
||||
device::ROS2 ros2;
|
||||
device::CBoard cboard(config_path);
|
||||
device::Camera camera(config_path);
|
||||
device::Camera back_camera("configs/camera.yaml");
|
||||
|
||||
auto_aim::YOLO yolo(config_path, false);
|
||||
auto_aim::Solver solver(config_path);
|
||||
auto_aim::Tracker tracker(config_path, solver);
|
||||
auto_aim::Aimer aimer(config_path);
|
||||
auto_aim::Shooter shooter(config_path);
|
||||
|
||||
omniperception::Decider decider(config_path);
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
std::chrono::steady_clock::time_point timestamp;
|
||||
device::Command last_command;
|
||||
|
||||
while (!exiter.exit()) {
|
||||
camera.read(img, timestamp);
|
||||
Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
|
||||
// recorder.record(img, q, timestamp);
|
||||
|
||||
/// 自瞄核心逻辑
|
||||
solver.set_R_gimbal2world(q);
|
||||
|
||||
Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
|
||||
|
||||
auto armors = yolo.detect(img);
|
||||
|
||||
decider.get_invincible_armor(ros2.subscribe_enemy_status());
|
||||
|
||||
decider.armor_filter(armors);
|
||||
|
||||
// decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target());
|
||||
|
||||
decider.set_priority(armors);
|
||||
|
||||
auto targets = tracker.track(armors, timestamp);
|
||||
|
||||
device::Command command{false, false, 0, 0};
|
||||
|
||||
/// 全向感知逻辑
|
||||
if (tracker.state() == "lost")
|
||||
command = decider.decide(yolo, gimbal_pos, back_camera);
|
||||
else
|
||||
command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode);
|
||||
|
||||
/// 发射逻辑
|
||||
command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);
|
||||
|
||||
cboard.send(command);
|
||||
|
||||
/// ROS2通信
|
||||
Eigen::Vector4d target_info = decider.get_target_info(armors, targets);
|
||||
|
||||
ros2.publish(target_info);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@ -1,196 +0,0 @@
|
||||
#include <fmt/core.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "src/device/camera.hpp"
|
||||
#include "src/device/cboard.hpp"
|
||||
#include "src/device/ros2/publish2nav.hpp"
|
||||
#include "src/device/ros2/ros2.hpp"
|
||||
#include "src/device/usbcamera/usbcamera.hpp"
|
||||
#include "src/module/auto_aim/aimer.hpp"
|
||||
#include "src/module/auto_aim/shooter.hpp"
|
||||
#include "src/module/auto_aim/solver.hpp"
|
||||
#include "src/module/auto_aim/tracker.hpp"
|
||||
#include "src/module/auto_aim/yolo.hpp"
|
||||
#include "src/module/omniperception/decider.hpp"
|
||||
#include "src/component/exiter.hpp"
|
||||
#include "src/component/img_tools.hpp"
|
||||
#include "src/component/logger.hpp"
|
||||
#include "src/component/math_tools.hpp"
|
||||
#include "src/component/plotter.hpp"
|
||||
#include "src/component/recorder.hpp"
|
||||
|
||||
using namespace std::chrono;
|
||||
|
||||
const std::string keys =
|
||||
"{help h usage ? | | 输出命令行参数说明}"
|
||||
"{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
component::Exiter exiter;
|
||||
component::Plotter plotter;
|
||||
component::Recorder recorder;
|
||||
|
||||
cv::CommandLineParser cli(argc, argv, keys);
|
||||
if (cli.has("help")) {
|
||||
cli.printMessage();
|
||||
return 0;
|
||||
}
|
||||
auto config_path = cli.get<std::string>(0);
|
||||
|
||||
device::ROS2 ros2;
|
||||
device::CBoard cboard(config_path);
|
||||
device::Camera camera(config_path);
|
||||
device::Camera back_camera("configs/camera.yaml");
|
||||
device::USBCamera usbcam1("video0", config_path);
|
||||
device::USBCamera usbcam2("video2", config_path);
|
||||
|
||||
auto_aim::YOLO yolo(config_path, false);
|
||||
auto_aim::Solver solver(config_path);
|
||||
auto_aim::Tracker tracker(config_path, solver);
|
||||
auto_aim::Aimer aimer(config_path);
|
||||
auto_aim::Shooter shooter(config_path);
|
||||
|
||||
omniperception::Decider decider(config_path);
|
||||
|
||||
cv::Mat img;
|
||||
|
||||
std::chrono::steady_clock::time_point timestamp;
|
||||
device::Command last_command;
|
||||
|
||||
while (!exiter.exit()) {
|
||||
camera.read(img, timestamp);
|
||||
Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
|
||||
// recorder.record(img, q, timestamp);
|
||||
|
||||
/// 自瞄核心逻辑
|
||||
solver.set_R_gimbal2world(q);
|
||||
|
||||
Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
|
||||
|
||||
auto armors = yolo.detect(img);
|
||||
|
||||
decider.get_invincible_armor(ros2.subscribe_enemy_status());
|
||||
|
||||
decider.armor_filter(armors);
|
||||
|
||||
decider.get_auto_aim_target(armors, ros2.subscribe_autoaim_target());
|
||||
|
||||
decider.set_priority(armors);
|
||||
|
||||
auto targets = tracker.track(armors, timestamp);
|
||||
|
||||
device::Command command{false, false, 0, 0};
|
||||
|
||||
/// 全向感知逻辑
|
||||
if (tracker.state() == "lost")
|
||||
command = decider.decide(yolo, gimbal_pos, usbcam1, usbcam2, back_camera);
|
||||
else
|
||||
command = aimer.aim(targets, timestamp, cboard.bullet_speed, cboard.shoot_mode);
|
||||
|
||||
/// 发射逻辑
|
||||
command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);
|
||||
|
||||
cboard.send(command);
|
||||
|
||||
/// ROS2通信
|
||||
Eigen::Vector4d target_info = decider.get_target_info(armors, targets);
|
||||
|
||||
ros2.publish(target_info);
|
||||
|
||||
/// debug
|
||||
component::draw_text(img, fmt::format("[{}]", tracker.state()), {10, 30}, {255, 255, 255});
|
||||
|
||||
nlohmann::json data;
|
||||
|
||||
// 装甲板原始观测数据
|
||||
data["armor_num"] = armors.size();
|
||||
if (!armors.empty()) {
|
||||
auto min_x = 1e10;
|
||||
auto & armor = armors.front();
|
||||
for (auto & a : armors) {
|
||||
if (a.center.x < min_x) {
|
||||
min_x = a.center.x;
|
||||
armor = a;
|
||||
}
|
||||
} //always left
|
||||
solver.solve(armor);
|
||||
data["armor_x"] = armor.xyz_in_world[0];
|
||||
data["armor_y"] = armor.xyz_in_world[1];
|
||||
data["armor_yaw"] = armor.ypr_in_world[0] * 57.3;
|
||||
data["armor_yaw_raw"] = armor.yaw_raw * 57.3;
|
||||
}
|
||||
|
||||
if (!targets.empty()) {
|
||||
auto target = targets.front();
|
||||
|
||||
// 当前帧target更新后
|
||||
std::vector<Eigen::Vector4d> armor_xyza_list = target.armor_xyza_list();
|
||||
for (const Eigen::Vector4d & xyza : armor_xyza_list) {
|
||||
auto image_points =
|
||||
solver.reproject_armor(xyza.head(3), xyza[3], target.armor_type, target.name);
|
||||
component::draw_points(img, image_points, {0, 255, 0});
|
||||
}
|
||||
|
||||
// aimer瞄准位置
|
||||
auto aim_point = aimer.debug_aim_point;
|
||||
Eigen::Vector4d aim_xyza = aim_point.xyza;
|
||||
auto image_points =
|
||||
solver.reproject_armor(aim_xyza.head(3), aim_xyza[3], target.armor_type, target.name);
|
||||
if (aim_point.valid)
|
||||
component::draw_points(img, image_points, {0, 0, 255});
|
||||
else
|
||||
component::draw_points(img, image_points, {255, 0, 0});
|
||||
|
||||
// 观测器内部数据
|
||||
Eigen::VectorXd x = target.ekf_x();
|
||||
data["x"] = x[0];
|
||||
data["vx"] = x[1];
|
||||
data["y"] = x[2];
|
||||
data["vy"] = x[3];
|
||||
data["z"] = x[4];
|
||||
data["vz"] = x[5];
|
||||
data["a"] = x[6] * 57.3;
|
||||
data["w"] = x[7];
|
||||
data["r"] = x[8];
|
||||
data["l"] = x[9];
|
||||
data["h"] = x[10];
|
||||
data["last_id"] = target.last_id;
|
||||
|
||||
// 卡方检验数据
|
||||
data["residual_yaw"] = target.ekf().data.at("residual_yaw");
|
||||
data["residual_pitch"] = target.ekf().data.at("residual_pitch");
|
||||
data["residual_distance"] = target.ekf().data.at("residual_distance");
|
||||
data["residual_angle"] = target.ekf().data.at("residual_angle");
|
||||
data["nis"] = target.ekf().data.at("nis");
|
||||
data["nees"] = target.ekf().data.at("nees");
|
||||
data["nis_fail"] = target.ekf().data.at("nis_fail");
|
||||
data["nees_fail"] = target.ekf().data.at("nees_fail");
|
||||
data["recent_nis_failures"] = target.ekf().data.at("recent_nis_failures");
|
||||
}
|
||||
|
||||
// 云台响应情况
|
||||
data["gimbal_yaw"] = gimbal_pos[0] * 57.3;
|
||||
data["gimbal_pitch"] = -gimbal_pos[1] * 57.3;
|
||||
data["shootmode"] = cboard.shoot_mode;
|
||||
if (command.control) {
|
||||
data["cmd_yaw"] = command.yaw * 57.3;
|
||||
data["cmd_pitch"] = command.pitch * 57.3;
|
||||
data["cmd_shoot"] = command.shoot;
|
||||
}
|
||||
|
||||
data["bullet_speed"] = cboard.bullet_speed;
|
||||
|
||||
plotter.plot(data);
|
||||
|
||||
cv::resize(img, img, {}, 0.5, 0.5); // 显示时缩小图片尺寸
|
||||
cv::imshow("reprojection", img);
|
||||
auto key = cv::waitKey(1);
|
||||
if (key == 'q') break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
179
src/task/sentry_mpc.cpp
Normal file
179
src/task/sentry_mpc.cpp
Normal file
@ -0,0 +1,179 @@
|
||||
#ifdef USE_ROS2
|
||||
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "src/device/camera.hpp"
|
||||
#include "src/device/gimbal/gimbal_ros.hpp"
|
||||
#include "src/module/auto_aim/aimer.hpp"
|
||||
#include "src/module/auto_aim/multithread/commandgener.hpp"
|
||||
#include "src/module/auto_aim/multithread/mt_detector.hpp"
|
||||
#include "src/module/auto_aim/shooter.hpp"
|
||||
#include "src/module/auto_aim/solver.hpp"
|
||||
#include "src/module/auto_aim/tracker.hpp"
|
||||
#include "src/module/auto_buff/buff_aimer.hpp"
|
||||
#include "src/module/auto_buff/buff_detector.hpp"
|
||||
#include "src/module/auto_buff/buff_solver.hpp"
|
||||
#include "src/module/auto_buff/buff_target.hpp"
|
||||
#include "src/module/auto_buff/buff_type.hpp"
|
||||
#include "src/component/exiter.hpp"
|
||||
#include "src/component/img_tools.hpp"
|
||||
#include "src/component/logger.hpp"
|
||||
#include "src/component/math_tools.hpp"
|
||||
#include "src/component/plotter.hpp"
|
||||
#include "src/component/recorder.hpp"
|
||||
|
||||
const std::string keys =
|
||||
"{help h usage ? | | 输出命令行参数说明}"
|
||||
"{@config-path | | yaml配置文件路径 }";
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
cv::CommandLineParser cli(argc, argv, keys);
|
||||
auto config_path = cli.get<std::string>("@config-path");
|
||||
if (cli.has("help") || !cli.has("@config-path")) {
|
||||
cli.printMessage();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// 初始化ROS2
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<rclcpp::Node>("sentry_mpc");
|
||||
|
||||
component::Exiter exiter;
|
||||
component::Plotter plotter;
|
||||
component::Recorder recorder;
|
||||
|
||||
device::GimbalROS gimbal(config_path, node);
|
||||
device::Camera camera(config_path);
|
||||
|
||||
auto_aim::YOLO yolo(config_path, true);
|
||||
auto_aim::Solver solver(config_path);
|
||||
auto_aim::Tracker tracker(config_path, solver);
|
||||
auto_aim::Planner planner(config_path);
|
||||
|
||||
component::ThreadSafeQueue<std::optional<auto_aim::Target>, true> target_queue(1);
|
||||
target_queue.push(std::nullopt);
|
||||
|
||||
auto_buff::Buff_Detector buff_detector(config_path);
|
||||
auto_buff::Solver buff_solver(config_path);
|
||||
auto_buff::SmallTarget buff_small_target;
|
||||
auto_buff::BigTarget buff_big_target;
|
||||
auto_buff::Aimer buff_aimer(config_path);
|
||||
|
||||
cv::Mat img;
|
||||
Eigen::Quaterniond q;
|
||||
std::chrono::steady_clock::time_point t;
|
||||
|
||||
std::atomic<bool> quit = false;
|
||||
|
||||
std::atomic<device::GimbalMode> mode{device::GimbalMode::IDLE};
|
||||
auto last_mode{device::GimbalMode::IDLE};
|
||||
|
||||
// 规划线程
|
||||
auto plan_thread = std::thread([&]() {
|
||||
auto t0 = std::chrono::steady_clock::now();
|
||||
uint16_t last_bullet_count = 0;
|
||||
|
||||
while (!quit) {
|
||||
if (!target_queue.empty() && mode == device::GimbalMode::AUTO_AIM) {
|
||||
auto target = target_queue.front();
|
||||
auto gs = gimbal.state();
|
||||
auto plan = planner.plan(target, gs.bullet_speed);
|
||||
|
||||
gimbal.send(
|
||||
plan.control, plan.fire, plan.yaw, plan.yaw_vel, plan.yaw_acc, plan.pitch, plan.pitch_vel,
|
||||
plan.pitch_acc);
|
||||
|
||||
std::this_thread::sleep_for(10ms);
|
||||
} else
|
||||
std::this_thread::sleep_for(200ms);
|
||||
}
|
||||
});
|
||||
|
||||
// ROS2 spin线程
|
||||
auto ros_thread = std::thread([&]() {
|
||||
while (!quit && rclcpp::ok()) {
|
||||
rclcpp::spin_some(node);
|
||||
std::this_thread::sleep_for(1ms);
|
||||
}
|
||||
});
|
||||
|
||||
component::logger()->info("[SentryMPC] Started with ROS2 communication");
|
||||
|
||||
while (!exiter.exit() && rclcpp::ok()) {
|
||||
mode = gimbal.mode();
|
||||
|
||||
if (last_mode != mode) {
|
||||
component::logger()->info("Switch to {}", gimbal.str(mode));
|
||||
last_mode = mode.load();
|
||||
}
|
||||
|
||||
camera.read(img, t);
|
||||
auto q = gimbal.q(t);
|
||||
auto gs = gimbal.state();
|
||||
recorder.record(img, q, t);
|
||||
solver.set_R_gimbal2world(q);
|
||||
|
||||
/// 自瞄
|
||||
if (mode.load() == device::GimbalMode::AUTO_AIM) {
|
||||
auto armors = yolo.detect(img);
|
||||
auto targets = tracker.track(armors, t);
|
||||
if (!targets.empty())
|
||||
target_queue.push(targets.front());
|
||||
else
|
||||
target_queue.push(std::nullopt);
|
||||
}
|
||||
|
||||
/// 打符
|
||||
else if (mode.load() == device::GimbalMode::SMALL_BUFF || mode.load() == device::GimbalMode::BIG_BUFF) {
|
||||
buff_solver.set_R_gimbal2world(q);
|
||||
|
||||
auto power_runes = buff_detector.detect(img);
|
||||
|
||||
buff_solver.solve(power_runes);
|
||||
|
||||
auto_aim::Plan buff_plan;
|
||||
if (mode.load() == device::GimbalMode::SMALL_BUFF) {
|
||||
buff_small_target.get_target(power_runes, t);
|
||||
auto target_copy = buff_small_target;
|
||||
buff_plan = buff_aimer.mpc_aim(target_copy, t, gs, true);
|
||||
} else if (mode.load() == device::GimbalMode::BIG_BUFF) {
|
||||
buff_big_target.get_target(power_runes, t);
|
||||
auto target_copy = buff_big_target;
|
||||
buff_plan = buff_aimer.mpc_aim(target_copy, t, gs, true);
|
||||
}
|
||||
gimbal.send(
|
||||
buff_plan.control, buff_plan.fire, buff_plan.yaw, buff_plan.yaw_vel, buff_plan.yaw_acc,
|
||||
buff_plan.pitch, buff_plan.pitch_vel, buff_plan.pitch_acc);
|
||||
|
||||
} else
|
||||
gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
|
||||
}
|
||||
|
||||
quit = true;
|
||||
if (plan_thread.joinable()) plan_thread.join();
|
||||
if (ros_thread.joinable()) ros_thread.join();
|
||||
gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
|
||||
|
||||
rclcpp::shutdown();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
#include <iostream>
|
||||
|
||||
int main()
|
||||
{
|
||||
std::cerr << "Error: This program requires ROS2 support. Please build with ROS2 enabled." << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
#endif // USE_ROS2
|
||||
@ -1,125 +0,0 @@
|
||||
#include <fmt/core.h>
|
||||
|
||||
#include <chrono>
|
||||
#include <future>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <nlohmann/json.hpp>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "src/device/camera.hpp"
|
||||
#include "src/device/cboard.hpp"
|
||||
#include "src/device/ros2/ros2.hpp"
|
||||
#include "src/device/usbcamera/usbcamera.hpp"
|
||||
#include "src/module/auto_aim/aimer.hpp"
|
||||
#include "src/module/auto_aim/shooter.hpp"
|
||||
#include "src/module/auto_aim/solver.hpp"
|
||||
#include "src/module/auto_aim/tracker.hpp"
|
||||
#include "src/module/auto_aim/yolo.hpp"
|
||||
#include "src/module/omniperception/decider.hpp"
|
||||
#include "src/module/omniperception/perceptron.hpp"
|
||||
#include "src/component/exiter.hpp"
|
||||
#include "src/component/img_tools.hpp"
|
||||
#include "src/component/logger.hpp"
|
||||
#include "src/component/math_tools.hpp"
|
||||
#include "src/component/plotter.hpp"
|
||||
#include "src/component/recorder.hpp"
|
||||
|
||||
using namespace std::chrono;
|
||||
|
||||
const std::string keys =
|
||||
"{help h usage ? | | 输出命令行参数说明}"
|
||||
"{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }";
|
||||
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
component::Exiter exiter;
|
||||
component::Plotter plotter;
|
||||
component::Recorder recorder;
|
||||
|
||||
cv::CommandLineParser cli(argc, argv, keys);
|
||||
if (cli.has("help")) {
|
||||
cli.printMessage();
|
||||
return 0;
|
||||
}
|
||||
auto config_path = cli.get<std::string>(0);
|
||||
|
||||
device::ROS2 ros2;
|
||||
device::CBoard cboard(config_path);
|
||||
device::Camera camera(config_path);
|
||||
device::USBCamera usbcam1("video0", config_path);
|
||||
device::USBCamera usbcam2("video2", config_path);
|
||||
device::USBCamera usbcam3("video4", config_path);
|
||||
device::USBCamera usbcam4("video6", config_path);
|
||||
|
||||
auto_aim::YOLO yolo(config_path, false);
|
||||
auto_aim::Solver solver(config_path);
|
||||
auto_aim::Tracker tracker(config_path, solver);
|
||||
auto_aim::Aimer aimer(config_path);
|
||||
auto_aim::Shooter shooter(config_path);
|
||||
|
||||
omniperception::Decider decider(config_path);
|
||||
omniperception::Perceptron perceptron(&usbcam1, &usbcam2, &usbcam3, &usbcam4, config_path);
|
||||
|
||||
omniperception::DetectionResult switch_target;
|
||||
cv::Mat img;
|
||||
std::chrono::steady_clock::time_point timestamp;
|
||||
device::Command last_command;
|
||||
|
||||
while (!exiter.exit()) {
|
||||
camera.read(img, timestamp);
|
||||
Eigen::Quaterniond q = cboard.imu_at(timestamp - 1ms);
|
||||
recorder.record(img, q, timestamp);
|
||||
/// 自瞄核心逻辑
|
||||
solver.set_R_gimbal2world(q);
|
||||
|
||||
Eigen::Vector3d gimbal_pos = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
|
||||
|
||||
auto armors = yolo.detect(img);
|
||||
|
||||
decider.get_invincible_armor(ros2.subscribe_enemy_status());
|
||||
|
||||
decider.armor_filter(armors);
|
||||
|
||||
decider.set_priority(armors);
|
||||
|
||||
auto detection_queue = perceptron.get_detection_queue();
|
||||
|
||||
decider.sort(detection_queue);
|
||||
|
||||
auto [switch_target, targets] = tracker.track(detection_queue, armors, timestamp);
|
||||
|
||||
device::Command command{false, false, 0, 0};
|
||||
|
||||
/// 全向感知逻辑
|
||||
if (tracker.state() == "switching") {
|
||||
command.control = switch_target.armors.empty() ? false : true;
|
||||
command.shoot = false;
|
||||
command.pitch = component::limit_rad(switch_target.delta_pitch);
|
||||
command.yaw = component::limit_rad(switch_target.delta_yaw + gimbal_pos[0]);
|
||||
}
|
||||
|
||||
else if (tracker.state() == "lost") {
|
||||
command = decider.decide(detection_queue);
|
||||
command.yaw = component::limit_rad(command.yaw + gimbal_pos[0]);
|
||||
}
|
||||
|
||||
else {
|
||||
command = aimer.aim(targets, timestamp, cboard.bullet_speed);
|
||||
}
|
||||
|
||||
/// 发射逻辑
|
||||
command.shoot = shooter.shoot(command, aimer, targets, gimbal_pos);
|
||||
// command.shoot = false;
|
||||
|
||||
cboard.send(command);
|
||||
|
||||
/// ROS2通信
|
||||
Eigen::Vector4d target_info = decider.get_target_info(armors, targets);
|
||||
|
||||
ros2.publish(target_info);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user