diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 48ecfc0..ce640c8 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/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( 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(); - 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( parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); + time_stamp_ = std::chrono::steady_clock::now(); if (!debug) [[likely]] return; diff --git a/src/tongji/identifier/armor_filter.hpp b/src/tongji/identifier/armor_filter.hpp index 8189cd3..eb46f93 100644 --- a/src/tongji/identifier/armor_filter.hpp +++ b/src/tongji/identifier/armor_filter.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -15,25 +16,18 @@ class ArmorFilter { public: explicit ArmorFilter() : invincible_armor_({ }) { } - std::vector FilterArmor( - std::vector armors) const { - armors.erase(std::remove_if(armors.begin(), armors.end(), [&](const auto& armor) { + auto FilterArmor(std::vector 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(filtered.begin(), filtered.end()); } void Update(enumeration::CarIDFlag ids) { diff --git a/src/tongji/identifier/tracker.hpp b/src/tongji/identifier/tracker.hpp index e11a045..1db24c9 100644 --- a/src/tongji/identifier/tracker.hpp +++ b/src/tongji/identifier/tracker.hpp @@ -1,7 +1,7 @@ #pragma once #include -#include + #include #include #include @@ -32,38 +32,41 @@ class Tracker final { ~Tracker() = default; auto SelectTrackingTargetID(const std::shared_ptr& 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 filtered_armors; + 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; + 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(static_cast(filtered_ids) | static_cast(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 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; - 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); }; } diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index a9a91d4..36ffb49 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -34,7 +34,7 @@ class Solver::Impl { std::shared_ptr EstimateAllArmorPoses( std::shared_ptr armors_in_image) { std::vector armor_plates; - + if (!armors_in_image) return nullptr; for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); i++) { const auto& armor_id = util::enumeration::GetArmorIdFlag(i); const auto& armors = armors_in_image->GetArmors(armor_id); diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 0115ade..6400fab 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -1,6 +1,5 @@ #include "state_machine.hpp" -#include #include #include "../identifier/tracker.hpp" @@ -17,8 +16,17 @@ class StateMachine::Impl { const enumeration::CarIDFlag& GetAllowdToFires() const { return target_id_; } void Update(std::shared_ptr 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 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(); } } diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 14747b5..82895da 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/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 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; diff --git a/tests/mocks/MockCamera.hpp b/tests/mocks/MockCamera.hpp deleted file mode 100644 index e58392a..0000000 --- a/tests/mocks/MockCamera.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#include - -#include "data/time_stamped.hpp" - -namespace world_exe::tests::mock { - -struct MockCamera { - Eigen::Vector3d position; - Eigen::Matrix3d orientation; - - MockCamera() - : position(Eigen::Vector3d::Zero()) - , orientation(Eigen::Matrix3d::Identity()) { } -}; - -class MockCameraInGimbal final { -public: - MockCameraInGimbal(double angular_speed = 1) - : time(std::chrono::steady_clock::now().time_since_epoch()) - , camera({ }) - , gimbal_origin(Eigen::Vector3d(0, 0, 0)) { } - - const data::TimeStamp& GetTimeStamp() const { return time; } - - void RotateYawPitchAndTranslate( - double t, double yaw_speed, double pitch_speed, const Eigen::Vector3d& linear_velocity) { - double yaw_angle = yaw_speed * t; - double pitch_angle = pitch_speed * t; - - Eigen::Matrix3d R_yaw = - Eigen::AngleAxisd(yaw_angle, Eigen::Vector3d::UnitZ()).toRotationMatrix(); - Eigen::Matrix3d R_pitch = - Eigen::AngleAxisd(pitch_angle, Eigen::Vector3d::UnitY()).toRotationMatrix(); - Eigen::Matrix3d rotation = R_yaw * R_pitch; - - Eigen::Vector3d translation = linear_velocity * t; - - MoveCamera(rotation, translation); - } - -private: - void MoveCamera(const Eigen::Matrix3d& rotation_matrix, const Eigen::Vector3d& translation) { - Eigen::Vector3d relative = camera.position - gimbal_origin; - camera.position = gimbal_origin + rotation_matrix * relative + translation; - camera.orientation = rotation_matrix * camera.orientation; - } - - data::TimeStamp time; - MockCamera camera; - Eigen::Vector3d gimbal_origin; -}; -} \ No newline at end of file