79 lines
2.1 KiB
C++
79 lines
2.1 KiB
C++
#include "tasks/auto_aim/planner/planner.hpp"
|
|
|
|
#include <chrono>
|
|
#include <nlohmann/json.hpp>
|
|
#include <opencv2/opencv.hpp>
|
|
#include <thread>
|
|
|
|
#include "io/gimbal/gimbal.hpp"
|
|
#include "tools/exiter.hpp"
|
|
#include "tools/logger.hpp"
|
|
#include "tools/math_tools.hpp"
|
|
#include "tools/plotter.hpp"
|
|
|
|
using namespace std::chrono_literals;
|
|
|
|
const std::string keys =
|
|
"{help h usage ? | | 输出命令行参数说明 }"
|
|
"{d | 3.0 | Target距离(m) }"
|
|
"{w | 5.0 | Target角速度(rad/s) }"
|
|
"{@config-path | | yaml配置文件路径 }";
|
|
|
|
int main(int argc, char * argv[])
|
|
{
|
|
cv::CommandLineParser cli(argc, argv, keys);
|
|
auto config_path = cli.get<std::string>("@config-path");
|
|
auto d = cli.get<double>("d");
|
|
auto w = cli.get<double>("w");
|
|
if (cli.has("help") || !cli.has("@config-path")) {
|
|
cli.printMessage();
|
|
return 0;
|
|
}
|
|
|
|
tools::Exiter exiter;
|
|
tools::Plotter plotter;
|
|
|
|
io::Gimbal gimbal(config_path);
|
|
auto_aim::Planner planner(config_path);
|
|
auto_aim::Target target(d, w, 0.2, 0.1);
|
|
|
|
auto t0 = std::chrono::steady_clock::now();
|
|
|
|
while (!exiter.exit()) {
|
|
target.predict(0.01);
|
|
|
|
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);
|
|
|
|
nlohmann::json data;
|
|
data["t"] = tools::delta_time(std::chrono::steady_clock::now(), t0);
|
|
|
|
data["gimbal_yaw"] = gs.yaw;
|
|
data["gimbal_yaw_vel"] = gs.yaw_vel;
|
|
data["gimbal_pitch"] = gs.pitch;
|
|
data["gimbal_pitch_vel"] = gs.pitch_vel;
|
|
|
|
data["target_yaw"] = plan.target_yaw;
|
|
data["target_pitch"] = plan.target_pitch;
|
|
|
|
data["plan_yaw"] = plan.yaw;
|
|
data["plan_yaw_vel"] = plan.yaw_vel;
|
|
data["plan_yaw_acc"] = plan.yaw_acc;
|
|
|
|
data["plan_pitch"] = plan.pitch;
|
|
data["plan_pitch_vel"] = plan.pitch_vel;
|
|
data["plan_pitch_acc"] = plan.pitch_acc;
|
|
|
|
plotter.plot(data);
|
|
|
|
std::this_thread::sleep_for(10ms);
|
|
}
|
|
|
|
gimbal.send(false, false, 0, 0, 0, 0, 0, 0);
|
|
|
|
return 0;
|
|
} |