Tongji dev 纠正bug

src/tongji/auto_aim_system.cpp

@@ -18,9 +18,7 @@
 #include "enum/car_id.hpp"
 #include "parameters/params_system_v1.hpp"
 #include "parameters/profile.hpp"
-#include "parameters/rm_parameters.hpp"
 #include "tongji/fire_controller/fire_controller.hpp"
-#include "tongji/identifier/identifier.hpp"
 #include "tongji/predictor/car_predictor/car_predictor_manager.hpp"
 #include "tongji/solver/solver.hpp"
 #include "tongji/state_machine/state_machine.hpp"
@@ -68,15 +66,16 @@ class AutoAimSystem::Impl {
         //     util::visualization::draw_armor_in_image(*armors_in_image, visualized);
         //     cv::imshow("identified", visualized);
         //     cv::waitKey(1);
-        // } else {
-        //     std::printf("No identified armors/n");
         // }
         // if (fps_.count()) std::cout << fps_.fps() << std::endl;
-        // return;
 
-        if (flag == enumeration::ArmorIdFlag::None) return;
-        // std::cout << "here" << std::endl;
-        state_machine_->Update(armors_in_image,
+        if (flag == enumeration::ArmorIdFlag::None) {
+            state_machine_->SetLostState();
+            return;
+        }
+
+        // TODO:update invincible_armors
+        state_machine_->Update(armors_in_image, enumeration::CarIDFlag::None,
             std::chrono::duration_cast<milliseconds>(
                 std::chrono::steady_clock::now() - time_stamp_));
 
@@ -99,8 +98,6 @@ class AutoAimSystem::Impl {
             parameters::ParamsForSystemV1::tracker_current_armors_event,
             live_target_manager_->Predict(flag, pack.camera_capture_begin_time_stamp));
 
-        state_machine_ = std::make_shared<state_machine::StateMachine>();
-
         const auto target_id = state_machine_->GetAllowdToFires();
 
         const auto gimbal_yaw = R_camera2gimbal.eulerAngles(2, 1, 0)[0];
@@ -111,6 +108,7 @@ class AutoAimSystem::Impl {
 
         core::EventBus::Publish<data::FireControl>(
             parameters::ParamsForSystemV1::fire_control_event, GetControlCommand());
+        time_stamp_ = std::chrono::steady_clock::now();
 
         if (!debug) [[likely]]
             return;

src/tongji/identifier/armor_filter.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <ranges>
 #include <unordered_set>
 #include <utility>
 #include <vector>
@@ -15,25 +16,18 @@ class ArmorFilter {
 public:
     explicit ArmorFilter()
         : invincible_armor_({ }) { }
-    std::vector<data::ArmorImageSpacing> FilterArmor(
-        std::vector<data::ArmorImageSpacing> armors) const {
 
-        armors.erase(std::remove_if(armors.begin(), armors.end(), [&](const auto& armor) {
+    auto FilterArmor(std::vector<data::ArmorImageSpacing> const& armors) const {
         // 25赛季没有5号装甲板
         // 不打前哨站
         // 过滤掉刚复活无敌的装甲板
+        auto filtered = armors | std::views::filter([&](const auto& armor) {
+            return armor.id != enumeration::ArmorIdFlag::InfantryV
+                && armor.id != enumeration::ArmorIdFlag::Outpost
+                && !invincible_armor_.count(armor.id);
+        });
 
-            return armor.id == enumeration::ArmorIdFlag::InfantryV
-                || armor.id == enumeration::ArmorIdFlag::Outpost
-                || invincible_armor_.count(armor.id);
-        }));
-        // 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); }));
-
-        return armors;
+        return std::vector<data::ArmorImageSpacing>(filtered.begin(), filtered.end());
     }
 
     void Update(enumeration::CarIDFlag ids) {

src/tongji/identifier/tracker.hpp

@@ -1,7 +1,7 @@
 #pragma once
 
 #include <ctime>
-#include <iostream>
+
 #include <memory>
 #include <opencv2/core/types.hpp>
 #include <vector>
@@ -32,38 +32,41 @@ class Tracker final {
     ~Tracker() = default;
 
     auto SelectTrackingTargetID(const std::shared_ptr<interfaces::IArmorInImage>& armors_in_image,
+        const enumeration::CarIDFlag& invincible_armors,
         const std::chrono::milliseconds& duration_from_last_update) noexcept
         -> enumeration::ArmorIdFlag const {
 
         CheckCameraOffline(duration_from_last_update);
+        armor_filter_->Update(invincible_armors);
 
         auto filtered_ids = enumeration::ArmorIdFlag::None;
 
-        std::vector<data::ArmorImageSpacing> filtered_armors;
+        std::vector<data::ArmorImageSpacing> filtered_armors { };
         for (uint32_t i = 0; i < static_cast<int>(enumeration::ArmorIdFlag::Count); ++i) {
             auto id = static_cast<enumeration::ArmorIdFlag>(
                 static_cast<uint32_t>(enumeration::ArmorIdFlag::Hero) << i);
 
-            if (armors_in_image->GetArmors(id).empty()) continue;
+            if (auto armors = armors_in_image->GetArmors(id); !armors.empty()) {
+                // 对从图像识别到的装甲板进行过滤
+                filtered_armors = armor_filter_->FilterArmor(armors);
+
+                if (filtered_armors.empty()) continue;
 
-            // 图像中出现的装甲板
-            auto armors = armors_in_image->GetArmors(id);
-            // 对从图像识别到的装甲板进行过滤
-            filtered_armors = armor_filter_->FilterArmor(armors);
-            if (!filtered_armors.empty()) {
                 filtered_ids =
                     static_cast<enumeration::ArmorIdFlag>(static_cast<uint32_t>(filtered_ids)
                         | static_cast<uint32_t>(filtered_armors[0].id));
             }
         }
 
-        // UpdateState(!(filtered_ids == enumeration::ArmorIdFlag::None));
+        UpdateState(filtered_ids != enumeration::ArmorIdFlag::None);
+
+        if (state_ == TrackState::Tracking)
+            tracking_car_id_ = decider_->GetBestArmor(filtered_armors);
 
-        tracking_car_id_ = decider_->GetBestArmor(filtered_armors);
         return tracking_car_id_;
     }
 
-    TrackState GetState() const { return state_; }
+    void SetLostState() { state_ = TrackState::Lost; }
 
 private:
     void UpdateState(bool found) {
@@ -129,8 +132,11 @@ class Tracker final {
     }
 
     void CheckCameraOffline(const std::chrono::milliseconds duration_from_last_update) {
-        if (state_ != TrackState::Lost && (duration_from_last_update > timeout_sec_))
+        // if (state_ != TrackState::Lost && (duration_from_last_update > timeout_sec_);
+        if ((duration_from_last_update > timeout_)) {
             SetState(TrackState::Lost);
+            // std::cout << "I am lost QAQ" << std::endl;
+        }
     }
 
     void SetState(TrackState new_state) {
@@ -147,14 +153,14 @@ class Tracker final {
     std::unique_ptr<identifier::ArmorFilter> armor_filter_;
     std::unique_ptr<Decider> 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;
-    const std::chrono::milliseconds timeout_sec_ = std::chrono::milliseconds(100);
+    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;
+    const std::chrono::milliseconds timeout_ = std::chrono::milliseconds(100);
 };
 
 }

src/tongji/solver/solver.cpp

@@ -34,7 +34,7 @@ class Solver::Impl {
     std::shared_ptr<world_exe::interfaces::IArmorInCamera> EstimateAllArmorPoses(
         std::shared_ptr<interfaces::IArmorInImage> armors_in_image) {
         std::vector<data::ArmorCameraSpacing> armor_plates;
-
+        if (!armors_in_image) return nullptr;
         for (int i = 0; i < static_cast<int>(enumeration::ArmorIdFlag::Count); i++) {
             const auto& armor_id = util::enumeration::GetArmorIdFlag(i);
             const auto& armors   = armors_in_image->GetArmors(armor_id);

src/tongji/state_machine/state_machine.cpp

@@ -1,6 +1,5 @@
 #include "state_machine.hpp"
 
-#include <iostream>
 #include <memory>
 
 #include "../identifier/tracker.hpp"
@@ -17,8 +16,17 @@ class StateMachine::Impl {
     const enumeration::CarIDFlag& GetAllowdToFires() const { return target_id_; }
 
     void Update(std::shared_ptr<interfaces::IArmorInImage> armors_in_image,
+        const enumeration::CarIDFlag& invincible_armors,
         const std::chrono::milliseconds& duration_from_last_update) {
-        target_id_ = tracker_->SelectTrackingTargetID(armors_in_image, duration_from_last_update);
+
+        target_id_ = enumeration::CarIDFlag::None;
+        target_id_ = tracker_->SelectTrackingTargetID(
+            armors_in_image, invincible_armors, duration_from_last_update);
+    }
+
+    void SetLostState() {
+        target_id_ = enumeration::CarIDFlag::None;
+        tracker_->SetLostState();
     }
 
 private:
@@ -35,7 +43,10 @@ const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const {
 }
 
 void StateMachine::Update(std::shared_ptr<interfaces::IArmorInImage> armors_in_image,
+    const enumeration::CarIDFlag& invincible_armors,
     const std::chrono::milliseconds& duration_from_last_update) {
-    return pimpl_->Update(armors_in_image, duration_from_last_update);
+    return pimpl_->Update(armors_in_image, invincible_armors, duration_from_last_update);
 }
+
+void StateMachine::SetLostState() { return pimpl_->SetLostState(); }
 }

src/tongji/state_machine/state_machine.hpp

@@ -16,8 +16,11 @@ class StateMachine final : public interfaces::ICarState {
     const enumeration ::CarIDFlag& GetAllowdToFires() const override;
 
     void Update(std::shared_ptr<interfaces::IArmorInImage> armors_in_image,
+        const enumeration::CarIDFlag& invincible_armors,
         const std::chrono::milliseconds& duration_from_last_update);
 
+    void SetLostState();
+
     StateMachine(const StateMachine&)                = delete;
     StateMachine& operator=(const StateMachine&)     = delete;
     StateMachine(StateMachine&&) noexcept            = default;