diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp new file mode 100644 index 0000000..6443340 --- /dev/null +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -0,0 +1,114 @@ +#include "fire_controller.hpp" + +#include +#include + +#include "../identifier/identified_armor.hpp" +#include "../predictor/live_target_manager/live_target_manager.hpp" +#include "../predictor/target_snapshot_manager/target_snapshot_manager.hpp" +#include "../state_machine/state_machine.hpp" +#include "data/fire_control.hpp" +#include "fire_decision.hpp" +#include "interfaces/target_predictor.hpp" + +namespace world_exe::tongji::fire_control { + +using TargetSnapshotManager = predictor::TargetSnapshotManager; +using StateMachine = state_machine::StateMachine; +using IdentifiedArmor = identifier::IdentifiedArmor; +using CarIDFlag = enumeration::CarIDFlag; +using LiveTargetManager = predictor::LiveTargetManager; +using TimeStamp = time_stamp::TimeStamp; + +class FireController::Impl { +public: + Impl(bool auto_fire, const double& control_delay_in_second, const double& bullet_speed, + double yaw_offset, double pitch_offset, + std::shared_ptr state_machine, + std::shared_ptr live_target_manager) + : control_delay_(control_delay_in_second) + , bullet_speed_(bullet_speed) + , fire_decision_(std::make_unique(auto_fire)) + , state_machine_(state_machine) + , live_target_manager_(std::move(live_target_manager)) { } + + // TODO:std::time_t + const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const { + + if (!identified_armors_ || !fire_decision_ || !state_machine_ || !live_target_manager_) + return { .fire_allowance = false }; + + auto converged_cars = state_machine_->GetAllowdToFires(); + auto snapshot_manager = live_target_manager_->GetPredictor(converged_cars); + if (!snapshot_manager) + return data::FireControl { .time_stamp = time_stamp_.GetTimeStamp(), + .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), + .fire_allowance = false }; + + auto armors_in_gimbal_control = snapshot_manager->Predictor(control_delay_); + allowed_target_id_ = state_machine_->GetAllowdToFires(); + + auto target_gimbal_spacing = + armors_in_gimbal_control->GetArmors(allowed_target_id_).front(); + + auto gimbal_command = + std::dynamic_pointer_cast(snapshot_manager)->GetGimbalCommand(); + + auto fire_command = + fire_decision_->ShouldFire(gimbal_command, target_gimbal_spacing.position); + firable_ = fire_command; + + data::FireControl result; + result.fire_allowance = fire_command; + result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; + result.time_stamp = time_stamp_.GetTimeStamp(); + return result; + } + + const CarIDFlag GetAttackCarId() const { + if (firable_) { } + return allowed_target_id_; + } + + void Update(std::shared_ptr armors, const double& gimbal_yaw) { + time_stamp_.SetTimeStamp(std::time(nullptr)); + UpdateIdentifiedArmor(armors); + UpdateGimbalPosition(gimbal_yaw); + } + +private: + void UpdateIdentifiedArmor(std::shared_ptr armors) { + identified_armors_ = armors; + } + void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; + TimeStamp GetTimeStamp() const { return time_stamp_; } + + double gimbal_yaw_; + double control_delay_; + double bullet_speed_; + + mutable CarIDFlag allowed_target_id_; + mutable double firable_ { false }; + + std::shared_ptr identified_armors_; + + std::unique_ptr fire_decision_; + time_stamp::TimeStamp time_stamp_ { std::time(nullptr) }; + + std::shared_ptr state_machine_; + std::shared_ptr live_target_manager_; +}; + +FireController::FireController(std::shared_ptr state_machine, bool auto_fire, + const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, + double pitch_offset, std::shared_ptr live_target_manager) + : pimpl_(std::make_unique(auto_fire, control_delay_in_second, bullet_speed, yaw_offset, + pitch_offset, state_machine, live_target_manager)) { } + +const data ::FireControl FireController::CalculateTarget(const std ::time_t& time_duration) const { + return pimpl_->CalculateTarget(time_duration); +} + +const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp new file mode 100644 index 0000000..19343e0 --- /dev/null +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include + +#include "../time_stamp/time_stamp.hpp" +#include "interfaces/armor_in_image.hpp" +#include "interfaces/car_state.hpp" +#include "interfaces/fire_controller.hpp" +#include "interfaces/target_predictor.hpp" + +namespace world_exe::tongji::fire_control { + +class FireController final : public interfaces::IFireControl { +public: + FireController(std::shared_ptr state_machine, bool auto_fire, + const double& control_delay_in_second, const double& bullet_speed, double yaw_offset, + double pitch_offset, std::shared_ptr live_target_manager); + + const data ::FireControl CalculateTarget(const std ::time_t& time_duration) const override; + const enumeration ::CarIDFlag GetAttackCarId() const override; + + void Update(std::shared_ptr armors, const double& gimbal_yaw); + + time_stamp::TimeStamp GetTimeStamp() const; + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp new file mode 100644 index 0000000..46088e8 --- /dev/null +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include + +#include "tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp" + +namespace world_exe::tongji::fire_control { + +class FireDecision { +public: + explicit FireDecision(const bool& auto_fire, const double& first_tolerance = 5, + const double& second_tolerance = 2, const double& judge_distance = 3) + : auto_fire_(auto_fire) + , last_gimbal_command_({ std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN() }) + , first_tolerance_(first_tolerance) + , second_tolerance_(second_tolerance) + , judge_distance_(judge_distance) { } + + bool ShouldFire( + predictor::GimbalCommand gimbal_command, const Eigen::Vector3d& valid_target_pos) { + + if (!auto_fire_) return false; + const auto& tolerance = std::sqrt(valid_target_pos.x() * valid_target_pos.x() + + valid_target_pos.y() * valid_target_pos.y()) + > judge_distance_ + ? second_tolerance_ + : first_tolerance_; + + if (std::abs(last_gimbal_command_.yaw - gimbal_yaw_) + < tolerance * 2 // 此时认为command突变不应该射击 + && std::abs(gimbal_yaw_ - last_gimbal_command_.yaw) < tolerance) { + last_gimbal_command_ = gimbal_command; + return true; + } + last_gimbal_command_ = gimbal_command; + return false; + } + +private: + bool auto_fire_; + predictor::GimbalCommand last_gimbal_command_; + + double gimbal_yaw_; + + double first_tolerance_ { 5 }; // 近距离射击容差，degree + double second_tolerance_ { 2 }; // 远距离射击容差，degree + double judge_distance_ { 3 }; // 距离判断阈值 +}; + +} \ No newline at end of file diff --git a/src/tongji/identifier/armor_filter.hpp b/src/tongji/identifier/armor_filter.hpp new file mode 100644 index 0000000..21699e0 --- /dev/null +++ b/src/tongji/identifier/armor_filter.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "util/index.hpp" + +namespace world_exe::tongji::identifier { + +class ArmorFilter { +public: + explicit ArmorFilter() + : invincible_armor_({ }) { } + std::vector FilterArmor( + std::vector armors) const { + + // 25赛季没有5号装甲板 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::InfantryV; })); + // 不打前哨站 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [](const auto& armor) { return armor.id == enumeration::ArmorIdFlag::Outpost; })); + // 过滤掉刚复活无敌的装甲板 + armors.erase(std::remove_if(armors.begin(), armors.end(), + [&](const auto& armor) { return invincible_armor_.count(armor.id); }), + armors.end()); + + return std::move(armors); + } + + void Update(enumeration::CarIDFlag ids) { + invincible_armor_.clear(); + for (auto id : util::enumeration::ExpandArmorIdFlags(ids)) { + invincible_armor_.emplace(std::move(id)); + } + } + +private: + std::unordered_set invincible_armor_; +}; +} \ No newline at end of file diff --git a/src/tongji/identifier/identified_armor.hpp b/src/tongji/identifier/identified_armor.hpp index b6ac9ff..14295e5 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -6,6 +6,7 @@ #include "util/index.hpp" namespace world_exe::tongji::identifier { + class IdentifiedArmor final : public interfaces::IArmorInImage, public interfaces::ITimeStamped { public: explicit IdentifiedArmor(const std::vector& armors) { @@ -23,8 +24,12 @@ class IdentifiedArmor final : public interfaces::IArmorInImage, public interface return armors_[util::enumeration::GetIndex(armor_id)]; } + static IdentifiedArmor DecorateIArmorInImage(const interfaces::IArmorInImage& armor) { + throw std::runtime_error("Not implemented"); + } + private: - std::time_t time_stamp_ { 0 }; + std::time_t time_stamp_ { std::time(nullptr) }; std::array, 8> armors_; }; } \ No newline at end of file diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index c314f74..f70af8f 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -19,7 +19,7 @@ #include "enum/armor_id.hpp" #include "identified_armor.hpp" #include "interfaces/armor_in_image.hpp" -#include "tongji/identifier/classifier.hpp" +#include "../identifier/classifier.hpp" #include "util/logger.hpp" #include "util/stringifier.hpp" diff --git a/src/tongji/predictor/in_gimbal_control_armor.hpp b/src/tongji/predictor/in_gimbal_control_armor.hpp index 117203a..c012806 100644 --- a/src/tongji/predictor/in_gimbal_control_armor.hpp +++ b/src/tongji/predictor/in_gimbal_control_armor.hpp @@ -4,10 +4,10 @@ #include #include +#include "../time_stamp/time_stamp.hpp" #include "data/armor_gimbal_control_spacing.hpp" #include "enum/armor_id.hpp" #include "interfaces/armor_in_gimbal_control.hpp" -#include "tongji/predictor/time_stamp.hpp" namespace world_exe::tongji::predictor { @@ -28,7 +28,7 @@ class InGimbalControlArmor final : public interfaces::IArmorInGimbalControl { const interfaces::ITimeStamped& GetTimeStamped() const override { return time_stamp_; } private: - TimeStamp time_stamp_; + time_stamp::TimeStamp time_stamp_; std::unordered_map> armors_map_; static const std::vector empty; diff --git a/src/util/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp similarity index 68% rename from src/util/extended_kalman_filter.hpp rename to src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp index 73fc86b..ce90cfd 100644 --- a/src/util/extended_kalman_filter.hpp +++ b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp @@ -1,23 +1,36 @@ #pragma once -#include #include #include #include #include -namespace world_exe::util { +#include + +namespace world_exe::tongji::predictor { +template // class ExtendedKalmanFilter { public: - Eigen::VectorXd x; - Eigen::MatrixXd P; + using XVec = Eigen::Matrix; + using ZVec = Eigen::Matrix; + using AMat = Eigen::Matrix; + using PMat = Eigen::Matrix; + using PDig = Eigen::Matrix; + using RMat = Eigen::Matrix; + using RDig = Eigen::Matrix; + using QMat = Eigen::Matrix; + using HMat = Eigen::Matrix; + + XVec x; + PMat P; ExtendedKalmanFilter() = default; ExtendedKalmanFilter( - const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, - std::function x_add = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) { + const XVec& x0, const PMat& P0, + std::function x_add = [](const XVec& a, const XVec& b) { + return a + b; + }) { data["residual_yaw"] = 0.0; data["residual_pitch"] = 0.0; data["residual_distance"] = 0.0; @@ -29,13 +42,12 @@ class ExtendedKalmanFilter { data["recent_nis_failures"] = 0.0; } - Eigen::VectorXd Update( - const double& dt, const Eigen::MatrixXd& A, const Eigen::MatrixXd& Q, - std::function f, - const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R, - std::function h, - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a - b; }) { + XVec Update( + const double& dt, const AMat& A, const QMat& Q, + std::function f, const ZVec& z, const HMat& H, + const RMat& R, std::function h, + std::function z_subtract = + [](const ZVec& a, const ZVec& b) { return a - b; }) { auto x_n = f(x, dt); @@ -90,8 +102,8 @@ class ExtendedKalmanFilter { double last_nis; private: - Eigen::MatrixXd I; - std::function x_add; + Eigen::Matrix I; + std::function x_add; int nees_count_ = 0; int nis_count_ = 0; diff --git a/src/tongji/predictor/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp similarity index 72% rename from src/tongji/predictor/predict_model.hpp rename to src/tongji/predictor/kalman_filter/predict_model.hpp index 8fe4b43..5af8395 100644 --- a/src/tongji/predictor/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -2,10 +2,12 @@ #include +#include #include #include #include "enum/car_id.hpp" +#include "extended_kalman_filter.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -54,23 +56,27 @@ class PredictModel { int GetArmorNum() const { return armor_num_; } // 防止夹角求和出现异常值 - std::function x_add = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { - Eigen::VectorXd c = a + b; - c(6) = util::math::clamp_pm_pi(c(6)); + std::function::XVec( + const ExtendedKalmanFilter<11, 4>::XVec&, const ExtendedKalmanFilter<11, 4>::XVec&)> + x_add = [](const ExtendedKalmanFilter<11, 4>::XVec& a, + const ExtendedKalmanFilter<11, 4>::XVec& b) { + ExtendedKalmanFilter<11, 4>::XVec c = a + b; + c(6) = util::math::clamp_pm_pi(c(6)); return c; }; // 防止夹角求和出现异常值 - std::function f = - [=, this](const Eigen::VectorXd& x, const double& dt) -> Eigen::VectorXd { - Eigen::VectorXd x_prior = A(dt) * x; - x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); + std::function::XVec( + const ExtendedKalmanFilter<11, 4>::XVec& x, const double& dt)> + f = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, + const double& dt) -> ExtendedKalmanFilter<11, 4>::XVec { + ExtendedKalmanFilter<11, 4>::XVec x_prior = A(dt) * x; + x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); return x_prior; }; - int MatchArmor(const Eigen::VectorXd& x, const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, + int MatchArmor(const ExtendedKalmanFilter<11, 4>::XVec& x, + const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) const { const auto& xyza_list = GetArmorXYZAList(x); @@ -104,7 +110,7 @@ class PredictModel { } // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& x, int id) const { + Eigen::Vector3d h_armor_xyz(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); @@ -116,7 +122,7 @@ class PredictModel { return { armor_x, armor_y, armor_z }; } - Eigen::MatrixXd H(const Eigen::VectorXd& x, int id) const { + ExtendedKalmanFilter<11, 4>::HMat H(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto use_l_h = (armor_num_ == 4) && (id == 1 || id == 3); @@ -132,7 +138,7 @@ class PredictModel { auto dz_dh = (use_l_h) ? 1.0 : 0.0; // clang-format off - Eigen::MatrixXd H_armor_xyza{ + Eigen::MatrixXd H_armor_xyza{ {1, 0, 0, 0, 0, 0, dx_da, 0, dx_dr, dx_dl, 0}, {0, 0, 1, 0, 0, 0, dy_da, 0, dy_dr, dy_dl, 0}, {0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dz_dh}, @@ -143,7 +149,7 @@ class PredictModel { Eigen::VectorXd armor_xyz = h_armor_xyz(x, id); Eigen::MatrixXd H_armor_ypd = util::math::xyz2ypd_jacobian(armor_xyz); // clang-format off - Eigen::MatrixXd H_armor_ypda{ + Eigen::Matrix H_armor_ypda{ {H_armor_ypd(0, 0), H_armor_ypd(0, 1), H_armor_ypd(0, 2), 0}, {H_armor_ypd(1, 0), H_armor_ypd(1, 1), H_armor_ypd(1, 2), 0}, {H_armor_ypd(2, 0), H_armor_ypd(2, 1), H_armor_ypd(2, 2), 0}, @@ -154,13 +160,13 @@ class PredictModel { return H_armor_ypda * H_armor_xyza; } - Eigen::MatrixXd R(const Eigen::Vector3d& armor_xyz_in_world, + ExtendedKalmanFilter<11, 4>::RMat R(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, int id) { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; auto center_yaw = std::atan2(armor_xyz_in_world(1), armor_xyz_in_world(0)); auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_world(0) - center_yaw); - Eigen::VectorXd R_dig(4); + ExtendedKalmanFilter<11, 4>::RDig R_dig(4); R_dig << 4e-3, 4e-3, log(std::abs(delta_angle) + 1) + 1, log(std::abs(armor_ypd_in_world(2)) + 1) / 200 + 9e-2; @@ -168,10 +174,11 @@ class PredictModel { return R_dig.asDiagonal(); } - Eigen::MatrixXd A(double dt) const { + ExtendedKalmanFilter<11, 4>::AMat A(double dt) const { // 状态转移矩阵 + ExtendedKalmanFilter<11, 4>::AMat _A; // clang-format off - return (Eigen::MatrixXd(11, 11) << + _A<< 1, dt, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, dt, 0, 0, 0, 0, 0, 0, 0, @@ -182,11 +189,13 @@ class PredictModel { 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; + //clang-format on + return _A; } - //clang-format on + - Eigen::MatrixXd Q(double dt) const { +ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { // Piecewise White Noise Model // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb double v1, v2; @@ -202,8 +211,10 @@ class PredictModel { auto c = dt * dt; // 预测过程噪声偏差的方差 + Eigen::Matrix _Q; // clang-format off - return (Eigen::MatrixXd(11, 11) << + _Q + << a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, b * v1, c * v1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, a * v1, b * v1, 0, 0, 0, 0, 0, 0, 0, @@ -214,29 +225,33 @@ class PredictModel { 0, 0, 0, 0, 0, 0, b * v2, c * v2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; // clang-format on + return _Q; } - std::function h = [this](const Eigen::VectorXd& x, - int id) { - Eigen::Vector3d xyz = this->h_armor_xyz(x, id); - Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); - double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); - return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; - }; + std::function::ZVec(const ExtendedKalmanFilter<11, 4>::XVec&, int)> + h = [this](const ExtendedKalmanFilter<11, 4>::XVec& x, int id) { + Eigen::Vector3d xyz = this->h_armor_xyz(x, id); + Eigen::Vector3d ypd = util::math::xyz2ypd(xyz); + double angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / this->armor_num_); + return Eigen::Vector4d { ypd(0), ypd(1), ypd(2), angle }; + }; // 防止夹角求差出现异常值 - std::function z_subtract = - [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { - Eigen::VectorXd c = a - b; - c(0) = util::math::clamp_pm_pi(c(0)); - c(1) = util::math::clamp_pm_pi(c(1)); - c(3) = util::math::clamp_pm_pi(c(3)); + std::function::ZVec( + const ExtendedKalmanFilter<11, 4>::ZVec&, const ExtendedKalmanFilter<11, 4>::ZVec&)> + z_subtract = [](const ExtendedKalmanFilter<11, 4>::ZVec& a, + const ExtendedKalmanFilter<11, 4>::ZVec& b) { + ExtendedKalmanFilter<11, 4>::ZVec c = a - b; + c(0) = util::math::clamp_pm_pi(c(0)); + c(1) = util::math::clamp_pm_pi(c(1)); + c(3) = util::math::clamp_pm_pi(c(3)); return c; }; - std::vector GetArmorXYZAList(const Eigen::VectorXd& ekf_x) const { + std::vector GetArmorXYZAList( + const ExtendedKalmanFilter<11, 4>::XVec& ekf_x) const { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { @@ -251,7 +266,7 @@ class PredictModel { int armor_num_; enumeration::CarIDFlag car_id_; - Eigen::VectorXd P0_dig_; + ExtendedKalmanFilter<11, 4>::PDig P0_dig_; double radius_; }; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp deleted file mode 100644 index c9fece5..0000000 --- a/src/tongji/predictor/live_target.hpp +++ /dev/null @@ -1,208 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/car_id.hpp" -#include "predict_model.hpp" -#include "util/extended_kalman_filter.hpp" -#include "util/math.hpp" - -namespace world_exe::tongji::predictor { - -struct TargetStatus { - bool jumped = false; - bool switched = false; - bool converged = false; - bool diverged = false; - bool lost = false; - bool reidentified = false; - int last_id = -1; - double lock_id_ = -1; - int switch_count = 0; - int update_count = 0; - int lost_count = 0; -}; - -class LiveTarget { -public: - TargetStatus status_; - - LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, - const enumeration::CarIDFlag& car_id, const std::time_t& t) - : last_time_stamp_(t) - , model_(car_id) { - - // x vx y vy z vz a w r l h - // a: angle - // w: angular velocity - // l: r2 - r1 - // h: z2 - z1 - auto center_x = - armor_xyz_in_world[0] + model_.GetRadius() * std::cos(armor_ypr_in_world[0]); - auto center_y = - armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); - auto center_z = armor_xyz_in_world[2]; - - Eigen::VectorXd x0(11); - x0 << center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, model_.GetRadius(), - 0, 0; - - Eigen::MatrixXd P0 = model_.GetP0Dig().asDiagonal(); - ekf_ = util::ExtendedKalmanFilter( - x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） - } - - Eigen::VectorXd GetEkfX() const { return ekf_.x; } - Eigen::VectorXd GetP0Dig() const { return model_.GetP0Dig(); } - const PredictModel& GetModel() const { return model_; } - std::time_t GetTimeStamp() const { return last_time_stamp_; } - - data::ArmorGimbalControlSpacing GetTargetArmorGimbalControlSpacings() { - const auto& ekf_x = GetEkfX(); - const auto& xyza_list = model_.GetArmorXYZAList(ekf_x); - - Eigen::Vector4d xyza; - data::ArmorGimbalControlSpacing armor; - - if (!status_.jumped) { - // 如果装甲板未发生过跳变，则只有当前装甲板的位置已知 - xyza = xyza_list.at(0); - } - - // 整车旋转中心的球坐标yaw - auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); - std::vector delta_angle_list; - for (int i = 0; i < model_.GetArmorNum(); i++) { - auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); - delta_angle_list.emplace_back(delta_angle); - } - - // 不考虑小陀螺 - if (std::abs(ekf_x[8]) <= 2 && model_.GetID() != enumeration::CarIDFlag::Outpost) { - // 选择在可射击范围内的装甲板 - std::vector id_list; - for (int i = 0; i < model_.GetArmorNum(); i++) { - if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; - id_list.push_back(i); - } - - // 锁定模式：防止在两个都呈45度的装甲板之间来回切换 - if (id_list.size() > 1) { - int id0 = id_list[0], id1 = id_list[1]; - - // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 - if (status_.lock_id_ != id0 && status_.lock_id_ != id1) - status_.lock_id_ = - (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) ? id0 - : id1; - xyza = xyza_list.at(status_.lock_id_); - } - - // 只有一个装甲板在可射击范围内时，退出锁定模式 - status_.lock_id_ = -1; - xyza = xyza_list.at(id_list[0]); - } - - double coming_angle, leaving_angle; - if (model_.GetID() == enumeration::CarIDFlag::Outpost) { - coming_angle = 70 / 57.3; - leaving_angle = 30 / 57.3; - } else { - coming_angle = comming_angle_; - leaving_angle = leaving_angle_; - } - - // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 - for (int i = 0; i < model_.GetArmorNum(); i++) { - if (std::abs(delta_angle_list[i]) > coming_angle) continue; - if (ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) xyza = xyza_list[i]; - else if (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle) xyza = xyza_list[i]; - } - - armor.id = model_.GetID(); - armor.position = xyza.head<3>(); - const auto& angle = xyza[3]; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - - return armor; - } - - void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { - - // 装甲板匹配 - int id = - model_.MatchArmor(ekf_.x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); - if (id != 0) status_.jumped = true; - status_.switched = (id != status_.last_id); - if (status_.switched) status_.switch_count++; - - status_.last_id = id; - Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); - status_.update_count++; - } - -private: - bool Converged() { - if (model_.GetID() != enumeration::CarIDFlag::Outpost && status_.update_count > 3 - && !this->Diverged()) { - status_.converged = true; - } - - // 前哨站特殊判断 - if (model_.GetID() == enumeration::CarIDFlag::Outpost && status_.update_count > 10 - && !this->Diverged()) { - status_.converged = true; - } - return status_.converged; - } - - bool Diverged() const { - auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; - auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; - - if (r_ok && l_ok) return false; - // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); - return true; - } - - void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, - const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, - const int& id, const double& dt) { - // 观测jacobi - auto H = model_.H(ekf_.x, id); - auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); - auto A = model_.A(dt); - auto Q = model_.Q(dt); - auto f = model_.f; - auto h = [this, id](const Eigen::VectorXd& x) { return model_.h(x, id); }; - auto z_subtract = model_.z_subtract; - - const Eigen::VectorXd& ypd = armor_ypd_in_world; - const Eigen::VectorXd& ypr = armor_ypr_in_world; - - // 获得观测量 - Eigen::VectorXd z(4); - z << ypd[0], ypd[1], ypd[2], ypr[0]; - - ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); - - // 前哨站转速特判 - if (this->Converged() && model_.GetID() == enumeration::CarIDFlag::Outpost - && std::abs(this->ekf_.x[7]) > 2) - this->ekf_.x[7] = this->ekf_.x[7] > 0 ? 2.51 : -2.51; - } - - std::time_t last_time_stamp_; - util::ExtendedKalmanFilter ekf_; - PredictModel model_; - - double comming_angle_ = 60 / 57.3; - double leaving_angle_ = 20 / 57.3; -}; -} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.cpp b/src/tongji/predictor/live_target_manager.cpp deleted file mode 100644 index 8d662af..0000000 --- a/src/tongji/predictor/live_target_manager.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include "live_target_manager.hpp" - -#include -#include -#include - -#include "enum/armor_id.hpp" -#include "enum/enum_tools.hpp" -#include "interfaces/predictor_update_package.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" -#include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/target_snapshot_manager.hpp" -#include "util/index.hpp" - -namespace world_exe::tongji::predictor { - -class LiveTargetManager::Impl { -public: - Impl() = default; - - void RegisterTarget(enumeration::ArmorIdFlag id, std::shared_ptr target) { - targets_[id] = std::move(target); - } - - void RemoveTarget(enumeration::ArmorIdFlag id) { targets_.erase(id); } - - std::shared_ptr Predict( - const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { - std::unordered_map> - result; - - for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_.find(id); - if (it != targets_.end() && it->second) { - result[id].emplace_back(it->second->GetTargetArmorGimbalControlSpacings()); - } - } - - return std::make_shared(result, time_stamp); - } - - std::shared_ptr GetPredictor( - const enumeration::ArmorIdFlag& flag) const { - std::unordered_map> snapshot_map; - - for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { - auto it = targets_.find(id); - if (it != targets_.end() && it->second) { - snapshot_map[id] = it->second; - } - } - - return std::make_shared(flag, snapshot_map); - } - - void Update(std::shared_ptr data, const double& dt) { - const auto& transform = data->GetTransform(); - const auto& rotation_transform = Eigen::Quaterniond(transform.linear()); - - for (const auto& [id, target] : targets_) { - const auto armors = data->GetArmors()->GetArmors(id); - - if (armors.empty()) continue; - - for (auto armor : armors) { - const auto& armor_in_world_position = transform * armor.position; - const auto& armor_in_world_oritaiton = rotation_transform * armor.orientation; - target->Update(dt, armor_in_world_position, - util::math::quaternion_to_euler(armor_in_world_oritaiton, 2, 1, 0), - util::math::xyz2ypd(armor_in_world_position)); - } - } - } - -private: - std::unordered_map> targets_; -}; - -LiveTargetManager::LiveTargetManager() = default; -LiveTargetManager::~LiveTargetManager() = default; - -std ::shared_ptr LiveTargetManager::Predict( - const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { - return pimpl_->Predict(id, time_stamp); -} - -std ::shared_ptr LiveTargetManager::GetPredictor( - const enumeration ::ArmorIdFlag& id) const { - return pimpl_->GetPredictor(id); -} - -} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager/decider.cpp b/src/tongji/predictor/live_target_manager/decider.cpp new file mode 100644 index 0000000..4db8573 --- /dev/null +++ b/src/tongji/predictor/live_target_manager/decider.cpp @@ -0,0 +1,78 @@ +#include "decider.hpp" + +#include +#include +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" + +namespace world_exe::tongji::predictor { + +class Decider::Impl { +public: + explicit Impl(PriorityMode mode = PriorityMode::MODE_ONE) + : mode_(mode) { } + + enumeration::ArmorIdFlag GetBestArmor(std::vector& armors) const { + if (armors.empty()) return enumeration::ArmorIdFlag::None; + + const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; + std::vector> armors_list; + for (const auto& armor : armors) { + armors_list.emplace_back(armor.id, priority_map.at(armor.id)); + } + + cv::Point2d img_center(1440.0 / 2, 1080.0 / 2); // TODO + std::sort(armors.begin(), armors.end(), [&](const auto& a, const auto& b) { + auto center_a = (a.image_points[0] + a.image_points[3]) * 0.5f; + auto center_b = (b.image_points[0] + b.image_points[3]) * 0.5f; + return cv::norm(center_a - img_center) < cv::norm(center_b - img_center); + }); + + std::sort(armors_list.begin(), armors_list.end(), + [](const auto& a, const auto& b) { return a.second < b.second; }); + return armors.front().id; + } + +private: + using ArmorPriority = world_exe::tongji::predictor::ArmorPriority; + using ArmorId = world_exe::enumeration::ArmorIdFlag; + + using PriorityMap = std::unordered_map; + + const PriorityMap mode1 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + + const PriorityMap mode2 = { + { ArmorId::Hero, ArmorPriority::Second }, // + { ArmorId::Engineer, ArmorPriority::Forth }, // + { ArmorId::InfantryIII, ArmorPriority::First }, // + { ArmorId::InfantryIV, ArmorPriority::First }, // + { ArmorId::InfantryV, ArmorPriority::Third }, // + { ArmorId::Sentry, ArmorPriority::Third }, // + { ArmorId::Outpost, ArmorPriority::Fifth }, // + { ArmorId::Base, ArmorPriority::Fifth }, // + { ArmorId::Unknow, ArmorPriority::Fifth } // + }; + PriorityMode mode_; +}; + +Decider::Decider(PriorityMode mode) + : pimpl_(std::make_unique(mode)) { } +Decider::~Decider() = default; + +enumeration::ArmorIdFlag Decider::GetBestArmor(std::vector& armors) const { + return pimpl_->GetBestArmor(armors); +} +} diff --git a/src/tongji/predictor/live_target_manager/decider.hpp b/src/tongji/predictor/live_target_manager/decider.hpp new file mode 100644 index 0000000..0a44730 --- /dev/null +++ b/src/tongji/predictor/live_target_manager/decider.hpp @@ -0,0 +1,35 @@ + + +#pragma once + +#include +#include + +#include "data/armor_image_spaceing.hpp" +#include "enum/armor_id.hpp" + +namespace world_exe::tongji::predictor { + +enum PriorityMode { MODE_ONE = 1, MODE_TWO }; + +enum class ArmorPriority { + First = 1, // + Second, // + Third, // + Forth, // + Fifth // +}; + +class Decider { +public: + Decider(PriorityMode mode = PriorityMode::MODE_ONE); + ~Decider(); + + enumeration::ArmorIdFlag GetBestArmor(std::vector& armors) const; + +private: + class Impl; + std::unique_ptr pimpl_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager/live_target.hpp b/src/tongji/predictor/live_target_manager/live_target.hpp new file mode 100644 index 0000000..d9369a4 --- /dev/null +++ b/src/tongji/predictor/live_target_manager/live_target.hpp @@ -0,0 +1,139 @@ +#pragma once + +#include +#include +#include +#include + +#include "../../time_stamp/time_stamp.hpp" +#include "../kalman_filter/extended_kalman_filter.hpp" +#include "../kalman_filter/predict_model.hpp" +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/car_id.hpp" + +namespace world_exe::tongji::predictor { + +class LiveTarget { +public: + LiveTarget(const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + const enumeration::CarIDFlag& car_id) + : last_see_time_stamp_(std::time(nullptr)) + , model_(car_id) { + + // x vx y vy z vz a w r l h + // a: angle + // w: angular velocity + // l: r2 - r1 + // h: z2 - z1 + auto center_x = + armor_xyz_in_world[0] + model_.GetRadius() * std::cos(armor_ypr_in_world[0]); + auto center_y = + armor_xyz_in_world[1] + model_.GetRadius() * std::sin(armor_ypr_in_world[0]); + auto center_z = armor_xyz_in_world[2]; + + ExtendedKalmanFilter<11, 4>::XVec x0; + x0 << center_x, 0, center_y, 0, center_z, 0, armor_ypr_in_world[0], 0, model_.GetRadius(), + 0, 0; + + ExtendedKalmanFilter<11, 4>::PMat P0 = model_.GetP0Dig().asDiagonal(); + ekf_ = ExtendedKalmanFilter<11, 4>( + x0, P0, model_.x_add); // 初始化滤波器（预测量、预测量协方差） + } + + ExtendedKalmanFilter<11, 4>::XVec GetEkfX() const { return ekf_.x; } + ExtendedKalmanFilter<11, 4>::PDig GetP0Dig() const { return model_.GetP0Dig(); } + const PredictModel& GetModel() const { return model_; } + time_stamp::TimeStamp LastSeen() const { return time_stamp::TimeStamp(last_see_time_stamp_); } + + std::vector GetArmorGimbalControlSpacings() const { + std::vector armors; + for (int id = 0; id < model_.GetArmorNum(); id++) { + auto angle = + util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + data::ArmorGimbalControlSpacing armor; + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + armors.emplace_back(std::move(armor)); + } + return armors; + } + + void Update(const double& dt, const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world) { + // 装甲板匹配 + int id = + model_.MatchArmor(ekf_.x, armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world); + last_id = id; + update_count++; + + Update_ypda(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id, dt); + + last_see_time_stamp_ = std::time(nullptr); + } + + bool IsConverged() const { return EvaluateConvergence(); } + +private: + bool EvaluateConvergence() const { + // 前哨站特殊判断 + const int required_count = (model_.GetID() == enumeration::CarIDFlag::Outpost) ? 10 : 3; + if (update_count < required_count) return false; + if (EvaluateDivergence()) return false; + + auto nis_failures = + std::accumulate(ekf_.recent_nis_failures.begin(), ekf_.recent_nis_failures.end(), 0); + if (nis_failures > 0.4 * ekf_.window_size) return false; + + return true; + } + + bool EvaluateDivergence() const { + auto r_ok = ekf_.x[8] > 0.05 && ekf_.x[8] < 0.5; + auto l_ok = ekf_.x[8] + ekf_.x[9] > 0.05 && ekf_.x[8] + ekf_.x[9] < 0.5; + + if (r_ok && l_ok) return false; + // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_.x[8], ekf_.x[9]); + return true; + } + + void Update_ypda(const Eigen::Vector3d& armor_xyz_in_world, + const Eigen::Vector3d& armor_ypr_in_world, const Eigen::Vector3d& armor_ypd_in_world, + const int& id, const double& dt) { + // 观测jacobi + auto H = model_.H(ekf_.x, id); + auto R = model_.R(armor_xyz_in_world, armor_ypr_in_world, armor_ypd_in_world, id); + auto A = model_.A(dt); + auto Q = model_.Q(dt); + auto f = model_.f; + auto h = [this, id](const ExtendedKalmanFilter<11, 4>::XVec& x) { return model_.h(x, id); }; + auto z_subtract = model_.z_subtract; + + const Eigen::Vector3d& ypd = armor_ypd_in_world; + const Eigen::Vector3d& ypr = armor_ypr_in_world; + + // 获得观测量 + ExtendedKalmanFilter<11, 4>::ZVec z(4); + z << ypd[0], ypd[1], ypd[2], ypr[0]; + + ekf_.Update(dt, A, Q, f, z, H, R, h, z_subtract); + + // 前哨站转速特判 + if (model_.GetID() == enumeration::CarIDFlag::Outpost && EvaluateConvergence()) { + constexpr double max_outpost_w = 2.51; + if (std::abs(ekf_.x[7]) > 2.0) { + ekf_.x[7] = ekf_.x[7] > 0 ? max_outpost_w : -max_outpost_w; + } + } + } + + std::time_t last_see_time_stamp_; + ExtendedKalmanFilter<11, 4> ekf_; + PredictModel model_; + + int last_id = -1; + int update_count = 0; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager/live_target_manager.cpp b/src/tongji/predictor/live_target_manager/live_target_manager.cpp new file mode 100644 index 0000000..ad20219 --- /dev/null +++ b/src/tongji/predictor/live_target_manager/live_target_manager.cpp @@ -0,0 +1,153 @@ +#include "live_target_manager.hpp" + +#include +#include +#include +#include + +#include "../in_gimbal_control_armor.hpp" +#include "../target_snapshot_manager/target_snapshot_manager.hpp" +#include "enum/armor_id.hpp" +#include "enum/car_id.hpp" +#include "interfaces/predictor_update_package.hpp" +#include "live_target.hpp" +#include "tracker.hpp" +#include "util/index.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +class LiveTargetManager::Impl { +public: + Impl(const double& time_delay, const double& yaw_offset, const double& pitch_offset, + double timeout_sec = 0.1) + : targets_map_() + , tracker_(std::make_unique()) + , last_update_timestamp_(std::time(nullptr)) + , tracking_id_(enumeration::CarIDFlag::None) + , time_delay_(time_delay) + , yaw_offset_(yaw_offset) + , pitch_offset_(pitch_offset) + , timeout_sec_(timeout_sec) { } + + std::shared_ptr Predict( + const enumeration::ArmorIdFlag& flag, const std::time_t& time_stamp) { + std::unordered_map> + result; + + for (auto id : util::enumeration::ExpandArmorIdFlags(flag)) { + auto it = targets_map_.find(id); + if (it != targets_map_.end() && it->second && it->second->IsConverged()) { + auto spacings = it->second->GetArmorGimbalControlSpacings(); + result[id] = spacings; + } + } + + return std::make_shared(result, time_stamp); + } // TODO: ** 目前 ** 我认为这个函数是多余的 + + std::shared_ptr GetPredictor( + const enumeration::ArmorIdFlag& flag) const { + + if (targets_map_.empty()) return nullptr; // TODO + + return std::make_shared( + flag, targets_map_, last_update_timestamp_, bullet_speed_, yaw_offset_, pitch_offset_); + } + + void Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed) { + + UpdateTimeStamp(data->GetTimeStamped().GetTimeStamp()); + UpdateTargetMap(data, now); + UpdateTarget(data, armors_in_image, now); + UpdateBulletSpeed(bullet_speed); + } + + auto GetAllowedTargetID() const -> enumeration::CarIDFlag const { + if (targets_map_.at(tracking_id_)->IsConverged()) { + return tracking_id_; + } + return enumeration::CarIDFlag::None; + } + +private: + void UpdateBulletSpeed(const double& bullet_speed) { bullet_speed_ = bullet_speed; } + void UpdateTimeStamp(const time_t& time_stamp) { last_update_timestamp_ = time_stamp; } + void UpdateTargetMap( + std::shared_ptr data, const std::time_t& now) { + const Eigen::Affine3d transform = data->GetTransform(); + const Eigen::Matrix3d rotation_matrix = transform.linear(); + const auto armors_interface = data->GetArmors(); + + targets_map_.clear(); + for (int i; i < static_cast(enumeration::CarIDFlag::Count); i++) { + auto id = static_cast( + static_cast(enumeration::CarIDFlag::Hero) << i); + + const auto& armors_list = armors_interface->GetArmors(id); + if (armors_list.empty()) return; + + const auto& armor = armors_list.front(); + const Eigen::Vector3d xyz_in_world = transform * armor.position; + const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + targets_map_[id] = + std::move(std::make_shared(xyz_in_world, ypr_in_world, id)); + } + } + + void UpdateTarget(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now) { + const Eigen::Affine3d transform = data->GetTransform(); + const Eigen::Matrix3d rotation_matrix = transform.linear(); + const auto armors_interface = data->GetArmors(); + + tracking_id_ = tracker_->SelectTrackingTargetID(armors_in_image, now); + + const auto& armors_list = armors_interface->GetArmors(tracking_id_); + if (armors_list.empty()) return; + + const auto& armor = armors_list.front(); + const Eigen::Vector3d xyz_in_world = transform * armor.position; + const Eigen::Vector3d ypr_in_world = rotation_matrix.eulerAngles(2, 1, 0); // ZYX + targets_map_[tracking_id_]->Update(static_cast(now - last_update_timestamp_), + xyz_in_world, ypr_in_world, util::math::xyz2ypd(xyz_in_world)); + } + + std::unordered_map> targets_map_; + std::unique_ptr tracker_; + std::time_t last_update_timestamp_; + enumeration::CarIDFlag tracking_id_; + + double bullet_speed_; + const double time_delay_; + const double yaw_offset_; + const double pitch_offset_; + + const double timeout_sec_; +}; + +LiveTargetManager::LiveTargetManager(const double& time_delay, const double& yaw_offset, + const double& pitch_offset, double timeout_sec) + : pimpl_(std::make_unique(time_delay, yaw_offset, pitch_offset, timeout_sec)) { } +LiveTargetManager::~LiveTargetManager() = default; + +std ::shared_ptr LiveTargetManager::Predict( + const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) { + return pimpl_->Predict(id, time_stamp); +} +std ::shared_ptr LiveTargetManager::GetPredictor( + const enumeration ::ArmorIdFlag& id) const { + return pimpl_->GetPredictor(id); +} + +void LiveTargetManager::Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed) { + return pimpl_->Update(data, armors_in_image, now, bullet_speed); +} +auto LiveTargetManager::GetAllowedTargetID() const -> enumeration::ArmorIdFlag const { + return pimpl_->GetAllowedTargetID(); +} +} \ No newline at end of file diff --git a/src/tongji/predictor/live_target_manager.hpp b/src/tongji/predictor/live_target_manager/live_target_manager.hpp similarity index 53% rename from src/tongji/predictor/live_target_manager.hpp rename to src/tongji/predictor/live_target_manager/live_target_manager.hpp index f78580f..88ebfce 100644 --- a/src/tongji/predictor/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager/live_target_manager.hpp @@ -3,22 +3,29 @@ #include #include "enum/armor_id.hpp" +#include "interfaces/armor_in_image.hpp" +#include "interfaces/predictor_update_package.hpp" #include "interfaces/target_predictor.hpp" - namespace world_exe::tongji::predictor { class LiveTargetManager final : public interfaces::ITargetPredictor { public: - LiveTargetManager(); + LiveTargetManager(const double& time_delay, const double& yaw_offset, + const double& pitch_offset, double timeout_sec = 0.1); ~LiveTargetManager(); std ::shared_ptr Predict( const enumeration ::ArmorIdFlag& id, const std ::time_t& time_stamp) override; - std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; + void Update(std::shared_ptr data, + const std::shared_ptr& armors_in_image, const std::time_t& now, + const double& bullet_speed); + + auto GetAllowedTargetID() const -> enumeration::ArmorIdFlag const; + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/predictor/live_target_manager/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp new file mode 100644 index 0000000..c30b2eb --- /dev/null +++ b/src/tongji/predictor/live_target_manager/tracker.hpp @@ -0,0 +1,172 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "../../identifier/armor_filter.hpp" +#include "../../identifier/identified_armor.hpp" +#include "../../time_stamp/time_stamp.hpp" +#include "../target_snapshot_manager/target_snapshot.hpp" +#include "../target_snapshot_manager/target_snapshot_manager.hpp" +#include "decider.hpp" +#include "enum/armor_id.hpp" + +namespace world_exe::tongji::predictor { + +enum class TrackState { + Lost, // + Detecting, // + Tracking, // + TempLost, // + Switching // +}; + +class Tracker final { + using TargetSnapshotManager = world_exe::tongji::predictor::TargetSnapshotManager; + using TargetSnapshot = world_exe::tongji::predictor::TargetSnapshot; + using ArmorInImage = world_exe::tongji::identifier::IdentifiedArmor; + +public: + Tracker() + : armor_filter_(std::make_unique()) + , decider_(std::make_unique()) + , last_track_timestamp_(std::time(nullptr)) { } + + ~Tracker() = default; + + auto SelectTrackingTargetID(const std::shared_ptr& armors_in_image, + const std::time_t& now) noexcept -> enumeration::ArmorIdFlag const { + CheckCameraOffline(now); + last_track_timestamp_.SetTimeStamp(now); + + auto filtered_ids = enumeration::ArmorIdFlag::None; + auto detected_ids = enumeration::ArmorIdFlag::None; + std::vector filtered_armors; + for (uint32_t i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); ++i) { + auto id = static_cast( + static_cast(enumeration::ArmorIdFlag::Hero) << i); + + if (armors_in_image->GetArmors(id).empty()) continue; + + // 图像中出现的装甲板 + auto armors = armors_in_image->GetArmors(id); + detected_ids = static_cast( + static_cast(detected_ids) | static_cast(id)); + + // 对从图像识别到的装甲板进行过滤 + filtered_armors = std::move(armor_filter_->FilterArmor(std::move(armors))); + if (!filtered_armors.empty()) { + filtered_ids = + static_cast(static_cast(filtered_ids) + | static_cast(filtered_armors.at(0).id)); + } + } + + UpdateState(!(detected_ids == enumeration::ArmorIdFlag::None)); + + tracking_car_id_ = decider_->GetBestArmor(filtered_armors); + return tracking_car_id_; + } + + void UpdateState(bool found) { + switch (state_) { + case TrackState::Lost: { + if (found) { + SetState(TrackState::Detecting); + detect_count_ = 1; + } + break; + } + + case TrackState::Detecting: { + if (found) { + detect_count_++; + if (detect_count_ >= min_detect_count_) SetState(TrackState::Tracking); + } else { + detect_count_ = 0; + SetState((pre_state_ == TrackState::Switching) ? TrackState::Switching + : TrackState::Lost); + } + break; + } + + case TrackState::Tracking: { + if (!found) { + temp_lost_count_ = 1; + SetState(TrackState::TempLost); + } + break; + } + + case TrackState::Switching: { + if (found) { + SetState(TrackState::Detecting); + } else { + temp_lost_count_++; + if (temp_lost_count_ > max_switch_count_) { + SetState(TrackState::Lost); + ResetTracking(); + }; + } + break; + } + + case TrackState::TempLost: { + if (found) { + SetState(TrackState::Tracking); + } else { + temp_lost_count_++; + max_temp_lost_count_ = (tracking_car_id_ == enumeration::ArmorIdFlag::Outpost) + ? outpost_max_temp_lost_count_ + : normal_max_temp_lost_count_; + + if (temp_lost_count_ > max_temp_lost_count_) { + SetState(TrackState::Lost); + ResetTracking(); + }; + } + break; + } + } + } + + TrackState GetState() const { return state_; } + +private: + void CheckCameraOffline(const std::time_t& now) { + // TODO:If the underlying timestamp is std::time_t, then this if branch will never be + // entered + if (state_ != TrackState::Lost + && static_cast(now - last_track_timestamp_.GetTimeStamp()) < 0.1) + SetState(TrackState::Lost); + } + + void SetState(TrackState new_state) { + pre_state_ = state_; + state_ = new_state; + } + + void ResetTracking() { tracking_car_id_ = enumeration::CarIDFlag::None; } + + world_exe::enumeration::CarIDFlag tracking_car_id_ { enumeration::CarIDFlag::None }; + TrackState state_ = TrackState::Lost; + TrackState pre_state_ = TrackState::Lost; + + std::unique_ptr armor_filter_; + std::unique_ptr decider_; + + int detect_count_ = 0; + int temp_lost_count_ = 0; + int max_temp_lost_count_ = 15; + const int min_detect_count_ = 5; + const int outpost_max_temp_lost_count_ = 75; + const int normal_max_temp_lost_count_ = max_temp_lost_count_; + const int max_switch_count_ = 200; + + time_stamp::TimeStamp last_track_timestamp_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp deleted file mode 100644 index 0dc4a61..0000000 --- a/src/tongji/predictor/target_snapshot.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once - -#include "tongji/predictor/live_target.hpp" -#include "tongji/predictor/predict_model.hpp" -#include "util/extended_kalman_filter.hpp" - -namespace world_exe::tongji::predictor { - -class TargetSnapshot { -public: - TargetSnapshot(const LiveTarget& target) - : model_(target.GetModel()) - , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) { } - - std::vector GetArmorGimbalControlSpacings() const { - std::vector armors; - for (int id = 0; id < model_.GetArmorNum(); id++) { - auto angle = - util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); - auto xyz = model_.h_armor_xyz(this->ekf_.x, id); - - data::ArmorGimbalControlSpacing armor; - armor.id = model_.GetID(); - armor.position = xyz; - armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); - armors.emplace_back(std::move(armor)); - } - return armors; - } - -private: - PredictModel model_; - util::ExtendedKalmanFilter ekf_; -}; - -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager.cpp deleted file mode 100644 index a660c9b..0000000 --- a/src/tongji/predictor/target_snapshot_manager.cpp +++ /dev/null @@ -1,51 +0,0 @@ -#include "target_snapshot_manager.hpp" - -#include -#include - -#include "data/armor_gimbal_control_spacing.hpp" -#include "enum/enum_tools.hpp" -#include "in_gimbal_control_armor.hpp" -#include "tongji/predictor/target_snapshot.hpp" - -namespace world_exe::tongji::predictor { -class TargetSnapshotManager::Impl { -public: - Impl(const enumeration::ArmorIdFlag& id, - const std::unordered_map>& snapshots) - : id_(id) { - for (const auto& [flag, target] : snapshots) { - snapshots_.emplace(flag, TargetSnapshot(*target)); - } - } - - const enumeration::ArmorIdFlag& GetId() const { return id_; } - - std::shared_ptr Predictor( - const std::time_t& time_stamp) const { - std::unordered_map> - result; - - for (const auto& [flag, snapshot] : snapshots_) { - result[flag] = snapshot.GetArmorGimbalControlSpacings(); - } - return std::make_shared(result, time_stamp); - } - -private: - std::unordered_map snapshots_; - enumeration::ArmorIdFlag id_; -}; - -TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, - const std::unordered_map> snapshots) - : pimpl_(std::make_unique(id, snapshots)) { } -TargetSnapshotManager::~TargetSnapshotManager() = default; - -const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } - -std ::shared_ptr TargetSnapshotManager::Predictor( - const std ::time_t& time_stamp) const { - return pimpl_->Predictor(time_stamp); -} -} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp new file mode 100644 index 0000000..2f2c15d --- /dev/null +++ b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include "enum/car_id.hpp" +#include "util/math.hpp" + +namespace world_exe::tongji::predictor { + +using CarIDFlag = enumeration::CarIDFlag; + +class AimPointChooser { +public: + AimPointChooser( + const double& comming_angle = 60 / 57.3, const double& leaving_angle = 20 / 57.3) + : comming_angle_(comming_angle) + , leaving_angle_(leaving_angle) { } + + std::pair ChooseAimArmor(const Eigen::Vector& ekf_x, + const std::vector& xyza_list, const CarIDFlag& single_id) { + const auto armor_num = xyza_list.size(); + int chosen_id = -1; + + // 整车旋转中心的球坐标yaw + const auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); + + // 如果delta_angle为0，则该装甲板中心和整车中心的连线在世界坐标系的xy平面过原点 + std::vector delta_angle_list; + for (int i = 0; i < armor_num; i++) { + auto delta_angle = util::math::clamp_pm_pi(xyza_list[i][3] - center_yaw); + delta_angle_list.emplace_back(delta_angle); + } + + // 不考虑小陀螺 + if (std::abs(ekf_x[8]) <= 2 && single_id != CarIDFlag::Outpost) { + // 选择在可射击范围内的装甲板 + std::vector id_list; + for (int i = 0; i < armor_num; i++) { + if (std::abs(delta_angle_list[i]) > 60 / 57.3) continue; + id_list.push_back(i); + } + + if (id_list.size() == 1) { + chosen_id = id_list[0]; + lock_id_ = -1; + } else if (id_list.size() > 1) { + const int id0 = id_list[0], id1 = id_list[1]; + // 未处于锁定模式时，选择delta_angle绝对值较小的装甲板，进入锁定模式 + if (lock_id_ != id0 && lock_id_ != id1) + lock_id_ = (std::abs(delta_angle_list[id0]) < std::abs(delta_angle_list[id1])) + ? id0 + : id1; + chosen_id = lock_id_; + } else { + chosen_id = -1; + } + } else { + // 小陀螺 + double coming_angle = + (single_id == CarIDFlag::Outpost) ? 70 / 57.3 : comming_angle_; + double leaving_angle = + (single_id == CarIDFlag::Outpost) ? 30 / 57.3 : leaving_angle_; + + // 在小陀螺时，一侧的装甲板不断出现，另一侧的装甲板不断消失，显然前者被打中的概率更高 + for (int i = 0; i < armor_num; i++) { + if (std::abs(delta_angle_list[i]) > coming_angle) continue; + if ((ekf_x[7] > 0 && delta_angle_list[i] < leaving_angle) + || (ekf_x[7] < 0 && delta_angle_list[i] > -leaving_angle)) { + chosen_id = i; + break; + } + } + } + + if (chosen_id == -1) { + return { false, xyza_list[0] }; + } + + return { + true, + xyza_list[chosen_id], + }; + } + +private: + double comming_angle_ = 60 / 57.3; // degree + double leaving_angle_ = 20 / 57.3; // degree + int lock_id_ = -1; +}; + +} diff --git a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp new file mode 100644 index 0000000..4c9122e --- /dev/null +++ b/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include +#include + +#include "aim_point_chooser.hpp" +#include "target_snapshot.hpp" +#include "trajectory.hpp" + +namespace world_exe::tongji::predictor { + +using TargetSnapshot = predictor::TargetSnapshot; + +struct AimSolution { + bool valid; + double yaw; + double pitch; + Eigen::Vector4d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） + double horizon_distance = 0; // 无人机专有 +}; + +class AimingSolver { +public: + AimingSolver( + const double& yaw_offset, const double& pitch_offset, const double& gravity = 9.7833) + : aim_point_chooser_(std::make_unique()) + , yaw_offset_(yaw_offset / 57.3) // degree to rad + , pitch_offset_(pitch_offset / 57.3) // degree to rad + , g_(gravity) { } + + AimSolution SolveAimSolution(const std::shared_ptr& snapshot, + const double& bullet_speed, const double& time_delay) { + if (!snapshot) return { false, 0, 0, { }, 0 }; + + // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) + double prev_fly_time = 0; + Eigen::Vector4d final_aim_point; + TrajectoryResult final_trajectory; + bool converged = false; + + // 预测目标在未来 dt时间后的位置 + for (int i = 0; i < 10; ++i) { + double dt = time_delay + prev_fly_time; + const auto aim = SelectPredictedAim(snapshot, dt); + if (!aim.has_value()) return { false, 0, 0, { }, 0 }; // failed: no valid aim point + + const auto traj = SolveTrajectory(aim->head(3), bullet_speed); + if (!traj.has_value()) return { false, 0, 0, { }, 0 }; // failed: trajectory unsolvable + + if (i > 0 && std::abs(traj->fly_time - prev_fly_time) < 0.001) { + final_aim_point = *aim; + final_trajectory = *traj; + converged = true; + break; + } + prev_fly_time = traj->fly_time; + } + if (!converged) return { false, 0, 0, { }, 0 }; // failed: trajectory did not converge + + const auto xyz = final_aim_point.head(3); + const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; + const double pitch = -(final_trajectory.pitch + pitch_offset_); + return { true, yaw, pitch, final_aim_point }; + } + +private: + std::optional SelectPredictedAim( + const std::shared_ptr& snapshot, const double& dt) const { + const auto& [selectable, aim_point] = aim_point_chooser_->ChooseAimArmor( + snapshot->Predict(dt), snapshot->GetPredictedXYZAList(dt), snapshot->GetID()); + return selectable ? std::optional { aim_point } : std::nullopt; + } + + std::optional SolveTrajectory( + const Eigen::Vector3d& xyz, const double& bullet_speed) const { + double d = std::hypot(xyz.x(), xyz.y()); + auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); + return result.solvable ? std::optional { result } : std::nullopt; + } + + double yaw_offset_, pitch_offset_; + const double g_; + + std::unique_ptr aim_point_chooser_; +}; +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp new file mode 100644 index 0000000..a1fa6bf --- /dev/null +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include + +#include "../../time_stamp/time_stamp.hpp" +#include "../kalman_filter/extended_kalman_filter.hpp" +#include "../kalman_filter/predict_model.hpp" +#include "../live_target_manager/live_target.hpp" + +namespace world_exe::tongji::predictor { + +class TargetSnapshot { +public: + TargetSnapshot(const LiveTarget& target) + : model_(target.GetModel()) + , ekf_(target.GetEkfX(), target.GetP0Dig().asDiagonal(), model_.x_add) + , time_stamp_(target.LastSeen()) { } + + // std::vector GetArmorGimbalControlSpacings() const { + // std::vector armors; + // for (int id = 0; id < model_.GetArmorNum(); id++) { + // auto angle = + // util::math::clamp_pm_pi(this->ekf_.x[6] + id * 2 * CV_PI / model_.GetArmorNum()); + // auto xyz = model_.h_armor_xyz(this->ekf_.x, id); + + // data::ArmorGimbalControlSpacing armor; + // armor.id = model_.GetID(); + // armor.position = xyz; + // armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); + // armors.emplace_back(std::move(armor)); + // } + // return armors; + // } + + auto GetPredictedXYZAList(const double& dt) -> std::vector const { + return model_.GetArmorXYZAList(this->Predict(dt)); + } + + auto GetTimeStamp() const { return time_stamp_; } + auto GetID() const { return model_.GetID(); } + auto GetEkfX() const { return ekf_.x; } + + auto Predict(const double& dt) -> Eigen::Vector const { + auto predicted_x = model_.f(ekf_.x, dt); + return predicted_x; + } + +private: + PredictModel model_; + ExtendedKalmanFilter<11, 4> ekf_; + time_stamp::TimeStamp time_stamp_; +}; + +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp new file mode 100644 index 0000000..ad9a586 --- /dev/null +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp @@ -0,0 +1,99 @@ +#include "target_snapshot_manager.hpp" + +#include +#include +#include + +#include "../in_gimbal_control_armor.hpp" +#include "../live_target_manager/live_target.hpp" +#include "aim_solver.hpp" +#include "data/armor_gimbal_control_spacing.hpp" +#include "enum/enum_tools.hpp" + +namespace world_exe::tongji::predictor { + +class TargetSnapshotManager::Impl { +public: + Impl(const enumeration::ArmorIdFlag& id, + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset) + : aim_solver_(std::make_unique(yaw_offset, pitch_offset)) + + , now_(now) + , ids_(id) + , bullet_speed_(bullet_speed) + , snapshots_(BuildSnapshots(live_target_map)) + , gimbal_command_({ std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN() }) { } + + const enumeration::ArmorIdFlag& GetId() const { return ids_; } + + std::shared_ptr Predictor( + const std::time_t& time_stamp) const { + + std::unordered_map> + result; + + for (const auto& [id, snapshot] : snapshots_) { + auto aim_solution = + aim_solver_->SolveAimSolution(std::make_unique(snapshot), + bullet_speed_, snapshot.GetTimeStamp().GetTimeStamp()); + + if (!aim_solution.valid) continue; + + auto target_pos = aim_solution.aim_point.head<3>(); + auto armor_yaw = aim_solution.aim_point[3]; + result.emplace(id, + std::vector { + data::ArmorGimbalControlSpacing { id, target_pos, + util::math::euler_to_quaternion(armor_yaw, 15. / 180. * CV_PI, 0) } }); + + gimbal_command_.yaw = aim_solution.yaw; + gimbal_command_.pitch = aim_solution.pitch; + } + return std::make_shared(result, time_stamp); + } + + auto GetGimbalCommand() const -> GimbalCommand const { return gimbal_command_; } + +private: + static std::unordered_map BuildSnapshots( + const std::unordered_map>& input) { + std::unordered_map result; + for (const auto& [id, target] : input) { + if (target) { + result.emplace(id, TargetSnapshot(*target)); + } + } + return result; + } + + std::unique_ptr aim_solver_; + const std::unordered_map snapshots_; + const std::time_t& now_; + const enumeration::ArmorIdFlag ids_; + const double bullet_speed_; + mutable GimbalCommand gimbal_command_; +}; + +TargetSnapshotManager::TargetSnapshotManager(const enumeration::ArmorIdFlag& id, + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset) + : pimpl_(std::make_unique( + id, live_target_map, now, bullet_speed, yaw_offset, pitch_offset)) { } +TargetSnapshotManager::~TargetSnapshotManager() = default; + +const enumeration ::ArmorIdFlag& TargetSnapshotManager::GetId() const { return pimpl_->GetId(); } +std ::shared_ptr TargetSnapshotManager::Predictor( + const std ::time_t& time_stamp) const { + return pimpl_->Predictor(time_stamp); +} + +auto TargetSnapshotManager::GetGimbalCommand() const -> GimbalCommand const { + return pimpl_->GetGimbalCommand(); +} +} \ No newline at end of file diff --git a/src/tongji/predictor/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp similarity index 65% rename from src/tongji/predictor/target_snapshot_manager.hpp rename to src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp index 02394fd..3176e9a 100644 --- a/src/tongji/predictor/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp @@ -3,23 +3,32 @@ #include #include +#include "../live_target_manager/live_target.hpp" #include "enum/armor_id.hpp" #include "interfaces/predictor.hpp" -#include "tongji/predictor/live_target.hpp" namespace world_exe::tongji::predictor { +struct GimbalCommand { + double yaw; + double pitch; +}; + class TargetSnapshotManager final : public interfaces::IPredictor { public: TargetSnapshotManager(const enumeration::ArmorIdFlag& id, - const std::unordered_map> snapshots); + const std::unordered_map>& + live_target_map, + const std::time_t& now, const double& bullet_speed, const double& yaw_offset, + const double& pitch_offset); ~TargetSnapshotManager(); const enumeration ::ArmorIdFlag& GetId() const override; - std ::shared_ptr Predictor( const std ::time_t& time_stamp) const override; + auto GetGimbalCommand() const -> GimbalCommand const; + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp new file mode 100644 index 0000000..ff5adc1 --- /dev/null +++ b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +namespace world_exe::tongji::predictor { + +struct TrajectoryResult { + bool solvable = true; + double fly_time; + double pitch; // 抬头为正,rad +}; + +struct TrajectorySolver { + // 不考虑空气阻力 + // v0 子弹初速度大小，单位：m/s + // d 目标水平距离，单位：m + // h 目标竖直高度，单位：m + // g 重力加速度，单位：m/s^2 + //(g·x²)/(2v₀²)·u² - x·u + (g·x²)/(2v₀²) + y = 0(其中u = tan(θ)) + static auto SolveTrajectory(const double& v0, const double& d, const double& h, const double& g) + -> TrajectoryResult const { + auto a = g * d * d / (2 * v0 * v0); + auto b = -d; + auto c = a + h; + auto delta = b * b - 4 * a * c; + + if (delta < 0) { + return { .solvable = false, .fly_time = 0, .pitch = 0 }; + } + + auto tan_pitch_1 = (-b + std::sqrt(delta)) / (2 * a); + auto tan_pitch_2 = (-b - std::sqrt(delta)) / (2 * a); + auto pitch_1 = std::atan(tan_pitch_1); + auto pitch_2 = std::atan(tan_pitch_2); + auto t_1 = d / (v0 * std::cos(pitch_1)); + auto t_2 = d / (v0 * std::cos(pitch_2)); + return { + .solvable = true, + .fly_time = (t_1 < t_2) ? t_1 : t_2, + .pitch = (t_1 < t_2) ? pitch_1 : pitch_2, + }; + } +}; + +} \ No newline at end of file diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp deleted file mode 100644 index 219e457..0000000 --- a/src/tongji/state_machine/car_state_manager.hpp +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include - -#include "tongji/predictor/time_stamp.hpp" - -namespace world_exe::tongji::car_state { - -class CarStateManager { -public: - CarStateManager(int switch_threshold = 5) - : switch_threshold_(switch_threshold) { } - - void Update(bool detected, std::time_t now_raw) { - if (detected) { - count_ = std::min(count_ + 1, switch_threshold_); - auto now = predictor::TimeStamp(now_raw); - last_seen_ = now; - } else { - count_ = std::max(count_ - 1, 0); - } - is_locked_ = (count_ >= switch_threshold_); - } - - bool IsLost(const predictor::TimeStamp& now, double timeout_sec) const { - return now.SecondsSince(last_seen_) > timeout_sec; - } - - bool IsLocked() const { return is_locked_; } - bool IsConverged() const { return is_converged_; } - bool IsDiverged() const { return is_diverged_; } - - void SetPriority(int p); - void SetThreshold(const int& value) { switch_threshold_ = value; } - void SetDiverged(bool diverged) { - is_diverged_ = diverged; - if (diverged) is_converged_ = false; - } - void SetPriority(const int& p) { priority_ = p; } - int GetPriority() const { return priority_; } - - predictor::TimeStamp LastSeen() const { return last_seen_; } - - void IncrementUpdateCount() { - update_count_++; - if (update_count_ > 3 && !is_diverged_) { - is_converged_ = true; - } - } - -private: - int count_ = 0; - int switch_threshold_; - int update_count_ = 0; - - bool is_locked_ = false; - bool is_converged_ = false; - bool is_diverged_ = false; - int priority_ = 100; // 默认最低优先级 - predictor::TimeStamp last_seen_ { 0 }; -}; -} \ No newline at end of file diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 442148d..990bf99 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,40 +1,38 @@ #include "state_machine.hpp" +#include + #include "enum/car_id.hpp" +#include "interfaces/target_predictor.hpp" +#include "tongji/predictor/live_target_manager/live_target_manager.hpp" namespace world_exe::tongji::state_machine { -StateMachine::StateMachine(int switch_threshold) - : switch_threshold_(switch_threshold) { - for (auto& manager : car_states_) { - manager = car_state::CarStateManager(switch_threshold_); - } -} - -const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } +class StateMachine::Impl { +public: + Impl(std::shared_ptr live_target_manager) + : live_target_manager_(std::move(live_target_manager)) { } -const interfaces::ICarState& StateMachine::Update( - const enumeration::CarIDFlag& car_detected, std::time_t now) { - tracing_state_ = enumeration::CarIDFlag::None; + const enumeration::CarIDFlag& GetAllowdToFires() const { return target_ids_; } - for (int i = 0; i < static_cast(enumeration::CarIDFlag::Count); ++i) { - auto id = static_cast(1 << i); - bool detected = (static_cast(car_detected) >> i) & 0x01; - - car_states_[i].Update(detected, now); + void Update() { + auto live_target_manager = + std::dynamic_pointer_cast(live_target_manager_); + target_ids_ = live_target_manager->GetAllowedTargetID(); + } - if (detected) { - car_states_[i].IncrementUpdateCount(); - } +private: + std::shared_ptr live_target_manager_; + enumeration::CarIDFlag target_ids_; +}; - if (car_states_[i].IsLocked() && car_states_[i].IsConverged() - && !car_states_[i].IsDiverged()) { - tracing_state_ = - static_cast(static_cast(tracing_state_) | (1 << i)); - } - } +StateMachine::StateMachine( + std::shared_ptr live_target_manager) + : pimpl_(std::make_unique(live_target_manager)) { } +StateMachine::~StateMachine() = default; - return *this; +const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { + return pimpl_->GetAllowdToFires(); } } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 8fb9872..c3417e3 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -1,24 +1,23 @@ #pragma once #include +#include -#include "car_state_manager.hpp" #include "enum/car_id.hpp" #include "interfaces/car_state.hpp" +#include "interfaces/target_predictor.hpp" namespace world_exe::tongji::state_machine { class StateMachine final : public interfaces::ICarState { public: - StateMachine(int switch_threshold = 5); + StateMachine(); + ~StateMachine(); - const enumeration::CarIDFlag& GetAllowdToFires() const override; - - const interfaces::ICarState& Update( - const enumeration::CarIDFlag& car_detected, std::time_t now); + const enumeration ::CarIDFlag& GetAllowdToFires() const override; + StateMachine(std::shared_ptr live_target_manager); private: - enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; - std::array car_states_; - int switch_threshold_; + class Impl; + std::unique_ptr pimpl_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/time_stamp.hpp b/src/tongji/time_stamp/time_stamp.hpp similarity index 92% rename from src/tongji/predictor/time_stamp.hpp rename to src/tongji/time_stamp/time_stamp.hpp index 90deacf..aa93ae0 100644 --- a/src/tongji/predictor/time_stamp.hpp +++ b/src/tongji/time_stamp/time_stamp.hpp @@ -2,7 +2,7 @@ #include "interfaces/time_stamped.hpp" -namespace world_exe::tongji::predictor { +namespace world_exe::tongji::time_stamp { class TimeStamp : public interfaces::ITimeStamped { public: TimeStamp(const std::time_t& time_stamp)