add fire_controller

src/tongji/fire_controller/fire_controller.cpp

@@ -0,0 +1,114 @@
+#include "fire_controller.hpp"
+
+#include <memory>
+#include <utility>
+
+#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<interfaces::ICarState> state_machine,
+        std::shared_ptr<interfaces::ITargetPredictor> live_target_manager)
+        : control_delay_(control_delay_in_second)
+        , bullet_speed_(bullet_speed)
+        , fire_decision_(std::make_unique<FireDecision>(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<double>::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<TargetSnapshotManager>(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<interfaces::IArmorInImage> armors, const double& gimbal_yaw) {
+        time_stamp_.SetTimeStamp(std::time(nullptr));
+        UpdateIdentifiedArmor(armors);
+        UpdateGimbalPosition(gimbal_yaw);
+    }
+
+private:
+    void UpdateIdentifiedArmor(std::shared_ptr<interfaces::IArmorInImage> 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<interfaces::IArmorInImage> identified_armors_;
+
+    std::unique_ptr<FireDecision> fire_decision_;
+    time_stamp::TimeStamp time_stamp_ { std::time(nullptr) };
+
+    std::shared_ptr<interfaces::ICarState> state_machine_;
+    std::shared_ptr<interfaces::ITargetPredictor> live_target_manager_;
+};
+
+FireController::FireController(std::shared_ptr<interfaces::ICarState> state_machine, bool auto_fire,
+    const double& control_delay_in_second, const double& bullet_speed, double yaw_offset,
+    double pitch_offset, std::shared_ptr<interfaces::ITargetPredictor> live_target_manager)
+    : pimpl_(std::make_unique<Impl>(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(); }
+
+}

src/tongji/fire_controller/fire_controller.hpp

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <memory>
+
+#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<interfaces::ICarState> state_machine, bool auto_fire,
+        const double& control_delay_in_second, const double& bullet_speed, double yaw_offset,
+        double pitch_offset, std::shared_ptr<interfaces::ITargetPredictor> 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<interfaces::IArmorInImage> armors, const double& gimbal_yaw);
+
+    time_stamp::TimeStamp GetTimeStamp() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+}

src/tongji/fire_controller/fire_decision.hpp

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <cmath>
+#include <cstdlib>
+
+#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<double>::quiet_NaN(),
+              std::numeric_limits<double>::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 };   // 距离判断阈值
+};
+
+}

src/tongji/identifier/armor_filter.hpp

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+#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<data::ArmorImageSpacing> FilterArmor(
+        std::vector<data::ArmorImageSpacing> 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<enumeration::ArmorIdFlag> invincible_armor_;
+};
+}

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<data::ArmorImageSpacing>& 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<std::vector<data::ArmorImageSpacing>, 8> armors_;
 };
 }

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"
 

src/tongji/predictor/in_gimbal_control_armor.hpp

@@ -4,10 +4,10 @@
 #include <unordered_map>
 #include <vector>
 
+#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<enumeration::ArmorIdFlag, std::vector<data::ArmorGimbalControlSpacing>>
         armors_map_;
     static const std::vector<data::ArmorGimbalControlSpacing> empty;

src/util/extended_kalman_filter.hpp → src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp

@@ -1,23 +1,36 @@
 #pragma once
 
-#include <Eigen/Dense>
 #include <deque>
 #include <functional>
 #include <map>
 #include <numeric>
 
-namespace world_exe::util {
+#include <Eigen/Dense>
+
+namespace world_exe::tongji::predictor {
+template <int xn, int zn> //
 class ExtendedKalmanFilter {
 public:
-    Eigen::VectorXd x;
-    Eigen::MatrixXd P;
+    using XVec = Eigen::Matrix<double, xn, 1>;
+    using ZVec = Eigen::Matrix<double, zn, 1>;
+    using AMat = Eigen::Matrix<double, xn, xn>;
+    using PMat = Eigen::Matrix<double, xn, xn>;
+    using PDig = Eigen::Matrix<double, xn, 1>;
+    using RMat = Eigen::Matrix<double, zn, zn>;
+    using RDig = Eigen::Matrix<double, zn, 1>;
+    using QMat = Eigen::Matrix<double, xn, xn>;
+    using HMat = Eigen::Matrix<double, zn, xn>;
+
+    XVec x;
+    PMat P;
 
     ExtendedKalmanFilter() = default;
 
     ExtendedKalmanFilter(
-        const Eigen::VectorXd& x0, const Eigen::MatrixXd& P0,
-        std::function<Eigen::VectorXd(const Eigen::VectorXd&, const Eigen::VectorXd&)> x_add =
-            [](const Eigen::VectorXd& a, const Eigen::VectorXd& b) { return a + b; }) {
+        const XVec& x0, const PMat& P0,
+        std::function<XVec(const XVec&, const XVec&)> 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<Eigen::VectorXd(const Eigen::VectorXd&, const double&)> f,
-        const Eigen::VectorXd& z, const Eigen::MatrixXd& H, const Eigen::MatrixXd& R,
-        std::function<Eigen::VectorXd(const Eigen::VectorXd&)> h,
-        std::function<Eigen::VectorXd(const Eigen::VectorXd&, const Eigen::VectorXd&)> 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<XVec(const XVec&, const double&)> f, const ZVec& z, const HMat& H,
+        const RMat& R, std::function<ZVec(const XVec&)> h,
+        std::function<ZVec(const ZVec&, const ZVec&)> 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<Eigen::VectorXd(const Eigen::VectorXd&, const Eigen::VectorXd&)> x_add;
+    Eigen::Matrix<double, xn, xn> I;
+    std::function<XVec(const XVec&, const XVec&)> x_add;
 
     int nees_count_  = 0;
     int nis_count_   = 0;