From 445f55fb1ec8775536db88c271c3ea0d63bdc113 Mon Sep 17 00:00:00 2001 From: Alray Qiu <70491601+AlrayQiu@users.noreply.github.com> Date: Mon, 13 Oct 2025 14:56:06 +0800 Subject: [PATCH] Revert "[Add] Fire_controller module(#12)" This reverts commit 9d493d41ef42b5ea6f73cdcaba1c86c199db1bd7. --- .../fire_controller/fire_controller.cpp | 114 ---------- .../fire_controller/fire_controller.hpp | 31 --- src/tongji/fire_controller/fire_decision.hpp | 52 ----- src/tongji/identifier/armor_filter.hpp | 45 ---- src/tongji/identifier/identified_armor.hpp | 7 +- src/tongji/identifier/identifier.cpp | 2 +- .../predictor/in_gimbal_control_armor.hpp | 4 +- src/tongji/predictor/live_target.hpp | 208 ++++++++++++++++++ src/tongji/predictor/live_target_manager.cpp | 92 ++++++++ .../live_target_manager.hpp | 13 +- .../predictor/live_target_manager/decider.cpp | 78 ------- .../predictor/live_target_manager/decider.hpp | 35 --- .../live_target_manager/live_target.hpp | 139 ------------ .../live_target_manager.cpp | 153 ------------- .../predictor/live_target_manager/tracker.hpp | 172 --------------- .../{kalman_filter => }/predict_model.hpp | 91 ++++---- src/tongji/predictor/target_snapshot.hpp | 36 +++ .../predictor/target_snapshot_manager.cpp | 51 +++++ .../target_snapshot_manager.hpp | 15 +- .../aim_point_chooser.hpp | 89 -------- .../target_snapshot_manager/aim_solver.hpp | 88 -------- .../target_snapshot.hpp | 54 ----- .../target_snapshot_manager.cpp | 99 --------- .../target_snapshot_manager/trajectory.hpp | 44 ---- .../{time_stamp => predictor}/time_stamp.hpp | 2 +- .../state_machine/car_state_manager.hpp | 62 ++++++ src/tongji/state_machine/state_machine.cpp | 50 +++-- src/tongji/state_machine/state_machine.hpp | 17 +- .../extended_kalman_filter.hpp | 44 ++-- 29 files changed, 549 insertions(+), 1338 deletions(-) delete mode 100644 src/tongji/fire_controller/fire_controller.cpp delete mode 100644 src/tongji/fire_controller/fire_controller.hpp delete mode 100644 src/tongji/fire_controller/fire_decision.hpp delete mode 100644 src/tongji/identifier/armor_filter.hpp create mode 100644 src/tongji/predictor/live_target.hpp create mode 100644 src/tongji/predictor/live_target_manager.cpp rename src/tongji/predictor/{live_target_manager => }/live_target_manager.hpp (53%) delete mode 100644 src/tongji/predictor/live_target_manager/decider.cpp delete mode 100644 src/tongji/predictor/live_target_manager/decider.hpp delete mode 100644 src/tongji/predictor/live_target_manager/live_target.hpp delete mode 100644 src/tongji/predictor/live_target_manager/live_target_manager.cpp delete mode 100644 src/tongji/predictor/live_target_manager/tracker.hpp rename src/tongji/predictor/{kalman_filter => }/predict_model.hpp (72%) create mode 100644 src/tongji/predictor/target_snapshot.hpp create mode 100644 src/tongji/predictor/target_snapshot_manager.cpp rename src/tongji/predictor/{target_snapshot_manager => }/target_snapshot_manager.hpp (65%) delete mode 100644 src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp delete mode 100644 src/tongji/predictor/target_snapshot_manager/aim_solver.hpp delete mode 100644 src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp delete mode 100644 src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp delete mode 100644 src/tongji/predictor/target_snapshot_manager/trajectory.hpp rename src/tongji/{time_stamp => predictor}/time_stamp.hpp (92%) create mode 100644 src/tongji/state_machine/car_state_manager.hpp rename src/{tongji/predictor/kalman_filter => util}/extended_kalman_filter.hpp (68%) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp deleted file mode 100644 index 6443340..0000000 --- a/src/tongji/fire_controller/fire_controller.cpp +++ /dev/null @@ -1,114 +0,0 @@ -#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 deleted file mode 100644 index 19343e0..0000000 --- a/src/tongji/fire_controller/fire_controller.hpp +++ /dev/null @@ -1,31 +0,0 @@ -#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 deleted file mode 100644 index 46088e8..0000000 --- a/src/tongji/fire_controller/fire_decision.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#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 deleted file mode 100644 index 21699e0..0000000 --- a/src/tongji/identifier/armor_filter.hpp +++ /dev/null @@ -1,45 +0,0 @@ -#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 14295e5..b6ac9ff 100644 --- a/src/tongji/identifier/identified_armor.hpp +++ b/src/tongji/identifier/identified_armor.hpp @@ -6,7 +6,6 @@ #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) { @@ -24,12 +23,8 @@ 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_ { std::time(nullptr) }; + std::time_t time_stamp_ { 0 }; 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 f70af8f..c314f74 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 "../identifier/classifier.hpp" +#include "tongji/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 9dbb878..627d12a 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: - time_stamp::TimeStamp time_stamp_; + TimeStamp time_stamp_; std::unordered_map> armors_map_; static inline const std::vector empty={}; diff --git a/src/tongji/predictor/live_target.hpp b/src/tongji/predictor/live_target.hpp new file mode 100644 index 0000000..c9fece5 --- /dev/null +++ b/src/tongji/predictor/live_target.hpp @@ -0,0 +1,208 @@ +#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 new file mode 100644 index 0000000..8d662af --- /dev/null +++ b/src/tongji/predictor/live_target_manager.cpp @@ -0,0 +1,92 @@ +#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/live_target_manager.hpp b/src/tongji/predictor/live_target_manager.hpp similarity index 53% rename from src/tongji/predictor/live_target_manager/live_target_manager.hpp rename to src/tongji/predictor/live_target_manager.hpp index 88ebfce..f78580f 100644 --- a/src/tongji/predictor/live_target_manager/live_target_manager.hpp +++ b/src/tongji/predictor/live_target_manager.hpp @@ -3,29 +3,22 @@ #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(const double& time_delay, const double& yaw_offset, - const double& pitch_offset, double timeout_sec = 0.1); + LiveTargetManager(); ~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/decider.cpp b/src/tongji/predictor/live_target_manager/decider.cpp deleted file mode 100644 index 4db8573..0000000 --- a/src/tongji/predictor/live_target_manager/decider.cpp +++ /dev/null @@ -1,78 +0,0 @@ -#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 deleted file mode 100644 index 0a44730..0000000 --- a/src/tongji/predictor/live_target_manager/decider.hpp +++ /dev/null @@ -1,35 +0,0 @@ - - -#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 deleted file mode 100644 index d9369a4..0000000 --- a/src/tongji/predictor/live_target_manager/live_target.hpp +++ /dev/null @@ -1,139 +0,0 @@ -#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 deleted file mode 100644 index ad20219..0000000 --- a/src/tongji/predictor/live_target_manager/live_target_manager.cpp +++ /dev/null @@ -1,153 +0,0 @@ -#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/tracker.hpp b/src/tongji/predictor/live_target_manager/tracker.hpp deleted file mode 100644 index c30b2eb..0000000 --- a/src/tongji/predictor/live_target_manager/tracker.hpp +++ /dev/null @@ -1,172 +0,0 @@ -#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/kalman_filter/predict_model.hpp b/src/tongji/predictor/predict_model.hpp similarity index 72% rename from src/tongji/predictor/kalman_filter/predict_model.hpp rename to src/tongji/predictor/predict_model.hpp index 5af8395..8fe4b43 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/predict_model.hpp @@ -2,12 +2,10 @@ #include -#include #include #include #include "enum/car_id.hpp" -#include "extended_kalman_filter.hpp" #include "util/math.hpp" namespace world_exe::tongji::predictor { @@ -56,27 +54,23 @@ class PredictModel { int GetArmorNum() const { return armor_num_; } // 防止夹角求和出现异常值 - 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)); + 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)); return c; }; // 防止夹角求和出现异常值 - 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)); + 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)); return x_prior; }; - int MatchArmor(const ExtendedKalmanFilter<11, 4>::XVec& x, - const Eigen::Vector3d& armor_xyz_in_world, const Eigen::Vector3d& armor_ypr_in_world, + int MatchArmor(const Eigen::VectorXd& 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); @@ -110,7 +104,7 @@ class PredictModel { } // 计算出装甲板中心的坐标（考虑长短轴） - Eigen::Vector3d h_armor_xyz(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { + Eigen::Vector3d h_armor_xyz(const Eigen::VectorXd& 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); @@ -122,7 +116,7 @@ class PredictModel { return { armor_x, armor_y, armor_z }; } - ExtendedKalmanFilter<11, 4>::HMat H(const ExtendedKalmanFilter<11, 4>::XVec& x, int id) const { + Eigen::MatrixXd H(const Eigen::VectorXd& 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); @@ -138,7 +132,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}, @@ -149,7 +143,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::Matrix H_armor_ypda{ + Eigen::MatrixXd 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}, @@ -160,13 +154,13 @@ class PredictModel { return H_armor_ypda * H_armor_xyza; } - ExtendedKalmanFilter<11, 4>::RMat R(const Eigen::Vector3d& armor_xyz_in_world, + Eigen::MatrixXd 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); - ExtendedKalmanFilter<11, 4>::RDig R_dig(4); + Eigen::VectorXd 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; @@ -174,11 +168,10 @@ class PredictModel { return R_dig.asDiagonal(); } - ExtendedKalmanFilter<11, 4>::AMat A(double dt) const { + Eigen::MatrixXd A(double dt) const { // 状态转移矩阵 - ExtendedKalmanFilter<11, 4>::AMat _A; // clang-format off - _A<< + return (Eigen::MatrixXd(11, 11) << 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, @@ -189,13 +182,11 @@ 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; - //clang-format on - return _A; + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1).finished(); } - + //clang-format on -ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { + Eigen::MatrixXd 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; @@ -211,10 +202,8 @@ ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { auto c = dt * dt; // 预测过程噪声偏差的方差 - Eigen::Matrix _Q; // clang-format off - _Q - << + return (Eigen::MatrixXd(11, 11) << 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, @@ -225,33 +214,29 @@ ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { 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; + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0).finished(); // clang-format on - return _Q; } - 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 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>::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)); + 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)); return c; }; - std::vector GetArmorXYZAList( - const ExtendedKalmanFilter<11, 4>::XVec& ekf_x) const { + std::vector GetArmorXYZAList(const Eigen::VectorXd& ekf_x) const { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { @@ -266,7 +251,7 @@ ExtendedKalmanFilter<11, 4>::QMat Q(double dt) const { int armor_num_; enumeration::CarIDFlag car_id_; - ExtendedKalmanFilter<11, 4>::PDig P0_dig_; + Eigen::VectorXd P0_dig_; double radius_; }; diff --git a/src/tongji/predictor/target_snapshot.hpp b/src/tongji/predictor/target_snapshot.hpp new file mode 100644 index 0000000..0dc4a61 --- /dev/null +++ b/src/tongji/predictor/target_snapshot.hpp @@ -0,0 +1,36 @@ +#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 new file mode 100644 index 0000000..a660c9b --- /dev/null +++ b/src/tongji/predictor/target_snapshot_manager.cpp @@ -0,0 +1,51 @@ +#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/target_snapshot_manager.hpp b/src/tongji/predictor/target_snapshot_manager.hpp similarity index 65% rename from src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp rename to src/tongji/predictor/target_snapshot_manager.hpp index 3176e9a..02394fd 100644 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.hpp +++ b/src/tongji/predictor/target_snapshot_manager.hpp @@ -3,32 +3,23 @@ #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>& - live_target_map, - const std::time_t& now, const double& bullet_speed, const double& yaw_offset, - const double& pitch_offset); + const std::unordered_map> snapshots); ~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/aim_point_chooser.hpp b/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp deleted file mode 100644 index 2f2c15d..0000000 --- a/src/tongji/predictor/target_snapshot_manager/aim_point_chooser.hpp +++ /dev/null @@ -1,89 +0,0 @@ -#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 deleted file mode 100644 index 4c9122e..0000000 --- a/src/tongji/predictor/target_snapshot_manager/aim_solver.hpp +++ /dev/null @@ -1,88 +0,0 @@ -#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 deleted file mode 100644 index a1fa6bf..0000000 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#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 deleted file mode 100644 index ad9a586..0000000 --- a/src/tongji/predictor/target_snapshot_manager/target_snapshot_manager.cpp +++ /dev/null @@ -1,99 +0,0 @@ -#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/trajectory.hpp b/src/tongji/predictor/target_snapshot_manager/trajectory.hpp deleted file mode 100644 index ff5adc1..0000000 --- a/src/tongji/predictor/target_snapshot_manager/trajectory.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#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/time_stamp/time_stamp.hpp b/src/tongji/predictor/time_stamp.hpp similarity index 92% rename from src/tongji/time_stamp/time_stamp.hpp rename to src/tongji/predictor/time_stamp.hpp index aa93ae0..90deacf 100644 --- a/src/tongji/time_stamp/time_stamp.hpp +++ b/src/tongji/predictor/time_stamp.hpp @@ -2,7 +2,7 @@ #include "interfaces/time_stamped.hpp" -namespace world_exe::tongji::time_stamp { +namespace world_exe::tongji::predictor { class TimeStamp : public interfaces::ITimeStamped { public: TimeStamp(const std::time_t& time_stamp) diff --git a/src/tongji/state_machine/car_state_manager.hpp b/src/tongji/state_machine/car_state_manager.hpp new file mode 100644 index 0000000..219e457 --- /dev/null +++ b/src/tongji/state_machine/car_state_manager.hpp @@ -0,0 +1,62 @@ +#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 990bf99..442148d 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,38 +1,40 @@ #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 { -class StateMachine::Impl { -public: - Impl(std::shared_ptr live_target_manager) - : live_target_manager_(std::move(live_target_manager)) { } +StateMachine::StateMachine(int switch_threshold) + : switch_threshold_(switch_threshold) { + for (auto& manager : car_states_) { + manager = car_state::CarStateManager(switch_threshold_); + } +} - const enumeration::CarIDFlag& GetAllowdToFires() const { return target_ids_; } +const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return tracing_state_; } - void Update() { - auto live_target_manager = - std::dynamic_pointer_cast(live_target_manager_); - target_ids_ = live_target_manager->GetAllowedTargetID(); - } +const interfaces::ICarState& StateMachine::Update( + const enumeration::CarIDFlag& car_detected, std::time_t now) { + tracing_state_ = enumeration::CarIDFlag::None; -private: - std::shared_ptr live_target_manager_; - enumeration::CarIDFlag 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; -StateMachine::StateMachine( - std::shared_ptr live_target_manager) - : pimpl_(std::make_unique(live_target_manager)) { } -StateMachine::~StateMachine() = default; + car_states_[i].Update(detected, now); + + if (detected) { + car_states_[i].IncrementUpdateCount(); + } + + if (car_states_[i].IsLocked() && car_states_[i].IsConverged() + && !car_states_[i].IsDiverged()) { + tracing_state_ = + static_cast(static_cast(tracing_state_) | (1 << i)); + } + } -const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { - return pimpl_->GetAllowdToFires(); + return *this; } } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index c3417e3..8fb9872 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -1,23 +1,24 @@ #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(); - ~StateMachine(); + StateMachine(int switch_threshold = 5); - const enumeration ::CarIDFlag& GetAllowdToFires() const override; - StateMachine(std::shared_ptr live_target_manager); + const enumeration::CarIDFlag& GetAllowdToFires() const override; + + const interfaces::ICarState& Update( + const enumeration::CarIDFlag& car_detected, std::time_t now); private: - class Impl; - std::unique_ptr pimpl_; + enumeration::CarIDFlag tracing_state_ = enumeration::CarIDFlag::None; + std::array car_states_; + int switch_threshold_; }; } \ No newline at end of file diff --git a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp b/src/util/extended_kalman_filter.hpp similarity index 68% rename from src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp rename to src/util/extended_kalman_filter.hpp index ce90cfd..73fc86b 100644 --- a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp +++ b/src/util/extended_kalman_filter.hpp @@ -1,36 +1,23 @@ #pragma once +#include #include #include #include #include -#include - -namespace world_exe::tongji::predictor { -template // +namespace world_exe::util { class ExtendedKalmanFilter { public: - 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; + Eigen::VectorXd x; + Eigen::MatrixXd P; ExtendedKalmanFilter() = default; ExtendedKalmanFilter( - const XVec& x0, const PMat& P0, - std::function x_add = [](const XVec& a, const XVec& b) { - return a + b; - }) { + const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0, + std::function x_add = + [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) { data["residual_yaw"] = 0.0; data["residual_pitch"] = 0.0; data["residual_distance"] = 0.0; @@ -42,12 +29,13 @@ class ExtendedKalmanFilter { data["recent_nis_failures"] = 0.0; } - 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; }) { + 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; }) { auto x_n = f(x, dt); @@ -102,8 +90,8 @@ class ExtendedKalmanFilter { double last_nis; private: - Eigen::Matrix I; - std::function x_add; + Eigen::MatrixXd I; + std::function x_add; int nees_count_ = 0; int nis_count_ = 0;