add balance_infantry
This commit is contained in:
parent
59a7072002
commit
9adee3edec
@ -111,4 +111,10 @@ target_link_libraries(uav ${OpenCV_LIBS} fmt::fmt yaml-cpp auto_aim auto_buff co
|
|||||||
add_executable(uav_debug src/task/uav_debug.cpp)
|
add_executable(uav_debug src/task/uav_debug.cpp)
|
||||||
target_link_libraries(uav_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
|
target_link_libraries(uav_debug ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
|
||||||
|
|
||||||
|
add_executable(balance_infantry src/task/balance_infantry.cpp)
|
||||||
|
target_link_libraries(balance_infantry ${OpenCV_LIBS} fmt::fmt yaml-cpp nlohmann_json::nlohmann_json auto_aim auto_buff component device)
|
||||||
|
|
||||||
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
70
src/task/README.md
Normal file
70
src/task/README.md
Normal file
@ -0,0 +1,70 @@
|
|||||||
|
# Task 目录说明
|
||||||
|
|
||||||
|
本目录存放各兵种的主程序，`test/` 子目录存放测试用例。
|
||||||
|
|
||||||
|
## 主程序
|
||||||
|
|
||||||
|
### 步兵（Standard）
|
||||||
|
|
||||||
|
| 文件 | 设备 | 线程 | 说明 |
|
||||||
|
|------|------|------|------|
|
||||||
|
| `standard.cpp` | CBoard (CAN) | 单线程 | 基础步兵自瞄，仅发送 yaw/pitch 位置指令 |
|
||||||
|
| `standard_mpc.cpp` | Gimbal (串口) | 多线程 | MPC 规划步兵，发送 pos/vel/acc，支持自瞄 + 打符 |
|
||||||
|
| `mt_standard.cpp` | CBoard (CAN) | 多线程 | 多线程步兵，检测与主循环分离，支持自瞄 + 打符 |
|
||||||
|
|
||||||
|
```bash
|
||||||
|
./build/standard configs/standard3.yaml
|
||||||
|
./build/standard_mpc configs/standard4.yaml
|
||||||
|
./build/mt_standard configs/standard3.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
### 无人机（UAV）
|
||||||
|
|
||||||
|
| 文件 | 设备 | 线程 | 说明 |
|
||||||
|
|------|------|------|------|
|
||||||
|
| `uav.cpp` | CBoard (CAN) | 单线程 | 无人机自瞄 + 打符，按模式切换 |
|
||||||
|
| `uav_debug.cpp` | CBoard (CAN) | 单线程 | 无人机调试版，含重投影可视化和 PlotJuggler 数据输出 |
|
||||||
|
|
||||||
|
```bash
|
||||||
|
./build/uav configs/uav.yaml
|
||||||
|
./build/uav_debug configs/uav.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
### 哨兵（Sentry）
|
||||||
|
|
||||||
|
| 文件 | 设备 | 线程 | 说明 |
|
||||||
|
|------|------|------|------|
|
||||||
|
| `sentry.cpp` | CBoard (CAN) | 单线程 | 哨兵基础版，全向感知 + ROS2 通信 |
|
||||||
|
| `sentry_multithread.cpp` | CBoard (CAN) | 多线程 | 哨兵多线程版，全向感知 + ROS2 |
|
||||||
|
| `sentry_bp.cpp` | CBoard (CAN) | 单线程 | 哨兵 + 后置摄像头，全向感知 + ROS2 |
|
||||||
|
| `sentry_debug.cpp` | CBoard (CAN) | 单线程 | 哨兵调试版，含可视化 + ROS2 |
|
||||||
|
|
||||||
|
```bash
|
||||||
|
./build/sentry configs/sentry.yaml
|
||||||
|
./build/sentry_multithread configs/sentry.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
### 调试程序（Debug）
|
||||||
|
|
||||||
|
| 文件 | 设备 | 线程 | 说明 |
|
||||||
|
|------|------|------|------|
|
||||||
|
| `auto_aim_debug_mpc.cpp` | Gimbal (串口) | 多线程 | 自瞄 MPC 调试，含重投影可视化和 PlotJuggler 数据 |
|
||||||
|
| `mt_auto_aim_debug.cpp` | CBoard (CAN) | 多线程 | 多线程自瞄调试，含可视化，支持自瞄 + 打符 |
|
||||||
|
| `auto_buff_debug.cpp` | CBoard (CAN) | 单线程 | 打符调试，含可视化和 PlotJuggler 数据 |
|
||||||
|
| `auto_buff_debug_mpc.cpp` | Gimbal (串口) | 单线程 | 打符 MPC 调试，含可视化和 PlotJuggler 数据 |
|
||||||
|
|
||||||
|
```bash
|
||||||
|
./build/auto_aim_debug_mpc configs/sentry.yaml
|
||||||
|
./build/mt_auto_aim_debug configs/sentry.yaml
|
||||||
|
./build/auto_buff_debug configs/uav.yaml
|
||||||
|
./build/auto_buff_debug_mpc configs/uav.yaml
|
||||||
|
```
|
||||||
|
|
||||||
|
## 设备对比
|
||||||
|
|
||||||
|
- CBoard (CAN)：通过 CAN 总线通信，发送 `Command{control, shoot, yaw, pitch}`，适用于 C 板下位机
|
||||||
|
- Gimbal (串口)：通过 USB 串口通信，发送 `pos/vel/acc`，配合 MPC 规划器使用，适用于达妙 MC02
|
||||||
|
|
||||||
|
## 测试用例
|
||||||
|
|
||||||
|
测试用例位于 `test/` 子目录，用于单独验证各模块功能，详见 [test/](test/) 目录。
|
||||||
120
src/task/balance_infantry.cpp
Normal file
120
src/task/balance_infantry.cpp
Normal file
@ -0,0 +1,120 @@
|
|||||||
|
#include <fmt/core.h>
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <nlohmann/json.hpp>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
|
||||||
|
#include "src/device/camera.hpp"
|
||||||
|
#include "src/device/cboard.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/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"
|
||||||
|
|
||||||
|
using namespace std::chrono;
|
||||||
|
|
||||||
|
const std::string keys =
|
||||||
|
"{help h usage ? | | 输出命令行参数说明}"
|
||||||
|
"{@config-path | configs/standard4.yaml | 位置参数，yaml配置文件路径 }";
|
||||||
|
|
||||||
|
int main(int argc, char * argv[])
|
||||||
|
{
|
||||||
|
cv::CommandLineParser cli(argc, argv, keys);
|
||||||
|
auto config_path = cli.get<std::string>(0);
|
||||||
|
if (cli.has("help") || config_path.empty()) {
|
||||||
|
cli.printMessage();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
component::Exiter exiter;
|
||||||
|
component::Plotter plotter;
|
||||||
|
component::Recorder recorder;
|
||||||
|
|
||||||
|
device::CBoard cboard(config_path);
|
||||||
|
device::Camera camera(config_path);
|
||||||
|
|
||||||
|
auto_aim::YOLO detector(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);
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
auto mode = device::Mode::idle;
|
||||||
|
auto last_mode = device::Mode::idle;
|
||||||
|
|
||||||
|
while (!exiter.exit()) {
|
||||||
|
camera.read(img, t);
|
||||||
|
q = cboard.imu_at(t - 1ms);
|
||||||
|
mode = cboard.mode;
|
||||||
|
|
||||||
|
if (last_mode != mode) {
|
||||||
|
component::logger()->info("Switch to {}", device::MODES[mode]);
|
||||||
|
last_mode = mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
solver.set_R_gimbal2world(q);
|
||||||
|
|
||||||
|
Eigen::Vector3d ypr = component::eulers(solver.R_gimbal2world(), 2, 1, 0);
|
||||||
|
|
||||||
|
/// 自瞄
|
||||||
|
if (mode == device::Mode::auto_aim || mode == device::Mode::outpost) {
|
||||||
|
auto armors = detector.detect(img);
|
||||||
|
|
||||||
|
auto targets = tracker.track(armors, t);
|
||||||
|
|
||||||
|
auto command = aimer.aim(targets, t, cboard.bullet_speed);
|
||||||
|
|
||||||
|
command.shoot = shooter.shoot(command, aimer, targets, ypr);
|
||||||
|
|
||||||
|
cboard.send(command);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// 打符
|
||||||
|
else if (mode == device::Mode::small_buff || mode == device::Mode::big_buff) {
|
||||||
|
buff_solver.set_R_gimbal2world(q);
|
||||||
|
|
||||||
|
auto power_runes = buff_detector.detect(img);
|
||||||
|
|
||||||
|
buff_solver.solve(power_runes);
|
||||||
|
|
||||||
|
device::Command buff_command;
|
||||||
|
if (mode == device::Mode::small_buff) {
|
||||||
|
buff_small_target.get_target(power_runes, t);
|
||||||
|
auto target_copy = buff_small_target;
|
||||||
|
buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true);
|
||||||
|
} else if (mode == device::Mode::big_buff) {
|
||||||
|
buff_big_target.get_target(power_runes, t);
|
||||||
|
auto target_copy = buff_big_target;
|
||||||
|
buff_command = buff_aimer.aim(target_copy, t, cboard.bullet_speed, true);
|
||||||
|
}
|
||||||
|
cboard.send(buff_command);
|
||||||
|
}
|
||||||
|
|
||||||
|
else
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
142
src/task/balance_infantry_mpc.cpp
Normal file
142
src/task/balance_infantry_mpc.cpp
Normal file
@ -0,0 +1,142 @@
|
|||||||
|
#include <chrono>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
|
#include "src/device/camera.hpp"
|
||||||
|
#include "src/device/dm_imu/dm_imu.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 | configs/standard4.yaml | 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
component::Exiter exiter;
|
||||||
|
component::Plotter plotter;
|
||||||
|
component::Recorder recorder;
|
||||||
|
|
||||||
|
device::Gimbal gimbal(config_path);
|
||||||
|
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([&]() {
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
while (!exiter.exit()) {
|
||||||
|
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();
|
||||||
|
gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user