106 lines
3.0 KiB
C++
106 lines
3.0 KiB
C++
#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;
|
||
} |