From 5fae3f24c259f8598b1262248496dde8b7b1922d Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 11 Nov 2025 00:06:59 +0800 Subject: [PATCH 01/14] refactor: adjust function signatures to eliminate clangd warnings --- src/tongji/auto_aim_system.cpp | 2 +- .../fire_controller/aim_point_chooser.hpp | 6 +-- src/tongji/fire_controller/aim_solver.hpp | 6 +-- .../fire_controller/fire_controller.cpp | 19 ++++---- .../fire_controller/fire_controller.hpp | 4 +- src/tongji/fire_controller/trajectory.hpp | 2 +- src/tongji/identifier/armor_filter.hpp | 3 +- src/tongji/identifier/classifier.hpp | 2 +- src/tongji/identifier/decider.cpp | 2 + src/tongji/identifier/decider.hpp | 2 +- src/tongji/identifier/identifier.cpp | 14 +++--- src/tongji/identifier/tracker.hpp | 2 +- .../predictor/car_predictor/car_predictor.hpp | 11 ++--- .../car_predictor/car_predictor_manager.cpp | 12 +++-- .../car_predictor/car_predictor_manager.hpp | 4 +- .../kalman_filter/extended_kalman_filter.hpp | 14 +++--- .../predictor/kalman_filter/predict_model.hpp | 46 +++++++++---------- src/tongji/solver/reprojection_util.hpp | 18 ++++---- src/tongji/solver/solved_armor.hpp | 5 +- src/tongji/solver/solver.cpp | 22 ++++----- src/tongji/solver/solver.hpp | 4 +- src/tongji/state_machine/state_machine.cpp | 4 +- src/tongji/state_machine/state_machine.hpp | 2 +- 23 files changed, 104 insertions(+), 102 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index ce640c8..bd92ada 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -30,7 +30,7 @@ using namespace std::chrono; class AutoAimSystem::Impl { public: - Impl(const bool& debug) + explicit Impl(const bool& debug) : debug(debug) , config_path_("/workspaces/src/alliance_ros_auto_aim/alliance_auto_aim/configs/" "example.yaml") diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index 51e2c71..690ce25 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -12,7 +12,7 @@ using CarIDFlag = enumeration::CarIDFlag; class AimPointChooser { public: - AimPointChooser(const std::string& config_path) { + explicit AimPointChooser(const std::string& config_path) { auto yaml = YAML::LoadFile(config_path); comming_angle_ = yaml["comming_angle"].as() / 57.3; // degree to rad leaving_angle_ = yaml["leaving_angle"].as() / 57.3; // degree to rad @@ -20,7 +20,7 @@ class AimPointChooser { std::pair ChooseAimArmor( const Eigen::Vector& ekf_x, - const std::vector armors, const CarIDFlag& single_id) { + std::vector const& armors, const CarIDFlag& single_id) { const auto armor_num = armors.size(); int chosen_id = -1; @@ -31,7 +31,7 @@ class AimPointChooser { for (int i = 0; i < armor_num; i++) { auto delta_angle = util::math::clamp_pm_pi( util::math::get_yaw_from_quaternion(armors[i].orientation) - center_yaw); - delta_angle_list.emplace_back(std::make_tuple(i, delta_angle)); + delta_angle_list.emplace_back(i, delta_angle); } std::sort(delta_angle_list.begin(), delta_angle_list.end(), [](const auto& a, const auto& b) { return std::get<1>(a) > std::get<1>(b); }); diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index cdeeb8f..9c17ea6 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -29,7 +29,7 @@ class AimingSolver { using PredictorModel = predictor::EKFModel<11, 4>; using EKF = predictor::ExtendedKalmanFilter; - AimingSolver(const std::string& config_path, const double& gravity = 9.7833) + explicit AimingSolver(const std::string& config_path, const double& gravity = 9.7833) : aim_point_chooser_(std::make_unique(config_path)) , g_(gravity) { @@ -39,8 +39,8 @@ class AimingSolver { bullet_speed_ = yaml["bullet_speed"].as(); } - AimSolution SolveAimSolution(std::shared_ptr snapshot, - data::TimeStamp time_stamp, const double& control_delay_s) { + AimSolution SolveAimSolution(std::shared_ptr const& snapshot, + data::TimeStamp const& time_stamp, const double& control_delay_s) { // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) double prev_fly_time_s; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 1e3a21e..9ee8e71 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include "../identifier/identified_armor.hpp" @@ -29,14 +30,14 @@ class FireController::Impl { , firable_(false) , aiming_solver_(std::make_unique(config_path)) , fire_decision_(std::make_unique(config_path)) - , state_machine_(state_machine) - , live_target_manager_(live_target_manager) { + , state_machine_(std::move(state_machine)) + , live_target_manager_(std::move(live_target_manager)) { auto yaml = YAML::LoadFile(config_path); control_delay_s_ = yaml["control_delay_s"].as(); } - const data ::FireControl CalculateTarget( + data ::FireControl CalculateTarget( const std::chrono::seconds& time_from_tracker_timepoint) const { if (!fire_decision_ || !state_machine_ || !live_target_manager_) @@ -57,8 +58,8 @@ class FireController::Impl { const auto gimbal_command = GimbalCommand { aim_solution.yaw, aim_solution.pitch }; const auto target_pos = Eigen::Vector3d { aim_solution.aim_point }; auto fire_command = aim_solution.valid - ? fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_pos) - : false; + ? fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_pos) + : false; firable_ = fire_command; data::FireControl result; @@ -68,7 +69,7 @@ class FireController::Impl { return result; } - const CarIDFlag GetAttackCarId() const { + CarIDFlag GetAttackCarId() const { if (firable_) return locked_target; return CarIDFlag::None; } @@ -81,7 +82,7 @@ class FireController::Impl { double gimbal_yaw_; CarIDFlag locked_target; - mutable double firable_; + mutable bool firable_; std::unique_ptr aiming_solver_; std::unique_ptr fire_decision_; @@ -90,8 +91,8 @@ class FireController::Impl { }; FireController::FireController(const std::string& config_path, - std::shared_ptr state_machine, - std::shared_ptr live_target_manager) + std::shared_ptr const& state_machine, + std::shared_ptr const& live_target_manager) : pimpl_(std::make_unique(config_path, state_machine, live_target_manager)) { } FireController::~FireController() = default; diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index ffa7d15..a2afa05 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -12,8 +12,8 @@ namespace world_exe::tongji::fire_control { class FireController final : public interfaces::IFireControl { public: FireController(const std::string& config_path, - std::shared_ptr state_machine, - std::shared_ptr live_target_manager); + std::shared_ptr const& state_machine, + std::shared_ptr const& live_target_manager); ~FireController(); const data ::FireControl CalculateTarget( diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp index 2b9c5e1..7c599d8 100644 --- a/src/tongji/fire_controller/trajectory.hpp +++ b/src/tongji/fire_controller/trajectory.hpp @@ -17,7 +17,7 @@ struct TrajectorySolver { // 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 { + -> TrajectoryResult { auto a = g * d * d / (2 * v0 * v0); auto b = -d; auto c = a + h; diff --git a/src/tongji/identifier/armor_filter.hpp b/src/tongji/identifier/armor_filter.hpp index eb46f93..0ab7e66 100644 --- a/src/tongji/identifier/armor_filter.hpp +++ b/src/tongji/identifier/armor_filter.hpp @@ -2,7 +2,6 @@ #include #include -#include #include #include "data/armor_image_spaceing.hpp" @@ -33,7 +32,7 @@ class ArmorFilter { void Update(enumeration::CarIDFlag ids) { invincible_armor_.clear(); for (auto id : util::enumeration::ExpandArmorIdFlags(ids)) { - invincible_armor_.insert(std::move(id)); + invincible_armor_.insert(id); } } diff --git a/src/tongji/identifier/classifier.hpp b/src/tongji/identifier/classifier.hpp index ca8e919..2367c6e 100644 --- a/src/tongji/identifier/classifier.hpp +++ b/src/tongji/identifier/classifier.hpp @@ -13,7 +13,7 @@ namespace world_exe::tongji::identifier { -// TODO:早期代码，和深大模型不适配，需要TODOTODO +// TODO:和深大模型不适配，需要TODOTODO class Classifier final { public: explicit Classifier(const std::string& config_path) { diff --git a/src/tongji/identifier/decider.cpp b/src/tongji/identifier/decider.cpp index 994bf39..6150c4c 100644 --- a/src/tongji/identifier/decider.cpp +++ b/src/tongji/identifier/decider.cpp @@ -20,6 +20,8 @@ class Decider::Impl { const PriorityMap& priority_map = (mode_ == PriorityMode::MODE_ONE) ? mode1 : mode2; std::vector> armors_list; + armors_list.reserve(armors.size()); + for (const auto& armor : armors) { armors_list.emplace_back(armor.id, priority_map.at(armor.id)); } diff --git a/src/tongji/identifier/decider.hpp b/src/tongji/identifier/decider.hpp index 4fef243..a83bc91 100644 --- a/src/tongji/identifier/decider.hpp +++ b/src/tongji/identifier/decider.hpp @@ -22,7 +22,7 @@ enum class ArmorPriority { class Decider { public: - Decider(PriorityMode mode = PriorityMode::MODE_ONE); + explicit Decider(PriorityMode mode = PriorityMode::MODE_ONE); ~Decider(); enumeration::ArmorIdFlag GetBestArmor(std::vector& armors) const; diff --git a/src/tongji/identifier/identifier.cpp b/src/tongji/identifier/identifier.cpp index 2f716f0..42541e5 100644 --- a/src/tongji/identifier/identifier.cpp +++ b/src/tongji/identifier/identifier.cpp @@ -53,8 +53,8 @@ class Identifier::Impl { void SetTargetColor(Color target_color) { target_color_ = target_color; } - const std::tuple, enumeration::CarIDFlag> - Identify(const cv::Mat& bgr_img) { + std::tuple, enumeration::CarIDFlag> Identify( + const cv::Mat& bgr_img) { // 彩色图转灰度图 cv::Mat gray_img; cv::cvtColor(bgr_img, gray_img, cv::COLOR_BGR2GRAY); @@ -201,7 +201,7 @@ class Identifier::Impl { cv::imwrite(img_path, armor_pattern); } - Color GetColor(const cv::Mat& bgr_img, const std::vector& contour) const { + static Color GetColor(const cv::Mat& bgr_img, const std::vector& contour) { int red_sum = 0, blue_sum = 0; for (const auto& point : contour) { @@ -212,8 +212,8 @@ class Identifier::Impl { return blue_sum > red_sum ? Color::blue : Color::red; } - cv::Mat GetPattern(const cv::Mat& bgr_img, const Lightbar& left_lightbar, - const Lightbar& right_lightbar) const { + static cv::Mat GetPattern( + const cv::Mat& bgr_img, const Lightbar& left_lightbar, const Lightbar& right_lightbar) { // 延长灯条获得装甲板角点 // 1.125 = 0.5 * armor_height / lightbar_length = 0.5 * 126mm / 56mm auto tl = left_lightbar.center - left_lightbar.top2bottom * 1.125; @@ -232,13 +232,13 @@ class Identifier::Impl { return bgr_img(roi); } - cv::Point2f GetCenterNorm(const cv::Mat& bgr_img, const cv::Point2f& center) const { + static cv::Point2f GetCenterNorm(const cv::Mat& bgr_img, const cv::Point2f& center) { auto h = bgr_img.rows; auto w = bgr_img.cols; return { center.x / w, center.y / h }; } - bool IsLargeArmor(double armor_ratio, enumeration::ArmorIdFlag armor_id) const { + static bool IsLargeArmor(double armor_ratio, enumeration::ArmorIdFlag armor_id) { /// 优先根据当前armor.ratio判断 /// TODO: 26赛季是否还需要根据比例判断大小装甲？能否根据图案直接判断？ diff --git a/src/tongji/identifier/tracker.hpp b/src/tongji/identifier/tracker.hpp index 1db24c9..1305537 100644 --- a/src/tongji/identifier/tracker.hpp +++ b/src/tongji/identifier/tracker.hpp @@ -34,7 +34,7 @@ class Tracker final { 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 { + -> enumeration::ArmorIdFlag { CheckCameraOffline(duration_from_last_update); armor_filter_->Update(invincible_armors); diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp index 570e851..3f4b144 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -50,7 +50,6 @@ class CarPredictor final : public interfaces::IPredictor { EKF::PMat P0 = model_.GetP0Dig().asDiagonal(); ekf_.emplace(x0, P0, model_); // 初始化滤波器（预测量、预测量协方差） - } const enumeration ::ArmorIdFlag& GetId() const override { return car_id_; } @@ -73,22 +72,22 @@ class CarPredictor final : public interfaces::IPredictor { } EKF::XVec GetEkfX() const { return ekf_->x; } - auto GetModel() const -> const PredictorModel { return model_; } - auto GetEkf() const -> const EKF { return ekf_.value(); } + auto GetModel() const -> PredictorModel { return model_; } + auto GetEkf() const -> EKF { return ekf_.value(); } data::TimeStamp LastSeen() const { return time_stamp_; } - auto GetPredictedXYZAList(const double& dt) -> std::vector const { + auto GetPredictedXYZAList(const double& dt) -> std::vector { const auto [x_n, P_n] = ekf_->PredictOnce(dt); return model_.GetArmorXYZAList(x_n); } - auto GetPredictedX(const double& dt) const -> const EKF::XVec { + auto GetPredictedX(const double& dt) const -> EKF::XVec { const auto& [x_n, P_n] = ekf_->PredictOnce(dt); return x_n; } - void Update(const data::TimeStamp time_stamp, const Eigen::Vector3d& armor_xyz_in_gimbal, + void Update(data::TimeStamp const& time_stamp, const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) { // 装甲板匹配 diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp index ae0d276..1a65a6e 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include "../in_gimbal_control_armor.hpp" @@ -19,7 +20,7 @@ namespace world_exe::tongji::predictor { class CarPredictorManager::Impl { public: - Impl(const std::string& config_path, const double& timeout_sec) + Impl(std::string_view config_path, const double& timeout_sec) : targets_map_() , last_update_timestamp_(data::TimeStamp { }) , config_path_(config_path) { } @@ -34,7 +35,7 @@ class CarPredictorManager::Impl { // if (!predictor->IsConverged()) { // continue; // } - // if (predictor->IsConverged()) + // if (predictor->IsConverged()) { auto spacings = predictor->Predictor(time_stamp); result.insert(result.end(), spacings->GetArmors(id).begin(), @@ -55,7 +56,7 @@ class CarPredictorManager::Impl { predictor->GetEkf(), predictor->GetModel(), predictor->LastSeen()); } - void Update(std::shared_ptr data) { + void Update(std::shared_ptr const& data) { last_update_timestamp_ = data->GetTimeStamp(); const Eigen::Affine3d transform = data->GetCameraToWorld(); @@ -81,7 +82,8 @@ class CarPredictorManager::Impl { } } - // std::erase_if(targets_map_, [](const auto& pair) { return pair.second->IsAppeared(); }); + // std::erase_if(targets_map_, [](const auto& pair) { return pair.second->IsAppeared(); + // }); } } @@ -105,7 +107,7 @@ std ::shared_ptr CarPredictorManager::GetPredictor( return pimpl_->GetPredictor(id); } -void CarPredictorManager::Update(std::shared_ptr data) { +void CarPredictorManager::Update(std::shared_ptr const& data) { return pimpl_->Update(data); } diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.hpp b/src/tongji/predictor/car_predictor/car_predictor_manager.hpp index d6190ba..40ae3b5 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.hpp @@ -11,7 +11,7 @@ namespace world_exe::tongji::predictor { class CarPredictorManager final : public interfaces::ITargetPredictor { public: - CarPredictorManager(const std::string& config_path, const double& timeout_sec = 0.1); + explicit CarPredictorManager(const std::string& config_path, const double& timeout_sec = 0.1); ~CarPredictorManager(); std ::shared_ptr Predict( @@ -19,7 +19,7 @@ class CarPredictorManager final : public interfaces::ITargetPredictor { std ::shared_ptr GetPredictor( const enumeration ::ArmorIdFlag& id) const override; - void Update(std::shared_ptr data); + void Update(std::shared_ptr const& data); CarPredictorManager(const CarPredictorManager&) = delete; CarPredictorManager& operator=(const CarPredictorManager&) = delete; diff --git a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp index bd95c1c..693752b 100644 --- a/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp +++ b/src/tongji/predictor/kalman_filter/extended_kalman_filter.hpp @@ -43,7 +43,7 @@ template class ExtendedKalmanFilter { : x(x0) , P(P0) , model_(model) - , I(Eigen::Matrix::Identity()) { + , I(Eigen::Matrix::Identity()) { // data["residual_yaw"] = 0.0; // data["residual_pitch"] = 0.0; // data["residual_distance"] = 0.0; @@ -56,7 +56,7 @@ template class ExtendedKalmanFilter { } // 无副作用，不修改x，仅预测 - auto PredictOnce(const double& dt) const -> const std::pair { + auto PredictOnce(const double& dt) const -> std::pair { const auto A = model_.A(dt); const auto Q = model_.Q(dt); const auto x_n = model_.f(x, dt); @@ -66,13 +66,13 @@ template class ExtendedKalmanFilter { } auto Update(const double& dt, const ZVec& z, const HMat& H, const RMat& R, const int& id) - -> const XVec { + -> XVec { const auto [x_prior, P_prior] = PredictOnce(dt); const ZVec z_prior = model_.h(x_prior, id); const ZVec residual = model_.z_subtract(z, z_prior); - const RMat S = H * P_prior * H.transpose() + R; + const RMat S = H * P_prior * H.transpose() + R; const Eigen::MatrixXd K = P_prior * H.transpose() * S.inverse(); // std::cout << P << "\n" << std::endl; @@ -91,15 +91,15 @@ template class ExtendedKalmanFilter { return x; } - auto IsDiverse() const -> bool const { return CalculateFailureRate() >= max_failure_rate; } + auto IsDiverse() const -> bool { return CalculateFailureRate() >= max_failure_rate; } private: - auto CalculateFailureRate() const -> bool const { + auto CalculateFailureRate() const -> bool { int failures = std::accumulate(recent_nis_failures.begin(), recent_nis_failures.end(), 0); return (double)failures / window_size; } - auto GetRecentNISFailureRate() const -> double const { + auto GetRecentNISFailureRate() const -> double { if (recent_nis_failures.empty()) return 0.0; int recent_failures = diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp index deed645..8124a8b 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -31,7 +31,7 @@ class EKFModel { using QMat = Eigen::Matrix; using HMat = Eigen::Matrix; - EKFModel(const enumeration::CarIDFlag& car_id) + explicit EKFModel(const enumeration::CarIDFlag& car_id) : car_id_(car_id) { bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII @@ -39,9 +39,7 @@ class EKFModel { || car_id == enumeration::CarIDFlag::InfantryV); P0_dig_.resize(11); - if (is_balance) { - P0_dig_ << 1, 64, 1, 64, 1, 64, 0.4, 100, 1, 1, 1; - } else if (car_id == enumeration::CarIDFlag::Outpost) { + if (car_id == enumeration::CarIDFlag::Outpost) { P0_dig_ << 1, 64, 1, 64, 1, 81, 0.4, 100, 1e-4, 0, 0; } else if (car_id == enumeration::CarIDFlag::Base) { P0_dig_ << 1, 64, 1, 64, 1, 64, 0.4, 100, 1e-4, 0, 0; @@ -67,19 +65,19 @@ class EKFModel { } } - auto GetP0Dig() const -> const PDig { return P0_dig_; } - auto GetRadius() const -> const double { return radius_; } - auto GetID() const -> const auto { return car_id_; } - auto GetArmorNum() const -> const int { return armor_num_; } + auto GetP0Dig() const -> PDig { return P0_dig_; } + auto GetRadius() const -> double { return radius_; } + auto GetID() const -> auto { return car_id_; } + auto GetArmorNum() const -> int { return armor_num_; } // 防止夹角求和出现异常值 - constexpr auto x_add(const XVec& a, const XVec& b) const -> const auto { + constexpr auto x_add(const XVec& a, const XVec& b) const -> auto { XVec c = a + b; c(6) = util ::math::clamp_pm_pi(c(6)); return c; } - constexpr auto z_substract(const ZVec& a, const ZVec& b) const -> const auto { + constexpr auto z_substract(const ZVec& a, const ZVec& b) const -> auto { auto c = a - b; c(0) = util::math::clamp_pm_pi(c(0)); c(1) = util::math::clamp_pm_pi(c(1)); @@ -87,11 +85,11 @@ class EKFModel { return c; } - auto A(double dt) const -> auto const { + auto A(double dt) const -> auto { // 状态转移矩阵 - AMat _A; + AMat A_mat; // clang-format off - _A<< + A_mat<< 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, @@ -104,12 +102,12 @@ class EKFModel { 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; + return A_mat; } // 防止夹角求和出现异常值 - auto f(const XVec& x, const double& dt)const ->const auto{ + auto f(const XVec& x, const double& dt)const -> auto{ XVec x_prior = this->A(dt) * x; x_prior(6) = util::math::clamp_pm_pi(x_prior(6)); return x_prior; @@ -118,7 +116,7 @@ class EKFModel { auto MatchArmor(const XVec& x, const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) const - ->const int { + -> int { const auto& xyza_list = GetArmorXYZAList(x); std::vector> xyza_i_list; @@ -151,7 +149,7 @@ class EKFModel { } // 计算出装甲板中心的坐标（考虑长短轴） - auto h_armor_xyz(const XVec& x, int id) const ->const auto { + auto h_armor_xyz(const XVec& x, int id) const -> auto { 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); @@ -163,7 +161,7 @@ class EKFModel { return Eigen::Vector3d { armor_x, armor_y, armor_z }; } - constexpr auto H(const XVec& x, int id) const->HMat const { + constexpr auto H(const XVec& x, int id) const->HMat { auto angle = util::math::clamp_pm_pi(x(6) + id * 2 * CV_PI / armor_num_); auto cos_angle=std::cos(angle); auto sin_angle=std::sin(angle); @@ -205,7 +203,7 @@ class EKFModel { } auto R(const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, - const Eigen::Vector3d& armor_ypd_in_gimbal, int id) const -> RMat const { + const Eigen::Vector3d& armor_ypd_in_gimbal, int id) const -> RMat { // Eigen::VectorXd R_dig{{4e-3, 4e-3, 1, 9e-2}}; auto center_yaw = std::atan2(armor_xyz_in_gimbal(1), armor_xyz_in_gimbal(0)); auto delta_angle = util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - center_yaw); @@ -217,7 +215,7 @@ class EKFModel { return R_dig.asDiagonal(); } - auto Q(const double& dt) const -> const auto { + auto Q(const double& dt) const -> auto { // Piecewise White Noise Model // https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python/blob/master/07-Kalman-Filter-Math.ipynb double v1, v2; @@ -235,9 +233,9 @@ class EKFModel { auto c = dt_ * dt_; // 预测过程噪声偏差的方差 - QMat _Q; + QMat Q_mat; // clang-format off - _Q + Q_mat << 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, @@ -251,7 +249,7 @@ class EKFModel { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; // clang-format on - return _Q; + return Q_mat; } Eigen::Vector4d h(const XVec& x, const int& id) const { @@ -270,7 +268,7 @@ class EKFModel { return c; }; - constexpr auto GetArmorXYZAList(const XVec& ekf_x) const -> const auto { + constexpr auto GetArmorXYZAList(const XVec& ekf_x) const -> auto { std::vector _armor_xyza_list; for (int i = 0; i < armor_num_; i++) { diff --git a/src/tongji/solver/reprojection_util.hpp b/src/tongji/solver/reprojection_util.hpp index 109163a..5d97812 100644 --- a/src/tongji/solver/reprojection_util.hpp +++ b/src/tongji/solver/reprojection_util.hpp @@ -21,11 +21,11 @@ class ReprojectionUtil { ReprojectionUtil() = default; ~ReprojectionUtil() = default; - double CalculateReprojectionError(const Eigen::Matrix3d& R_camera2gimbal, + static double CalculateReprojectionError(const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_gimbal2camera, const world_exe::data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& armor_yaw, - const double& armor_pitch, const double& inclined) const { + const double& armor_pitch, const double& inclined) { auto image_points = ReprojectArmor(R_camera2gimbal, t_gimbal2camera, armor_xyz_in_gimbal, armor_yaw, armor_pitch, armor_in_image.isLargeArmor); @@ -41,9 +41,9 @@ class ReprojectionUtil { } private: - std::vector ReprojectArmor(const Eigen::Matrix3d& R_camera2gimbal, + static std::vector ReprojectArmor(const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_gimbal2camera, const Eigen::Vector3d& armor_xyz_in_gimbal, - const double& armor_yaw, const double& armor_pitch, const bool& is_large) const { + const double& armor_yaw, const double& armor_pitch, const bool& is_large) { auto sin_yaw = std::sin(armor_yaw); auto cos_yaw = std::cos(armor_yaw); @@ -69,9 +69,9 @@ class ReprojectionUtil { // get rvec tvec cv::Vec3d rvec; - cv::Mat _R_armor2camera_cv; - cv::eigen2cv(R_armor2camera_cv, _R_armor2camera_cv); - cv::Rodrigues(_R_armor2camera_cv, rvec); + cv::Mat R_armor2camera_cv_mat; + cv::eigen2cv(R_armor2camera_cv, R_armor2camera_cv_mat); + cv::Rodrigues(R_armor2camera_cv_mat, rvec); cv::Vec3d tvec(t_armor2camera_cv[0], t_armor2camera_cv[1], t_armor2camera_cv[2]); std::vector image_points; @@ -84,8 +84,8 @@ class ReprojectionUtil { return image_points; } - double SJTU_cost(const std::vector& cv_refs, - const std::vector& cv_pts, const double& inclined) const { + static double SJTU_cost(const std::vector& cv_refs, + const std::vector& cv_pts, const double& inclined) { std::size_t size = cv_refs.size(); std::vector refs; std::vector pts; diff --git a/src/tongji/solver/solved_armor.hpp b/src/tongji/solver/solved_armor.hpp index 7e6fe34..57cbf82 100644 --- a/src/tongji/solver/solved_armor.hpp +++ b/src/tongji/solver/solved_armor.hpp @@ -3,14 +3,15 @@ #include #include -#include "interfaces/armor_in_camera.hpp" #include "data/time_stamped.hpp" +#include "interfaces/armor_in_camera.hpp" #include "util/index.hpp" namespace world_exe::tongji::solver { class SolvedArmor final : public interfaces::IArmorInCamera { public: - explicit SolvedArmor(const std::vector& armors, data::TimeStamp when_image_come) + explicit SolvedArmor( + const std::vector& armors, data::TimeStamp const& when_image_come) : time_stamp_(when_image_come) { for (const auto& armor : armors) { armors_[util::enumeration::GetIndex(armor.id)].emplace_back(armor); diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 36ffb49..7d26bc9 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -31,8 +31,8 @@ class Solver::Impl { , t_camera2gimbal_(Eigen::Vector3d::Zero()) , reprojection_util_(std::make_unique()) { } - std::shared_ptr EstimateAllArmorPoses( - std::shared_ptr armors_in_image) { + static std::shared_ptr EstimateAllArmorPoses( + std::shared_ptr const& armors_in_image) { std::vector armor_plates; if (!armors_in_image) return nullptr; for (int i = 0; i < static_cast(enumeration::ArmorIdFlag::Count); i++) { @@ -53,7 +53,7 @@ class Solver::Impl { t_camera2gimbal_ = t_camera2gimbal; } - auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto { + auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> auto { Eigen::Vector3d xyz_in_gimbal = R_camera2gimbal_.transpose() * xyz_in_camera + t_camera2gimbal_; return xyz_in_gimbal; @@ -61,7 +61,7 @@ class Solver::Impl { auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, - const double& initial_armor_yaw_in_gimbal) const -> const double { + const double& initial_armor_yaw_in_gimbal) const -> double { constexpr double SEARCH_RANGE = 140; // degree const auto yaw0 = util::math::clamp_pm_pi(gimbal_yaw - SEARCH_RANGE / 2 * CV_PI / 180.0); @@ -86,13 +86,13 @@ class Solver::Impl { return best_yaw; } - const data::TimeStamp GetTimeStamp() const { + static data::TimeStamp GetTimeStamp() { return data::TimeStamp(std::chrono::steady_clock::now().time_since_epoch()); } private: - data::ArmorCameraSpacing EstimatePose( - const world_exe::data::ArmorImageSpacing& armor_in_image) const { + static data::ArmorCameraSpacing EstimatePose( + const world_exe::data::ArmorImageSpacing& armor_in_image) { const auto& [xyz_in_camera, R_armor2camera] = EstimatePnp(armor_in_image); data::ArmorCameraSpacing pose; @@ -102,8 +102,8 @@ class Solver::Impl { return pose; } - auto EstimatePnp(const world_exe::data::ArmorImageSpacing& armor_in_image) const - -> const std::tuple { + static auto EstimatePnp(const world_exe::data::ArmorImageSpacing& armor_in_image) + -> std::tuple { const auto& object_points = armor_in_image.isLargeArmor ? parameters::Robomaster::LargeArmorObjectPointsOpencv : parameters::Robomaster::NormalArmorObjectPointsOpencv; @@ -151,12 +151,12 @@ void Solver::SetCamera2Gimbal( auto Solver::CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, - const double& initial_armor_yaw_in_gimbal) const -> const double { + const double& initial_armor_yaw_in_gimbal) const -> double { return pimpl_->CalculateOptimizeYaw( armor_in_image, armor_xyz_in_gimbal, gimbal_yaw, initial_armor_yaw_in_gimbal); } -auto Solver::Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto { +auto Solver::Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> auto { return pimpl_->Camera2Gimbal(xyz_in_camera); } } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 5247343..3a9ec51 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -15,10 +15,10 @@ class Solver final : public interfaces::IPnpSolver { void SetCamera2Gimbal( const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal); - auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto; + auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> auto; auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, - const double& initial_armor_yaw_in_gimbal) const -> const double; + const double& initial_armor_yaw_in_gimbal) const -> double; Solver(const Solver&) = delete; Solver& operator=(const Solver&) = delete; diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 6400fab..162e292 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -15,7 +15,7 @@ class StateMachine::Impl { const enumeration::CarIDFlag& GetAllowdToFires() const { return target_id_; } - void Update(std::shared_ptr armors_in_image, + void Update(std::shared_ptr const& armors_in_image, const enumeration::CarIDFlag& invincible_armors, const std::chrono::milliseconds& duration_from_last_update) { @@ -42,7 +42,7 @@ const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } -void StateMachine::Update(std::shared_ptr armors_in_image, +void StateMachine::Update(std::shared_ptr const& armors_in_image, const enumeration::CarIDFlag& invincible_armors, const std::chrono::milliseconds& duration_from_last_update) { return pimpl_->Update(armors_in_image, invincible_armors, duration_from_last_update); diff --git a/src/tongji/state_machine/state_machine.hpp b/src/tongji/state_machine/state_machine.hpp index 82895da..2366126 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -15,7 +15,7 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; - void Update(std::shared_ptr armors_in_image, + void Update(std::shared_ptr const& armors_in_image, const enumeration::CarIDFlag& invincible_armors, const std::chrono::milliseconds& duration_from_last_update); From fbea67447a0ed370bf06084bf3cea24cfa288ad7 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 13 Nov 2025 02:20:26 +0800 Subject: [PATCH 02/14] fix(predictor): correct timestamp in replica predictor and add getter method --- CMakeLists.txt | 2 +- configs/example.yaml | 9 +-- include/data/time_stamped.hpp | 4 ++ include/interfaces/fire_controller.hpp | 3 +- include/interfaces/predictor.hpp | 2 + src/tongji/auto_aim_system.cpp | 32 +++++++-- .../fire_controller/aim_point_chooser.hpp | 20 +++--- src/tongji/fire_controller/aim_solver.hpp | 72 ++++++++++++++----- .../fire_controller/fire_controller.cpp | 51 +++++++++---- .../fire_controller/fire_controller.hpp | 4 +- .../predictor/car_predictor/car_predictor.hpp | 4 +- .../car_predictor/car_predictor_manager.cpp | 2 +- .../predictor/kalman_filter/predict_model.hpp | 11 ++- src/util/parameters/params_system_v1.cpp | 9 ++- src/v1/auto_aim_system_v1.cpp | 2 +- src/v1/fire_controller/fire_controller.cpp | 11 +-- src/v1/fire_controller/fire_controller.hpp | 3 +- src/v1/predictor/car/car_predictor.cpp | 18 +++-- src/v1/predictor/car/car_predictor.hpp | 4 +- src/v1/predictor/predictor_manager.cpp | 14 ++-- 20 files changed, 190 insertions(+), 87 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e80351..79257ee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.22) project(alliance_auto_aim VERSION 0.0.1 LANGUAGES CXX) # 编译选项 -set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD 23) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_POSITION_INDEPENDENT_CODE ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/configs/example.yaml b/configs/example.yaml index cdeb620..afb8f4f 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -27,18 +27,19 @@ second_tolerance: 100 # 远距离射击容差，degree judge_distance: 2 #距离判断阈值 #####-----fire_controller parameters-----##### -control_delay_s: 0.1 +# control_delay_s: 0.1 +control_delay_s: 0.0 #####-----aiming_solver parameters-----##### yaw_offset: 1.5 # degree pitch_offset: -0.5 # degree -bullet_speed: 26 +bullet_speed: 20 #####-----aime_point_chooser parameters-----##### comming_angle: 60 # degree leaving_angle: 20 # degree #####-----solver parameters-----##### -R_muzzle2camera: -t_muzzle2camera: +# R_muzzle2camera: +# t_muzzle2camera: diff --git a/include/data/time_stamped.hpp b/include/data/time_stamped.hpp index 4ae93dc..7e1e424 100644 --- a/include/data/time_stamped.hpp +++ b/include/data/time_stamped.hpp @@ -23,6 +23,10 @@ struct TimeStamp { inline TimeStamp operator+ (const TimeStamp& other) const noexcept {return TimeStamp{stamp_ + other.stamp_};} inline TimeStamp operator* (const double& ratio) const noexcept {return from_nanosec(stamp_.count() * ratio);} inline TimeStamp operator/ (const double& ratio) const noexcept {return from_nanosec(stamp_.count() / ratio);} + + inline operator std::chrono::steady_clock::time_point() const { + return std::chrono::steady_clock::time_point{stamp_}; +} template static inline TimeStamp from_seconds(const T time){return TimeStamp{std::chrono::seconds(static_cast(time))};} diff --git a/include/interfaces/fire_controller.hpp b/include/interfaces/fire_controller.hpp index 4c5a3e2..5d2db4f 100644 --- a/include/interfaces/fire_controller.hpp +++ b/include/interfaces/fire_controller.hpp @@ -18,7 +18,8 @@ class IFireControl { * @param time_duration 额外时间差 典型值：当前时刻到当前帧传感器传入内容的时间差 * @return const data::FireControl */ - virtual const data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) const = 0; + virtual const data::FireControl CalculateTarget( + const std::chrono::nanoseconds& time_duration) const = 0; /** * @brief 获取当前火控系统锁定的车辆ID diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index 9824f66..f8ce708 100644 --- a/include/interfaces/predictor.hpp +++ b/include/interfaces/predictor.hpp @@ -27,6 +27,8 @@ class IPredictor { virtual std::shared_ptr Predictor( const data::TimeStamp& time_stamp) const = 0; + virtual data::TimeStamp GetTimeStamp() const = 0; + virtual ~IPredictor() = default; }; } \ No newline at end of file diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index bd92ada..3b19b38 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -4,9 +4,12 @@ #include #include +#include +#include #include #include #include +#include #include #include "../v1/sync/syncer.hpp" @@ -16,6 +19,7 @@ #include "data/sync_data.hpp" #include "data/time_stamped.hpp" #include "enum/car_id.hpp" +#include "interfaces/armor_in_gimbal_control.hpp" #include "parameters/params_system_v1.hpp" #include "parameters/profile.hpp" #include "tongji/fire_controller/fire_controller.hpp" @@ -23,6 +27,7 @@ #include "tongji/solver/solver.hpp" #include "tongji/state_machine/state_machine.hpp" #include "utils/fps_counter.hpp" +#include "utils/visualization.hpp" #include "v1/identifier/identifier.hpp" namespace world_exe::tongji { @@ -32,8 +37,10 @@ class AutoAimSystem::Impl { public: explicit Impl(const bool& debug) : debug(debug) - , config_path_("/workspaces/src/alliance_ros_auto_aim/alliance_auto_aim/configs/" - "example.yaml") + , config_path_(std::filesystem::path { __FILE__ }.parent_path().parent_path().parent_path() + / "configs" + / "example." + "yaml") , fps_() { identifier_ = std::make_unique( parameters::ParamsForSystemV1::szu_model_path(), @@ -71,6 +78,7 @@ class AutoAimSystem::Impl { if (flag == enumeration::ArmorIdFlag::None) { state_machine_->SetLostState(); + std::println("no armors identified"); return; } @@ -101,6 +109,11 @@ class AutoAimSystem::Impl { const auto target_id = state_machine_->GetAllowdToFires(); const auto gimbal_yaw = R_camera2gimbal.eulerAngles(2, 1, 0)[0]; + // std::cout << R_camera2gimbal.eulerAngles(2, 1, 0) << std::endl; + + // std::cout << "gimbal_yaw: " << gimbal_yaw << std::endl; + // const auto gimbal_yaw = 0.; + time_stamp_ = pack.camera_capture_begin_time_stamp; fire_controller_->UpdateGimbalPosition(gimbal_yaw); /// 这里应该有一个线程进行稳定的输出之类的 @@ -108,7 +121,10 @@ class AutoAimSystem::Impl { core::EventBus::Publish( parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); - time_stamp_ = std::chrono::steady_clock::now(); + + core::EventBus::Publish>( + world_exe::parameters::ParamsForSystemV1::get_lastest_predictor_event, + fire_controller_->GetArmorsToView()); if (!debug) [[likely]] return; @@ -123,7 +139,7 @@ class AutoAimSystem::Impl { parameters::ParamsForSystemV1::tracker_update_event, combined); core::EventBus::Publish( parameters::ParamsForSystemV1::car_tracing_event, state_machine_->GetAllowdToFires()); - // std::cout << "here" << std::endl; + // if (armors_in_image) { // auto visualized = raw.mat.clone(); // util::visualization::draw_armor_in_image(*armors_in_image, visualized); @@ -149,8 +165,12 @@ class AutoAimSystem::Impl { data::FireControl GetControlCommand() { fire_controller_->GetAttackCarId(); - return fire_controller_->CalculateTarget( - std::chrono::duration_cast(std::chrono::steady_clock::now() - time_stamp_)); + // std::println("time_stamp_ns{},time_stamp_s{}", + // std::chrono::duration_cast(std::chrono::steady_clock::now() - + // time_stamp_), std::chrono::duration_cast(std::chrono::steady_clock::now() - + // time_stamp_)); + return fire_controller_->CalculateTarget(std::chrono::duration_cast( + std::chrono::steady_clock::now() - time_stamp_)); } private: diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index 690ce25..57ffb49 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -1,11 +1,10 @@ #pragma once +#include #include "data/armor_gimbal_control_spacing.hpp" #include "enum/car_id.hpp" #include "util/math.hpp" -#include - namespace world_exe::tongji::fire_control { using CarIDFlag = enumeration::CarIDFlag; @@ -25,7 +24,7 @@ class AimPointChooser { int chosen_id = -1; // 整车旋转中心的球坐标yaw - const auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); + const auto center_yaw = std::atan2(ekf_x[2], ekf_x[0]); // rad std::vector> delta_angle_list; for (int i = 0; i < armor_num; i++) { @@ -33,15 +32,18 @@ class AimPointChooser { util::math::get_yaw_from_quaternion(armors[i].orientation) - center_yaw); delta_angle_list.emplace_back(i, delta_angle); } - std::sort(delta_angle_list.begin(), delta_angle_list.end(), - [](const auto& a, const auto& b) { return std::get<1>(a) > std::get<1>(b); }); + std::sort( + delta_angle_list.begin(), delta_angle_list.end(), [](const auto& a, const auto& b) { + return std::abs(std::get<1>(a)) < std::abs(std::get<1>(b)); + }); // 不考虑小陀螺 if (std::abs(ekf_x[8]) <= 2 && single_id != CarIDFlag::Outpost) { // 选择在可射击范围内的装甲板 std::vector id_list; for (const auto& [id, delta_angle] : delta_angle_list) { - if (std::abs(delta_angle) > 60 / 57.3) continue; + // std::println("id:{},delta_angle: {}", id, delta_angle / std::numbers::pi * 180); + if (std::abs(delta_angle) > (60 / 57.3)) continue; id_list.emplace_back(id); } @@ -58,11 +60,10 @@ class AimPointChooser { } } 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_; + 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 (const auto& [id, delta_angle] : delta_angle_list) { if (std::abs(delta_angle) > coming_angle) continue; if ((ekf_x[7] > 0 && delta_angle < leaving_angle) @@ -76,7 +77,6 @@ class AimPointChooser { if (chosen_id == -1) { return { false, armors[std::get<0>(delta_angle_list.front())] }; } - return { true, armors[chosen_id], diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index 9c17ea6..bdd8b1f 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -1,15 +1,21 @@ #pragma once +#include #include +#include #include #include #include +#include +#include #include #include #include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.hpp" +#include "interfaces/armor_in_gimbal_control.hpp" +#include "tongji/predictor/in_gimbal_control_armor.hpp" #include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" #include "tongji/predictor/kalman_filter/predict_model.hpp" #include "trajectory.hpp" @@ -40,7 +46,7 @@ class AimingSolver { } AimSolution SolveAimSolution(std::shared_ptr const& snapshot, - data::TimeStamp const& time_stamp, const double& control_delay_s) { + std::chrono::milliseconds control_delay) { // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) double prev_fly_time_s; @@ -49,24 +55,35 @@ class AimingSolver { bool converged = false; // HACK:不同击打点影响飞行时间的迭代，需要根据整车的状态（转速和坐标）来选择击打点，不得已将指针转换为派生类 auto snapshot_derived = std::dynamic_pointer_cast(snapshot); + if (!snapshot_derived) + throw std::runtime_error("Failed to cast snapshot to CarPredictor. Unexpected object " + "type."); // 预测目标在未来 dt时间后的位置 for (int i = 0; i < 10; ++i) { - const auto& dt = control_delay_s + prev_fly_time_s; + const auto& dt = prev_fly_time_s + (double)(control_delay).count() / 1000.; const auto& armors = - snapshot->Predictor(time_stamp + data::TimeStamp::from_seconds(dt)); - - const auto& aim_point = SelectPredictedAim(snapshot_derived->GetPredictedX(dt), - armors->GetArmors(snapshot->GetId()), snapshot->GetId()); - if (!aim_point.has_value()) - return { false, std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), { }, - 0 }; // failed: no valid aim point + snapshot->Predictor(snapshot->GetTimeStamp() + data::TimeStamp::from_seconds(dt)); + + const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); + + armors_to_view_ = std::make_shared( + armors_to_view, snapshot->GetTimeStamp() + data::TimeStamp::from_seconds(dt)); + + const auto& aim_point = SelectPredictedAim( + snapshot_derived->GetPredictedX(dt), armors_to_view, snapshot->GetId()); + + if (!aim_point.has_value()) { + continue; + std::println("no valid aim point"); + + } // failed: no valid aim point + const auto traj = SolveTrajectory(aim_point.value(), bullet_speed_); - if (!traj.has_value()) - return { false, std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), { }, - 0 }; // failed: trajectory unsolvable + if (!traj.has_value()) { + continue; + std::println("trajectory unsolvable"); + } if (i > 0 && std::abs(traj->fly_time - prev_fly_time_s) < 0.001) { final_aim_point = *aim_point; @@ -76,25 +93,38 @@ class AimingSolver { } prev_fly_time_s = traj->fly_time; } - if (!converged) + if (!converged) { return { false, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), { }, 0 }; // failed: trajectory did not converge + } else { + std::println("trajectory converged"); + } + + const auto xyz = final_aim_point; + const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; + // const double pitch = -(final_trajectory.pitch + pitch_offset_); + const double pitch = (final_trajectory.pitch + pitch_offset_); - const auto xyz = final_aim_point; - 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 }; } + auto GetArmorsToView() -> std::shared_ptr { + return armors_to_view_; + } + private: std::optional SelectPredictedAim(const EKF::XVec& ekf_x, const std::vector& armors, const CarIDFlag& id) const { + // TODO:maybe there is a bug! const auto& [selectable, aim_point_in_gimbal] = aim_point_chooser_->ChooseAimArmor(ekf_x, armors, id); + // std::println("ekf_x:({},{},{})", ekf_x(0), ekf_x(2), ekf_x(4)); if (!selectable) return std::nullopt; + // std::println("aim_point_in_gimbal:({},{},{})", aim_point_in_gimbal.position.x(), + // aim_point_in_gimbal.position.y(), aim_point_in_gimbal.position.z()); return aim_point_in_gimbal.position; } @@ -102,12 +132,18 @@ class AimingSolver { 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_); + + if (!result.solvable) { + std::println("solve trajectory failed: d={}, z={},speed={}", d, xyz.z(), bullet_speed); + } + return result.solvable ? std::optional { result } : std::nullopt; } double yaw_offset_, pitch_offset_; double bullet_speed_; const double g_; + std::shared_ptr armors_to_view_; std::unique_ptr aim_point_chooser_; }; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 9ee8e71..7ade29d 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -33,38 +34,54 @@ class FireController::Impl { , state_machine_(std::move(state_machine)) , live_target_manager_(std::move(live_target_manager)) { - auto yaml = YAML::LoadFile(config_path); - control_delay_s_ = yaml["control_delay_s"].as(); + auto yaml = YAML::LoadFile(config_path); + control_delay_ = std::chrono::duration_cast( + std::chrono::duration(yaml["control_delay_s"].as())); } data ::FireControl CalculateTarget( - const std::chrono::seconds& time_from_tracker_timepoint) const { - + const std::chrono::nanoseconds& time_from_tracker_timepoint) const { + // std::println("time_point: {}", time_from_tracker_timepoint.count()); if (!fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; const auto& lockable_target = state_machine_->GetAllowdToFires(); const auto& snapshot_manager = live_target_manager_->GetPredictor(lockable_target); + if (!snapshot_manager) + // TODO:这里的时间戳不太对吧 return data::FireControl { .time_stamp = data::TimeStamp { time_from_tracker_timepoint }, .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; - // const auto& armors_in_gimbal = snapshot_manager->Predictor(time_from_tracker_timepoint); // TODO:这里不应该指针转换 - const auto& aim_solution = aiming_solver_->SolveAimSolution( - snapshot_manager, time_from_tracker_timepoint, control_delay_s_); + const auto& aim_solution = + aiming_solver_->SolveAimSolution(snapshot_manager, control_delay_); + armors_to_view_ = aiming_solver_->GetArmorsToView(); + if (!aim_solution.valid) { + std::println("aim solution invalid ,solver failed"); + return data::FireControl { .time_stamp = + data::TimeStamp { snapshot_manager->GetTimeStamp() + + time_from_tracker_timepoint }, + .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), + .fire_allowance = false }; + } const auto gimbal_command = GimbalCommand { aim_solution.yaw, aim_solution.pitch }; const auto target_pos = Eigen::Vector3d { aim_solution.aim_point }; - auto fire_command = aim_solution.valid + + auto fire_command = aim_solution.valid ? fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_pos) : false; - firable_ = fire_command; + firable_ = fire_command; data::FireControl result; result.fire_allowance = fire_command; - result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; + // result.fire_allowance = true; + + // result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; + result.gimbal_dir << cos(gimbal_command.yaw) * cos(gimbal_command.pitch), + sin(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.pitch); result.time_stamp = data::TimeStamp { time_from_tracker_timepoint }; return result; } @@ -76,8 +93,12 @@ class FireController::Impl { void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; + auto GetArmorsToView() -> std::shared_ptr { + return armors_to_view_; + } + private: - double control_delay_s_; + std::chrono::milliseconds control_delay_; double gimbal_yaw_; @@ -88,6 +109,8 @@ class FireController::Impl { std::unique_ptr fire_decision_; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; + + mutable std::shared_ptr armors_to_view_; }; FireController::FireController(const std::string& config_path, @@ -97,7 +120,7 @@ FireController::FireController(const std::string& config_path, FireController::~FireController() = default; const data ::FireControl FireController::CalculateTarget( - const std::chrono::seconds& time_duration) const { + const std::chrono::nanoseconds& time_duration) const { return pimpl_->CalculateTarget(time_duration); } const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } @@ -106,4 +129,8 @@ void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { return pimpl_->UpdateGimbalPosition(gimbal_yaw); }; +auto FireController::GetArmorsToView() -> std::shared_ptr { + return pimpl_->GetArmorsToView(); +} + } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index a2afa05..b0399d1 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -17,11 +17,13 @@ class FireController final : public interfaces::IFireControl { ~FireController(); const data ::FireControl CalculateTarget( - const std::chrono::seconds& time_duration) const override; + const std::chrono::nanoseconds& time_duration) const override; const enumeration ::CarIDFlag GetAttackCarId() const override; void UpdateGimbalPosition(const double& gimbal_yaw); + auto GetArmorsToView() -> std::shared_ptr; + FireController(const FireController&) = delete; FireController& operator=(const FireController&) = delete; FireController(FireController&&) noexcept = default; diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp index 3f4b144..a71eba9 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -57,6 +57,7 @@ class CarPredictor final : public interfaces::IPredictor { std ::shared_ptr Predictor( const data ::TimeStamp& time_stamp) const override { const auto ekf_x = this->GetPredictedX((time_stamp - time_stamp_).to_seconds()); + std::vector armors; for (int id = 0; id < model_.GetArmorNum(); id++) { auto angle = util::math::clamp_pm_pi(ekf_x[6] + id * 2 * CV_PI / model_.GetArmorNum()); @@ -75,7 +76,7 @@ class CarPredictor final : public interfaces::IPredictor { auto GetModel() const -> PredictorModel { return model_; } auto GetEkf() const -> EKF { return ekf_.value(); } - data::TimeStamp LastSeen() const { return time_stamp_; } + data::TimeStamp GetTimeStamp() const override { return time_stamp_; } auto GetPredictedXYZAList(const double& dt) -> std::vector { const auto [x_n, P_n] = ekf_->PredictOnce(dt); @@ -89,7 +90,6 @@ class CarPredictor final : public interfaces::IPredictor { void Update(data::TimeStamp const& time_stamp, const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) { - // 装甲板匹配 int id = model_.MatchArmor( ekf_->x, armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal); diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp index 1a65a6e..2bd262d 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -53,7 +53,7 @@ class CarPredictorManager::Impl { if (it == targets_map_.end()) return nullptr; const auto& [id, predictor] = *it; return std::make_shared( - predictor->GetEkf(), predictor->GetModel(), predictor->LastSeen()); + predictor->GetEkf(), predictor->GetModel(), predictor->GetTimeStamp()); } void Update(std::shared_ptr const& data) { diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp index 8124a8b..4416084 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -34,9 +34,9 @@ class EKFModel { explicit EKFModel(const enumeration::CarIDFlag& car_id) : car_id_(car_id) { - bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII - || car_id == enumeration::CarIDFlag::InfantryIV - || car_id == enumeration::CarIDFlag::InfantryV); + // bool is_balance = (car_id == enumeration::CarIDFlag::InfantryIII + // || car_id == enumeration::CarIDFlag::InfantryIV + // || car_id == enumeration::CarIDFlag::InfantryV); P0_dig_.resize(11); if (car_id == enumeration::CarIDFlag::Outpost) { @@ -55,10 +55,7 @@ class EKFModel { radius_ = 0.2; } - if (is_balance) { - armor_num_ = 2; - } else if (car_id == enumeration::CarIDFlag::Outpost - | car_id == enumeration::CarIDFlag::Base) { + if (car_id == enumeration::CarIDFlag::Outpost | car_id == enumeration::CarIDFlag::Base) { armor_num_ = 3; } else { armor_num_ = 4; diff --git a/src/util/parameters/params_system_v1.cpp b/src/util/parameters/params_system_v1.cpp index f86d58d..e9b52fa 100644 --- a/src/util/parameters/params_system_v1.cpp +++ b/src/util/parameters/params_system_v1.cpp @@ -1,14 +1,17 @@ #include "parameters/params_system_v1.hpp" +#include #include #include namespace world_exe::parameters { struct ParamsForSystemV1::Impl { public: - std::string model_path = "/workspaces/src/alliance_ros_auto_aim/alliance_auto_aim/models/" - "szu_identify_model.onnx"; - std::string device = "AUTO"; + std::string model_path = + std::filesystem::path { __FILE__ }.parent_path().parent_path().parent_path().parent_path() + / "models" / "szu_identify_model.onnx"; + + std::string device = "AUTO"; double control_delay_in_second = 0.05; double velocity_begin = 26; double gravity = 9.81; diff --git a/src/v1/auto_aim_system_v1.cpp b/src/v1/auto_aim_system_v1.cpp index bdb36be..99c2793 100644 --- a/src/v1/auto_aim_system_v1.cpp +++ b/src/v1/auto_aim_system_v1.cpp @@ -35,7 +35,7 @@ using namespace std::chrono; class world_exe::v1::SystemV1::Impl{ public: - Impl(const bool& debug) : debug(debug) { + explicit Impl(const bool& debug) : debug(debug) { time_point_ = std::chrono::steady_clock::now(); predictor = std::make_shared(); sync = std::make_shared(seconds(2),6e-6); diff --git a/src/v1/fire_controller/fire_controller.cpp b/src/v1/fire_controller/fire_controller.cpp index de57210..ca056e3 100644 --- a/src/v1/fire_controller/fire_controller.cpp +++ b/src/v1/fire_controller/fire_controller.cpp @@ -46,8 +46,9 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { predictor_ = predictor; }; - const world_exe::data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) { - std::chrono::seconds fly_time{0}; + const world_exe::data::FireControl CalculateTarget( + const std::chrono::nanoseconds& time_duration) { + std::chrono::seconds fly_time { 0 }; const auto& pre1 = predictor_->Predictor(fly_time + time_duration + control_delay_); const auto& pre2 = pre1->GetArmors(predictor_->GetId()); double min_angular_dis = 1e9; @@ -73,7 +74,9 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { trajectory_solver::gravity_only(armors[index].position, velocity_begin_, gravity_); } - return { .time_stamp = data::TimeStamp{std::chrono::nanoseconds(fly_time + time_duration + control_delay_)}, .fire_allowance = true }; + return { .time_stamp = data::TimeStamp { std::chrono::nanoseconds( + fly_time + time_duration + control_delay_) }, + .fire_allowance = true }; } private: @@ -89,7 +92,7 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { const world_exe::data::FireControl // world_exe::v1::fire_control::TracingFireControl::CalculateTarget( - const std::chrono::seconds& time_duration) const { + const std::chrono::nanoseconds& time_duration) const { return pimpl_->CalculateTarget(time_duration); } diff --git a/src/v1/fire_controller/fire_controller.hpp b/src/v1/fire_controller/fire_controller.hpp index 171c0e6..0f982f9 100644 --- a/src/v1/fire_controller/fire_controller.hpp +++ b/src/v1/fire_controller/fire_controller.hpp @@ -8,7 +8,8 @@ namespace world_exe::v1::fire_control { class TracingFireControl final : public interfaces::IFireControl { public: - const data::FireControl CalculateTarget(const std::chrono::seconds& time_duration) const override; + const data::FireControl CalculateTarget( + const std::chrono::nanoseconds& time_duration) const override; const enumeration::CarIDFlag GetAttackCarId() const override; void set_armor(const std::shared_ptr& armors); void SetPredictor(const std::shared_ptr& predictor); diff --git a/src/v1/predictor/car/car_predictor.cpp b/src/v1/predictor/car/car_predictor.cpp index 43c674f..f135ce4 100644 --- a/src/v1/predictor/car/car_predictor.cpp +++ b/src/v1/predictor/car/car_predictor.cpp @@ -15,10 +15,11 @@ class CarPredictor::Impl { const enumeration::ArmorIdFlag& GetId() const { return id_; } - std::shared_ptr Predictor(const data::TimeStamp& time_stamp) { + std::shared_ptr Predictor( + const data::TimeStamp& time_stamp) { auto ptr = std::make_shared(); - ptr->SetWithSingleId(ekf_.get_predict_output_armor( - id_, (time_stamp - create_time_stamp_).to_seconds()), + ptr->SetWithSingleId( + ekf_.get_predict_output_armor(id_, (time_stamp - create_time_stamp_).to_seconds()), time_stamp); return ptr; } @@ -27,9 +28,9 @@ class CarPredictor::Impl { inline void SetEkf(const CarPredictEkf& ekf) { ekf_ = ekf; } - inline void SetTimeStamp(const data::TimeStamp& time_stamp) { - create_time_stamp_ = time_stamp; - } + inline void SetTimeStamp(const data::TimeStamp& time_stamp) { create_time_stamp_ = time_stamp; } + + data ::TimeStamp GetTimeStamp() const { return create_time_stamp_; } private: enumeration::CarIDFlag id_; @@ -39,7 +40,7 @@ class CarPredictor::Impl { CarPredictor::CarPredictor() : pimpl_(std::make_unique( - enumeration::CarIDFlag::None, CarPredictEkf {}, data::TimeStamp {})) { } + enumeration::CarIDFlag::None, CarPredictEkf { }, data::TimeStamp { })) { } CarPredictor::CarPredictor(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, const data::TimeStamp& create_time_stamp) @@ -61,4 +62,7 @@ void CarPredictor::SetEkf(const CarPredictEkf& ekf) { return pimpl_->SetEkf(ekf) void CarPredictor::SetTimeStamp(const data::TimeStamp& time_stamp) { return pimpl_->SetTimeStamp(time_stamp); }; + +data ::TimeStamp CarPredictor::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } + } \ No newline at end of file diff --git a/src/v1/predictor/car/car_predictor.hpp b/src/v1/predictor/car/car_predictor.hpp index 9289164..e3e542d 100644 --- a/src/v1/predictor/car/car_predictor.hpp +++ b/src/v1/predictor/car/car_predictor.hpp @@ -6,8 +6,10 @@ #include "interfaces/predictor.hpp" namespace world_exe::v1::predictor { -class CarPredictor final: public interfaces::IPredictor { +class CarPredictor final : public interfaces::IPredictor { public: + data ::TimeStamp GetTimeStamp() const override; + CarPredictor(); ~CarPredictor(); CarPredictor(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, diff --git a/src/v1/predictor/predictor_manager.cpp b/src/v1/predictor/predictor_manager.cpp index f4f2b65..a0614b5 100644 --- a/src/v1/predictor/predictor_manager.cpp +++ b/src/v1/predictor/predictor_manager.cpp @@ -31,7 +31,7 @@ class PredictorManager::Impl { std::atan(tmp_armor.position.y() / tmp_armor.position.x()), -std::atan(tmp_armor.position.z() / tmp_armor.position.x()), tmp_armor.position.norm(); - predictors_[i].Update(input, {}, dt.to_seconds()); + predictors_[i].Update(input, { }, dt.to_seconds()); } else if (armors.size() == 2) { // 当同时识别到两块装甲板时，优先更新近的那块，再更新远的 const auto armor0_yaw = util::math::get_yaw_from_quaternion(armors[0].orientation); @@ -47,27 +47,27 @@ class PredictorManager::Impl { std::atan(tmp_armor0.position.y() / tmp_armor0.position.x()), -std::atan(tmp_armor0.position.z() / tmp_armor0.position.x()), tmp_armor0.position.norm(); - predictors_[i].Update(input, {}, dt.to_seconds()); + predictors_[i].Update(input, { }, dt.to_seconds()); // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor1.orientation), std::atan(tmp_armor1.position.y() / tmp_armor1.position.x()), -std::atan(tmp_armor1.position.z() / tmp_armor1.position.x()), tmp_armor1.position.norm(); - predictors_[i].Update(input, {}, 0.); + predictors_[i].Update(input, { }, 0.); } else { input << util::math::get_yaw_from_quaternion(tmp_armor1.orientation), std::atan(tmp_armor1.position.y() / tmp_armor1.position.x()), -std::atan(tmp_armor1.position.z() / tmp_armor1.position.x()), tmp_armor1.position.norm(); - predictors_[i].Update(input, {}, dt.to_seconds()); + predictors_[i].Update(input, { }, dt.to_seconds()); // 同时识别到一辆车的两块装甲板时要调这个函数 predictors_[i].set_second_armor(); input << util::math::get_yaw_from_quaternion(tmp_armor0.orientation), std::atan(tmp_armor0.position.y() / tmp_armor0.position.x()), -std::atan(tmp_armor0.position.z() / tmp_armor0.position.x()), tmp_armor0.position.norm(); - predictors_[i].Update(input, {}, 0.); + predictors_[i].Update(input, { }, 0.); } } } @@ -99,7 +99,7 @@ class PredictorManager::Impl { } private: - data::TimeStamp last_update_time_stamp_ {}; + data::TimeStamp last_update_time_stamp_ { }; std::array predictors_; Eigen::Affine3d transform_from_camera_to_gimbal_; @@ -116,7 +116,7 @@ std::shared_ptr PredictorManager::Predict( return pimpl_->Predict(id, time_stamp); }; -void PredictorManager::Update(std::shared_ptr data) { +void PredictorManager::Update(std::shared_ptrdata) { return pimpl_->Update(data); }; From d7e728b0b7fd8dcfa8eefd91cf35d5dd32302033 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 13 Nov 2025 03:14:13 +0800 Subject: [PATCH 03/14] chore: clean up debugging code and temporary artifacts --- src/tongji/auto_aim_system.cpp | 8 +---- .../fire_controller/aim_point_chooser.hpp | 1 - src/tongji/fire_controller/aim_solver.hpp | 33 +++++++------------ .../fire_controller/fire_controller.cpp | 17 +++++----- 4 files changed, 21 insertions(+), 38 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 3b19b38..e6ee299 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -108,11 +108,9 @@ class AutoAimSystem::Impl { const auto target_id = state_machine_->GetAllowdToFires(); + // TODO:读编码器还是旋转矩阵？ const auto gimbal_yaw = R_camera2gimbal.eulerAngles(2, 1, 0)[0]; - // std::cout << R_camera2gimbal.eulerAngles(2, 1, 0) << std::endl; - // std::cout << "gimbal_yaw: " << gimbal_yaw << std::endl; - // const auto gimbal_yaw = 0.; time_stamp_ = pack.camera_capture_begin_time_stamp; fire_controller_->UpdateGimbalPosition(gimbal_yaw); @@ -165,10 +163,6 @@ class AutoAimSystem::Impl { data::FireControl GetControlCommand() { fire_controller_->GetAttackCarId(); - // std::println("time_stamp_ns{},time_stamp_s{}", - // std::chrono::duration_cast(std::chrono::steady_clock::now() - - // time_stamp_), std::chrono::duration_cast(std::chrono::steady_clock::now() - - // time_stamp_)); return fire_controller_->CalculateTarget(std::chrono::duration_cast( std::chrono::steady_clock::now() - time_stamp_)); } diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index 57ffb49..63e04f5 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -42,7 +42,6 @@ class AimPointChooser { // 选择在可射击范围内的装甲板 std::vector id_list; for (const auto& [id, delta_angle] : delta_angle_list) { - // std::println("id:{},delta_angle: {}", id, delta_angle / std::numbers::pi * 180); if (std::abs(delta_angle) > (60 / 57.3)) continue; id_list.emplace_back(id); } diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index bdd8b1f..ea5a5a5 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -14,8 +14,6 @@ #include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.hpp" -#include "interfaces/armor_in_gimbal_control.hpp" -#include "tongji/predictor/in_gimbal_control_armor.hpp" #include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" #include "tongji/predictor/kalman_filter/predict_model.hpp" #include "trajectory.hpp" @@ -26,7 +24,7 @@ struct AimSolution { bool valid; double yaw; double pitch; - Eigen::Vector3d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） + Eigen::Vector3d aim_point; double horizon_distance = 0; // 无人机专有 }; @@ -65,13 +63,12 @@ class AimingSolver { const auto& armors = snapshot->Predictor(snapshot->GetTimeStamp() + data::TimeStamp::from_seconds(dt)); - const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); + // const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); + // armors_to_view_ = std::make_shared( + // armors_to_view, snapshot->GetTimeStamp() + data::TimeStamp::from_seconds(dt)); - armors_to_view_ = std::make_shared( - armors_to_view, snapshot->GetTimeStamp() + data::TimeStamp::from_seconds(dt)); - - const auto& aim_point = SelectPredictedAim( - snapshot_derived->GetPredictedX(dt), armors_to_view, snapshot->GetId()); + const auto& aim_point = SelectPredictedAim(snapshot_derived->GetPredictedX(dt), + armors->GetArmors(snapshot->GetId()), snapshot->GetId()); if (!aim_point.has_value()) { continue; @@ -101,30 +98,24 @@ class AimingSolver { std::println("trajectory converged"); } - const auto xyz = final_aim_point; - const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; - // const double pitch = -(final_trajectory.pitch + pitch_offset_); + const auto xyz = final_aim_point; + 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 }; } - auto GetArmorsToView() -> std::shared_ptr { - return armors_to_view_; - } + // auto GetArmorsToView() -> std::shared_ptr { + // return armors_to_view_; + // } private: std::optional SelectPredictedAim(const EKF::XVec& ekf_x, const std::vector& armors, const CarIDFlag& id) const { - - // TODO:maybe there is a bug! const auto& [selectable, aim_point_in_gimbal] = aim_point_chooser_->ChooseAimArmor(ekf_x, armors, id); - // std::println("ekf_x:({},{},{})", ekf_x(0), ekf_x(2), ekf_x(4)); if (!selectable) return std::nullopt; - // std::println("aim_point_in_gimbal:({},{},{})", aim_point_in_gimbal.position.x(), - // aim_point_in_gimbal.position.y(), aim_point_in_gimbal.position.z()); return aim_point_in_gimbal.position; } @@ -143,7 +134,7 @@ class AimingSolver { double yaw_offset_, pitch_offset_; double bullet_speed_; const double g_; - std::shared_ptr armors_to_view_; + // std::shared_ptr armors_to_view_; std::unique_ptr aim_point_chooser_; }; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 7ade29d..7f84941 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -41,7 +41,6 @@ class FireController::Impl { data ::FireControl CalculateTarget( const std::chrono::nanoseconds& time_from_tracker_timepoint) const { - // std::println("time_point: {}", time_from_tracker_timepoint.count()); if (!fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; @@ -57,7 +56,7 @@ class FireController::Impl { // TODO:这里不应该指针转换 const auto& aim_solution = aiming_solver_->SolveAimSolution(snapshot_manager, control_delay_); - armors_to_view_ = aiming_solver_->GetArmorsToView(); + // armors_to_view_ = aiming_solver_->GetArmorsToView(); if (!aim_solution.valid) { std::println("aim solution invalid ,solver failed"); @@ -93,9 +92,9 @@ class FireController::Impl { void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; - auto GetArmorsToView() -> std::shared_ptr { - return armors_to_view_; - } + // auto GetArmorsToView() -> std::shared_ptr { + // return armors_to_view_; + // } private: std::chrono::milliseconds control_delay_; @@ -110,7 +109,7 @@ class FireController::Impl { std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; - mutable std::shared_ptr armors_to_view_; + // mutable std::shared_ptr armors_to_view_; }; FireController::FireController(const std::string& config_path, @@ -129,8 +128,8 @@ void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { return pimpl_->UpdateGimbalPosition(gimbal_yaw); }; -auto FireController::GetArmorsToView() -> std::shared_ptr { - return pimpl_->GetArmorsToView(); -} +// auto FireController::GetArmorsToView() -> std::shared_ptr { +// return pimpl_->GetArmorsToView(); +// } } From 33c084b83910dc5f730c278bfd315d0e5e175914 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 13 Nov 2025 23:55:39 +0800 Subject: [PATCH 04/14] refactor(fire_control): update flight time iteration interface and dependent logic --- configs/example.yaml | 4 -- include/interfaces/fire_controller.hpp | 10 ++-- include/interfaces/predictor.hpp | 2 +- src/tongji/auto_aim_system.cpp | 17 +++--- src/tongji/fire_controller/aim_solver.hpp | 37 +++++++------ .../fire_controller/fire_controller.cpp | 53 ++++++++----------- .../fire_controller/fire_controller.hpp | 6 +-- src/tongji/identifier/tracker.hpp | 22 +++++--- .../predictor/car_predictor/car_predictor.hpp | 4 +- .../car_predictor/car_predictor_manager.cpp | 2 +- src/tongji/state_machine/state_machine.cpp | 14 +++-- src/tongji/state_machine/state_machine.hpp | 3 +- src/v1/fire_controller/fire_controller.cpp | 21 ++++---- src/v1/fire_controller/fire_controller.hpp | 7 ++- src/v1/predictor/car/car_predictor.cpp | 2 - src/v1/predictor/car/car_predictor.hpp | 2 - 16 files changed, 92 insertions(+), 114 deletions(-) diff --git a/configs/example.yaml b/configs/example.yaml index afb8f4f..3683855 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -26,10 +26,6 @@ first_tolerance: 100 # 近距离射击容差，degree second_tolerance: 100 # 远距离射击容差，degree judge_distance: 2 #距离判断阈值 -#####-----fire_controller parameters-----##### -# control_delay_s: 0.1 -control_delay_s: 0.0 - #####-----aiming_solver parameters-----##### yaw_offset: 1.5 # degree pitch_offset: -0.5 # degree diff --git a/include/interfaces/fire_controller.hpp b/include/interfaces/fire_controller.hpp index 5d2db4f..c8ca9ef 100644 --- a/include/interfaces/fire_controller.hpp +++ b/include/interfaces/fire_controller.hpp @@ -2,7 +2,6 @@ #include "data/fire_control.hpp" #include "enum/car_id.hpp" -#include #include namespace world_exe::interfaces { @@ -16,17 +15,16 @@ class IFireControl { * @brief 计算云台和发射系统控制量 * * @param time_duration 额外时间差 典型值：当前时刻到当前帧传感器传入内容的时间差 - * @return const data::FireControl + * @return data::FireControl */ - virtual const data::FireControl CalculateTarget( - const std::chrono::nanoseconds& time_duration) const = 0; + virtual data::FireControl CalculateTarget(data::TimeStamp const& time_stamp) const = 0; /** * @brief 获取当前火控系统锁定的车辆ID * - * @return const enumeration::CarIDFlag : 火控系统锁定的车辆ID + * @return enumeration::CarIDFlag : 火控系统锁定的车辆ID */ - virtual const enumeration::CarIDFlag GetAttackCarId() const = 0; + virtual enumeration::CarIDFlag GetAttackCarId() const = 0; virtual ~IFireControl() = default; }; diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index f8ce708..2133e8d 100644 --- a/include/interfaces/predictor.hpp +++ b/include/interfaces/predictor.hpp @@ -27,7 +27,7 @@ class IPredictor { virtual std::shared_ptr Predictor( const data::TimeStamp& time_stamp) const = 0; - virtual data::TimeStamp GetTimeStamp() const = 0; + // virtual data::TimeStamp GetTimeStamp() const = 0; virtual ~IPredictor() = default; }; diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index e6ee299..b21d418 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include #include #include @@ -27,7 +26,7 @@ #include "tongji/solver/solver.hpp" #include "tongji/state_machine/state_machine.hpp" #include "utils/fps_counter.hpp" -#include "utils/visualization.hpp" +// #include "utils/visualization.hpp" #include "v1/identifier/identifier.hpp" namespace world_exe::tongji { @@ -53,8 +52,7 @@ class AutoAimSystem::Impl { state_machine_ = std::make_shared(); fire_controller_ = std::make_unique( config_path_, state_machine_, live_target_manager_); - time_stamp_ = std::chrono::steady_clock::now(); - syncer_ = std::make_unique(seconds(2), 6e-6); + syncer_ = std::make_unique(seconds(2), 6e-6); core::EventBus::Subscript( parameters::ParamsForSystemV1::raw_image_event, @@ -83,9 +81,7 @@ class AutoAimSystem::Impl { } // TODO:update invincible_armors - state_machine_->Update(armors_in_image, enumeration::CarIDFlag::None, - std::chrono::duration_cast( - std::chrono::steady_clock::now() - time_stamp_)); + state_machine_->Update(armors_in_image, enumeration::CarIDFlag::None, time_stamp_); // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 auto [pack, check] = syncer_->get_data(raw.stamp); @@ -161,17 +157,18 @@ class AutoAimSystem::Impl { void SetTransfroms(const data::CameraGimbalMuzzleSyncData& data) { syncer_->set_data(data); } + // TODO:时间戳有待fix data::FireControl GetControlCommand() { fire_controller_->GetAttackCarId(); - return fire_controller_->CalculateTarget(std::chrono::duration_cast( - std::chrono::steady_clock::now() - time_stamp_)); + return fire_controller_->CalculateTarget( + data::TimeStamp(steady_clock::now().time_since_epoch())); } private: bool debug; const std::string config_path_; world_exe::util::FpsCounter fps_; - std::chrono::steady_clock::time_point time_stamp_; + data::TimeStamp time_stamp_; std::unique_ptr identifier_; std::unique_ptr pnp_solver_; std::shared_ptr state_machine_; diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index ea5a5a5..012fd11 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -14,6 +14,7 @@ #include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.hpp" +#include "data/time_stamped.hpp" #include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" #include "tongji/predictor/kalman_filter/predict_model.hpp" #include "trajectory.hpp" @@ -44,10 +45,11 @@ class AimingSolver { } AimSolution SolveAimSolution(std::shared_ptr const& snapshot, - std::chrono::milliseconds control_delay) { + data::TimeStamp const& time_stamp, std::chrono::milliseconds control_delay) { - // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) - double prev_fly_time_s; + // time_stamp.to_seconds(),control_delay); 迭代求解飞行时间 + // (最多10次，收敛条件：相邻两次fly_time差 <0.001) + double prev_fly_time_s = 0; Eigen::Vector3d final_aim_point; TrajectoryResult final_trajectory; bool converged = false; @@ -61,25 +63,25 @@ class AimingSolver { for (int i = 0; i < 10; ++i) { const auto& dt = prev_fly_time_s + (double)(control_delay).count() / 1000.; const auto& armors = - snapshot->Predictor(snapshot->GetTimeStamp() + data::TimeStamp::from_seconds(dt)); + snapshot->Predictor(time_stamp + data::TimeStamp::from_seconds(dt)); - // const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); - // armors_to_view_ = std::make_shared( - // armors_to_view, snapshot->GetTimeStamp() + data::TimeStamp::from_seconds(dt)); + const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); + armors_to_view_ = std::make_shared( + armors_to_view, time_stamp + data::TimeStamp::from_seconds(dt)); - const auto& aim_point = SelectPredictedAim(snapshot_derived->GetPredictedX(dt), - armors->GetArmors(snapshot->GetId()), snapshot->GetId()); + const auto& aim_point = SelectPredictedAim( + snapshot_derived->GetPredictedX(dt), armors_to_view, snapshot->GetId()); if (!aim_point.has_value()) { - continue; std::println("no valid aim point"); + continue; } // failed: no valid aim point const auto traj = SolveTrajectory(aim_point.value(), bullet_speed_); if (!traj.has_value()) { - continue; std::println("trajectory unsolvable"); + continue; } if (i > 0 && std::abs(traj->fly_time - prev_fly_time_s) < 0.001) { @@ -90,12 +92,13 @@ class AimingSolver { } prev_fly_time_s = traj->fly_time; } + if (!converged) { + std::println("trajectory diverse"); + return { false, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), { }, 0 }; // failed: trajectory did not converge - } else { - std::println("trajectory converged"); } const auto xyz = final_aim_point; @@ -105,9 +108,9 @@ class AimingSolver { return { true, yaw, pitch, final_aim_point }; } - // auto GetArmorsToView() -> std::shared_ptr { - // return armors_to_view_; - // } + auto GetArmorsToView() -> std::shared_ptr { + return armors_to_view_; + } private: std::optional SelectPredictedAim(const EKF::XVec& ekf_x, @@ -134,7 +137,7 @@ class AimingSolver { double yaw_offset_, pitch_offset_; double bullet_speed_; const double g_; - // std::shared_ptr armors_to_view_; + std::shared_ptr armors_to_view_; std::unique_ptr aim_point_chooser_; }; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 7f84941..6638259 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -32,37 +32,29 @@ class FireController::Impl { , aiming_solver_(std::make_unique(config_path)) , fire_decision_(std::make_unique(config_path)) , state_machine_(std::move(state_machine)) - , live_target_manager_(std::move(live_target_manager)) { + , live_target_manager_(std::move(live_target_manager)) + , control_delay_(100) { } - auto yaml = YAML::LoadFile(config_path); - control_delay_ = std::chrono::duration_cast( - std::chrono::duration(yaml["control_delay_s"].as())); - } - - data ::FireControl CalculateTarget( - const std::chrono::nanoseconds& time_from_tracker_timepoint) const { + data ::FireControl CalculateTarget(data::TimeStamp const& time_stamp) const { if (!fire_decision_ || !state_machine_ || !live_target_manager_) return { .fire_allowance = false }; - const auto& lockable_target = state_machine_->GetAllowdToFires(); + const auto& lockable_target = state_machine_->GetAllowdToFires(); + const auto& snapshot_manager = live_target_manager_->GetPredictor(lockable_target); if (!snapshot_manager) - // TODO:这里的时间戳不太对吧 - return data::FireControl { .time_stamp = - data::TimeStamp { time_from_tracker_timepoint }, + return data::FireControl { .time_stamp = data::TimeStamp { time_stamp }, .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; // TODO:这里不应该指针转换 const auto& aim_solution = - aiming_solver_->SolveAimSolution(snapshot_manager, control_delay_); - // armors_to_view_ = aiming_solver_->GetArmorsToView(); + aiming_solver_->SolveAimSolution(snapshot_manager, time_stamp, control_delay_); + armors_to_view_ = aiming_solver_->GetArmorsToView(); if (!aim_solution.valid) { std::println("aim solution invalid ,solver failed"); - return data::FireControl { .time_stamp = - data::TimeStamp { snapshot_manager->GetTimeStamp() - + time_from_tracker_timepoint }, + return data::FireControl { .time_stamp = time_stamp, .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; } @@ -75,13 +67,13 @@ class FireController::Impl { firable_ = fire_command; data::FireControl result; - result.fire_allowance = fire_command; - // result.fire_allowance = true; + // result.fire_allowance = fire_command; + result.fire_allowance = true; // result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; result.gimbal_dir << cos(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.pitch); - result.time_stamp = data::TimeStamp { time_from_tracker_timepoint }; + result.time_stamp = time_stamp; return result; } @@ -92,9 +84,9 @@ class FireController::Impl { void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; - // auto GetArmorsToView() -> std::shared_ptr { - // return armors_to_view_; - // } + auto GetArmorsToView() -> std::shared_ptr { + return armors_to_view_; + } private: std::chrono::milliseconds control_delay_; @@ -109,7 +101,7 @@ class FireController::Impl { std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; - // mutable std::shared_ptr armors_to_view_; + mutable std::shared_ptr armors_to_view_; }; FireController::FireController(const std::string& config_path, @@ -118,18 +110,17 @@ FireController::FireController(const std::string& config_path, : pimpl_(std::make_unique(config_path, state_machine, live_target_manager)) { } FireController::~FireController() = default; -const data ::FireControl FireController::CalculateTarget( - const std::chrono::nanoseconds& time_duration) const { - return pimpl_->CalculateTarget(time_duration); +data ::FireControl FireController::CalculateTarget(data::TimeStamp const& time_stamp) const { + return pimpl_->CalculateTarget(time_stamp); } -const CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } +CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { return pimpl_->UpdateGimbalPosition(gimbal_yaw); }; -// auto FireController::GetArmorsToView() -> std::shared_ptr { -// return pimpl_->GetArmorsToView(); -// } +auto FireController::GetArmorsToView() -> std::shared_ptr { + return pimpl_->GetArmorsToView(); +} } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index b0399d1..4038b5b 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -1,6 +1,5 @@ #pragma once -#include #include #include "interfaces/car_state.hpp" @@ -16,9 +15,8 @@ class FireController final : public interfaces::IFireControl { std::shared_ptr const& live_target_manager); ~FireController(); - const data ::FireControl CalculateTarget( - const std::chrono::nanoseconds& time_duration) const override; - const enumeration ::CarIDFlag GetAttackCarId() const override; + data ::FireControl CalculateTarget(data::TimeStamp const& time_stamp) const override; + enumeration ::CarIDFlag GetAttackCarId() const override; void UpdateGimbalPosition(const double& gimbal_yaw); diff --git a/src/tongji/identifier/tracker.hpp b/src/tongji/identifier/tracker.hpp index 1305537..127b79b 100644 --- a/src/tongji/identifier/tracker.hpp +++ b/src/tongji/identifier/tracker.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -7,6 +8,7 @@ #include #include "armor_filter.hpp" +#include "data/time_stamped.hpp" #include "decider.hpp" #include "enum/armor_id.hpp" #include "identified_armor.hpp" @@ -27,16 +29,18 @@ class Tracker final { public: Tracker() : armor_filter_(std::make_unique()) - , decider_(std::make_unique()) { } + , decider_(std::make_unique()) + , time_stamp_(data::TimeStamp()) { } ~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 enumeration::CarIDFlag& invincible_armors, data::TimeStamp const& time_stamp) noexcept + -> enumeration::ArmorIdFlag { + + CheckCameraOffline(time_stamp); + time_stamp_ = time_stamp; - CheckCameraOffline(duration_from_last_update); armor_filter_->Update(invincible_armors); auto filtered_ids = enumeration::ArmorIdFlag::None; @@ -131,9 +135,11 @@ class Tracker final { } } - void CheckCameraOffline(const std::chrono::milliseconds duration_from_last_update) { + void CheckCameraOffline(data::TimeStamp const& time_stamp) { // if (state_ != TrackState::Lost && (duration_from_last_update > timeout_sec_); - if ((duration_from_last_update > timeout_)) { + auto duration_from_last_update = time_stamp - time_stamp_; + if ((duration_from_last_update.to_nanosec() + > (double)std::chrono::duration_cast(timeout_).count())) { SetState(TrackState::Lost); // std::cout << "I am lost QAQ" << std::endl; } @@ -150,6 +156,8 @@ class Tracker final { TrackState state_ = TrackState::Lost; TrackState pre_state_ = TrackState::Lost; + data::TimeStamp time_stamp_; + std::unique_ptr armor_filter_; std::unique_ptr decider_; diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp index a71eba9..be42927 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -69,15 +69,13 @@ class CarPredictor final : public interfaces::IPredictor { armor.orientation = util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0); armors.emplace_back(std::move(armor)); } - return std::make_shared(armors, time_stamp_); + return std::make_shared(armors, time_stamp); } EKF::XVec GetEkfX() const { return ekf_->x; } auto GetModel() const -> PredictorModel { return model_; } auto GetEkf() const -> EKF { return ekf_.value(); } - data::TimeStamp GetTimeStamp() const override { return time_stamp_; } - auto GetPredictedXYZAList(const double& dt) -> std::vector { const auto [x_n, P_n] = ekf_->PredictOnce(dt); return model_.GetArmorXYZAList(x_n); diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp index 2bd262d..13e7d1a 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -53,7 +53,7 @@ class CarPredictorManager::Impl { if (it == targets_map_.end()) return nullptr; const auto& [id, predictor] = *it; return std::make_shared( - predictor->GetEkf(), predictor->GetModel(), predictor->GetTimeStamp()); + predictor->GetEkf(), predictor->GetModel(), data::TimeStamp { last_update_timestamp_ }); } void Update(std::shared_ptr const& data) { diff --git a/src/tongji/state_machine/state_machine.cpp b/src/tongji/state_machine/state_machine.cpp index 162e292..f7075f8 100644 --- a/src/tongji/state_machine/state_machine.cpp +++ b/src/tongji/state_machine/state_machine.cpp @@ -3,6 +3,7 @@ #include #include "../identifier/tracker.hpp" +#include "data/time_stamped.hpp" #include "enum/car_id.hpp" namespace world_exe::tongji::state_machine { @@ -16,12 +17,10 @@ class StateMachine::Impl { const enumeration::CarIDFlag& GetAllowdToFires() const { return target_id_; } void Update(std::shared_ptr const& armors_in_image, - const enumeration::CarIDFlag& invincible_armors, - const std::chrono::milliseconds& duration_from_last_update) { - + const enumeration::CarIDFlag& invincible_armors, data::TimeStamp const& time_stamp) { target_id_ = enumeration::CarIDFlag::None; - target_id_ = tracker_->SelectTrackingTargetID( - armors_in_image, invincible_armors, duration_from_last_update); + target_id_ = + tracker_->SelectTrackingTargetID(armors_in_image, invincible_armors, time_stamp); } void SetLostState() { @@ -43,9 +42,8 @@ const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { } void StateMachine::Update(std::shared_ptr const& armors_in_image, - const enumeration::CarIDFlag& invincible_armors, - const std::chrono::milliseconds& duration_from_last_update) { - return pimpl_->Update(armors_in_image, invincible_armors, duration_from_last_update); + const enumeration::CarIDFlag& invincible_armors, data::TimeStamp const& time_stamp) { + return pimpl_->Update(armors_in_image, invincible_armors, time_stamp); } 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 2366126..77c92a7 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -16,8 +16,7 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; void Update(std::shared_ptr const& armors_in_image, - const enumeration::CarIDFlag& invincible_armors, - const std::chrono::milliseconds& duration_from_last_update); + const enumeration::CarIDFlag& invincible_armors, data::TimeStamp const& time_stamp); void SetLostState(); diff --git a/src/v1/fire_controller/fire_controller.cpp b/src/v1/fire_controller/fire_controller.cpp index ca056e3..02d4849 100644 --- a/src/v1/fire_controller/fire_controller.cpp +++ b/src/v1/fire_controller/fire_controller.cpp @@ -15,7 +15,7 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { : control_delay_((static_cast(control_delay_in_second * 1e9))) , velocity_begin_(velocity_begin) , gravity_(gravity) { } - const enumeration::CarIDFlag GetAttackCarId() const { + enumeration::CarIDFlag GetAttackCarId() const { double max_dot = -2; enumeration::CarIDFlag ret = enumeration::ArmorIdFlag::None; @@ -46,10 +46,9 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { predictor_ = predictor; }; - const world_exe::data::FireControl CalculateTarget( - const std::chrono::nanoseconds& time_duration) { + world_exe::data::FireControl CalculateTarget(data::TimeStamp const& time_stamp) { std::chrono::seconds fly_time { 0 }; - const auto& pre1 = predictor_->Predictor(fly_time + time_duration + control_delay_); + const auto& pre1 = predictor_->Predictor(time_stamp + fly_time + control_delay_); const auto& pre2 = pre1->GetArmors(predictor_->GetId()); double min_angular_dis = 1e9; int index = -1, index_ = 0; @@ -67,15 +66,13 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { if (index == -1) return no_allow_; for (int i = 5; i-- > 0;) { - const auto& armors_in_gimbal_control = - predictor_->Predictor(fly_time + time_duration + control_delay_); + const auto& armors_in_gimbal_control = predictor_->Predictor(fly_time + control_delay_); const auto& armors = armors_in_gimbal_control->GetArmors(predictor_->GetId()); const auto& [fly_time, dir] = trajectory_solver::gravity_only(armors[index].position, velocity_begin_, gravity_); } - return { .time_stamp = data::TimeStamp { std::chrono::nanoseconds( - fly_time + time_duration + control_delay_) }, + return { .time_stamp = data::TimeStamp { time_stamp + fly_time + control_delay_ }, .fire_allowance = true }; } @@ -90,13 +87,13 @@ class world_exe::v1::fire_control::TracingFireControl::Impl { std::shared_ptr predictor_; }; -const world_exe::data::FireControl // +world_exe::data::FireControl // world_exe::v1::fire_control::TracingFireControl::CalculateTarget( - const std::chrono::nanoseconds& time_duration) const { - return pimpl_->CalculateTarget(time_duration); + data::TimeStamp const& time_stamp) const { + return pimpl_->CalculateTarget(time_stamp); } -const world_exe::enumeration::CarIDFlag +world_exe::enumeration::CarIDFlag world_exe::v1::fire_control::TracingFireControl::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } diff --git a/src/v1/fire_controller/fire_controller.hpp b/src/v1/fire_controller/fire_controller.hpp index 0f982f9..1b753fe 100644 --- a/src/v1/fire_controller/fire_controller.hpp +++ b/src/v1/fire_controller/fire_controller.hpp @@ -1,16 +1,15 @@ #pragma once #include "data/fire_control.hpp" +#include "data/time_stamped.hpp" #include "interfaces/fire_controller.hpp" #include "interfaces/predictor.hpp" -#include #include namespace world_exe::v1::fire_control { class TracingFireControl final : public interfaces::IFireControl { public: - const data::FireControl CalculateTarget( - const std::chrono::nanoseconds& time_duration) const override; - const enumeration::CarIDFlag GetAttackCarId() const override; + data::FireControl CalculateTarget(data::TimeStamp const& time_stamp) const override; + enumeration::CarIDFlag GetAttackCarId() const override; void set_armor(const std::shared_ptr& armors); void SetPredictor(const std::shared_ptr& predictor); void SetTargetCarID(const enumeration::CarIDFlag& tracing_id); diff --git a/src/v1/predictor/car/car_predictor.cpp b/src/v1/predictor/car/car_predictor.cpp index f135ce4..b087fd7 100644 --- a/src/v1/predictor/car/car_predictor.cpp +++ b/src/v1/predictor/car/car_predictor.cpp @@ -63,6 +63,4 @@ void CarPredictor::SetTimeStamp(const data::TimeStamp& time_stamp) { return pimpl_->SetTimeStamp(time_stamp); }; -data ::TimeStamp CarPredictor::GetTimeStamp() const { return pimpl_->GetTimeStamp(); } - } \ No newline at end of file diff --git a/src/v1/predictor/car/car_predictor.hpp b/src/v1/predictor/car/car_predictor.hpp index e3e542d..e9776a1 100644 --- a/src/v1/predictor/car/car_predictor.hpp +++ b/src/v1/predictor/car/car_predictor.hpp @@ -8,8 +8,6 @@ namespace world_exe::v1::predictor { class CarPredictor final : public interfaces::IPredictor { public: - data ::TimeStamp GetTimeStamp() const override; - CarPredictor(); ~CarPredictor(); CarPredictor(const enumeration::CarIDFlag& id, const CarPredictEkf& ekf, From a6e93bb17bfbc6bd3770750c1eb017583b20fa1c Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Fri, 14 Nov 2025 00:42:56 +0800 Subject: [PATCH 05/14] refactor(predictor): adapt to updated fire control iteration interface --- src/tongji/auto_aim_system.cpp | 6 ++--- src/tongji/fire_controller/aim_solver.hpp | 18 +++++++-------- .../fire_controller/fire_controller.cpp | 16 +++++++------- .../predictor/car_predictor/car_predictor.hpp | 22 +++++++++---------- 4 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index b21d418..554ce86 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -116,9 +116,9 @@ class AutoAimSystem::Impl { core::EventBus::Publish( parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); - core::EventBus::Publish>( - world_exe::parameters::ParamsForSystemV1::get_lastest_predictor_event, - fire_controller_->GetArmorsToView()); + // core::EventBus::Publish>( + // world_exe::parameters::ParamsForSystemV1::get_lastest_predictor_event, + // fire_controller_->GetArmorsToView()); if (!debug) [[likely]] return; diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index 012fd11..c4f6a27 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -47,7 +47,7 @@ class AimingSolver { AimSolution SolveAimSolution(std::shared_ptr const& snapshot, data::TimeStamp const& time_stamp, std::chrono::milliseconds control_delay) { - // time_stamp.to_seconds(),control_delay); 迭代求解飞行时间 + // 迭代求解飞行时间 // (最多10次，收敛条件：相邻两次fly_time差 <0.001) double prev_fly_time_s = 0; Eigen::Vector3d final_aim_point; @@ -65,12 +65,12 @@ class AimingSolver { const auto& armors = snapshot->Predictor(time_stamp + data::TimeStamp::from_seconds(dt)); - const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); - armors_to_view_ = std::make_shared( - armors_to_view, time_stamp + data::TimeStamp::from_seconds(dt)); + // const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); + // armors_to_view_ = std::make_shared( + // armors_to_view, time_stamp + data::TimeStamp::from_seconds(dt)); const auto& aim_point = SelectPredictedAim( - snapshot_derived->GetPredictedX(dt), armors_to_view, snapshot->GetId()); + snapshot_derived->GetPredictedX(time_stamp), armors->GetArmors(snapshot->GetId()), snapshot->GetId()); if (!aim_point.has_value()) { std::println("no valid aim point"); @@ -108,9 +108,9 @@ class AimingSolver { return { true, yaw, pitch, final_aim_point }; } - auto GetArmorsToView() -> std::shared_ptr { - return armors_to_view_; - } + // auto GetArmorsToView() -> std::shared_ptr { + // return armors_to_view_; + // } private: std::optional SelectPredictedAim(const EKF::XVec& ekf_x, @@ -137,7 +137,7 @@ class AimingSolver { double yaw_offset_, pitch_offset_; double bullet_speed_; const double g_; - std::shared_ptr armors_to_view_; + // std::shared_ptr armors_to_view_; std::unique_ptr aim_point_chooser_; }; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 6638259..d9e3e3d 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -50,7 +50,7 @@ class FireController::Impl { // TODO:这里不应该指针转换 const auto& aim_solution = aiming_solver_->SolveAimSolution(snapshot_manager, time_stamp, control_delay_); - armors_to_view_ = aiming_solver_->GetArmorsToView(); + // armors_to_view_ = aiming_solver_->GetArmorsToView(); if (!aim_solution.valid) { std::println("aim solution invalid ,solver failed"); @@ -84,9 +84,9 @@ class FireController::Impl { void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; - auto GetArmorsToView() -> std::shared_ptr { - return armors_to_view_; - } + // auto GetArmorsToView() -> std::shared_ptr { + // return armors_to_view_; + // } private: std::chrono::milliseconds control_delay_; @@ -101,7 +101,7 @@ class FireController::Impl { std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; - mutable std::shared_ptr armors_to_view_; + // mutable std::shared_ptr armors_to_view_; }; FireController::FireController(const std::string& config_path, @@ -119,8 +119,8 @@ void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { return pimpl_->UpdateGimbalPosition(gimbal_yaw); }; -auto FireController::GetArmorsToView() -> std::shared_ptr { - return pimpl_->GetArmorsToView(); -} +// auto FireController::GetArmorsToView() -> std::shared_ptr { +// return pimpl_->GetArmorsToView(); +// } } diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp index be42927..1f8fa89 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -6,6 +6,7 @@ #include #include +#include #include "../in_gimbal_control_armor.hpp" #include "../kalman_filter/extended_kalman_filter.hpp" @@ -22,10 +23,9 @@ class CarPredictor final : public interfaces::IPredictor { using PredictorModel = EKFModel<11, 4>; using EKF = ExtendedKalmanFilter; - explicit CarPredictor( - const EKF& ekf, const PredictorModel& model, const data::TimeStamp& time_stamp) + explicit CarPredictor(const EKF& ekf, PredictorModel model, const data::TimeStamp& time_stamp) : ekf_(ekf) - , model_(model) + , model_(std::move(model)) , time_stamp_(time_stamp) { } explicit CarPredictor(const Eigen::Vector3d& armor_xyz_in_gimbal, @@ -76,14 +76,9 @@ class CarPredictor final : public interfaces::IPredictor { auto GetModel() const -> PredictorModel { return model_; } auto GetEkf() const -> EKF { return ekf_.value(); } - auto GetPredictedXYZAList(const double& dt) -> std::vector { - const auto [x_n, P_n] = ekf_->PredictOnce(dt); - return model_.GetArmorXYZAList(x_n); - } - - auto GetPredictedX(const double& dt) const -> EKF::XVec { - const auto& [x_n, P_n] = ekf_->PredictOnce(dt); - return x_n; + auto GetPredictedX(data::TimeStamp const& time_stamp) -> EKF::XVec { + auto dt = (time_stamp - time_stamp_).to_seconds(); + return GetPredictedX(dt); } void Update(data::TimeStamp const& time_stamp, const Eigen::Vector3d& armor_xyz_in_gimbal, @@ -115,6 +110,11 @@ class CarPredictor final : public interfaces::IPredictor { } private: + auto GetPredictedX(double dt) const -> EKF::XVec { + const auto& [x_n, P_n] = ekf_->PredictOnce(dt); + return x_n; + } + void Update_ypda(const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal, const int& id, const double& dt) { From d246184129faf23fc03cd89ab47e82168818b603 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 15 Nov 2025 21:20:56 +0800 Subject: [PATCH 06/14] fix(predictor): correct transformation package transmission error - Simulation tests pass with corrected transformation flow - TODO: integrate tests with RMCS system --- src/tongji/auto_aim_system.cpp | 20 ++++---- src/tongji/fire_controller/aim_solver.hpp | 23 +++++---- .../fire_controller/fire_controller.cpp | 18 +++---- .../predictor/car_predictor/car_predictor.hpp | 3 +- .../car_predictor/car_predictor_manager.cpp | 33 ++++++++----- .../predictor/kalman_filter/predict_model.hpp | 2 +- tests/mock_gimbal_test.cpp | 18 ++++--- tests/mocks/mock_camera_tranform.hpp | 48 ++++++++++--------- 8 files changed, 92 insertions(+), 73 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 554ce86..d60033a 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -85,9 +85,13 @@ class AutoAimSystem::Impl { // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 auto [pack, check] = syncer_->get_data(raw.stamp); - if (!check) - pack.camera_capture_begin_time_stamp = - data::TimeStamp(steady_clock::now().time_since_epoch()); + if (!check) { + // TODO：等待传入真实数据 + // pack.camera_capture_begin_time_stamp = + // data::TimeStamp(steady_clock::now().time_since_epoch()); + std::println(" no sync data"); + return; + } const auto R_camera2gimbal = pack.camera_to_gimbal.rotation(); const auto t_camera2gimbal = pack.camera_to_gimbal.translation(); @@ -116,9 +120,9 @@ class AutoAimSystem::Impl { core::EventBus::Publish( parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); - // core::EventBus::Publish>( - // world_exe::parameters::ParamsForSystemV1::get_lastest_predictor_event, - // fire_controller_->GetArmorsToView()); + core::EventBus::Publish>( + world_exe::parameters::ParamsForSystemV1::get_lastest_predictor_event, + fire_controller_->GetArmorsToView()); if (!debug) [[likely]] return; @@ -159,7 +163,8 @@ class AutoAimSystem::Impl { // TODO:时间戳有待fix data::FireControl GetControlCommand() { - fire_controller_->GetAttackCarId(); + auto attacked_id = fire_controller_->GetAttackCarId(); + return fire_controller_->CalculateTarget( data::TimeStamp(steady_clock::now().time_since_epoch())); } @@ -175,7 +180,6 @@ class AutoAimSystem::Impl { std::shared_ptr live_target_manager_; std::unique_ptr syncer_; std::unique_ptr fire_controller_; - // std::unique_ptr mock_camera_tranform_; }; AutoAimSystem::AutoAimSystem(const bool& debug) diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index c4f6a27..19914b0 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -65,15 +65,15 @@ class AimingSolver { const auto& armors = snapshot->Predictor(time_stamp + data::TimeStamp::from_seconds(dt)); - // const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); - // armors_to_view_ = std::make_shared( - // armors_to_view, time_stamp + data::TimeStamp::from_seconds(dt)); + const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); - const auto& aim_point = SelectPredictedAim( - snapshot_derived->GetPredictedX(time_stamp), armors->GetArmors(snapshot->GetId()), snapshot->GetId()); + armors_to_view_ = std::make_shared( + armors_to_view, time_stamp + data::TimeStamp::from_seconds(dt)); + + const auto& aim_point = SelectPredictedAim(snapshot_derived->GetPredictedX(time_stamp), + armors->GetArmors(snapshot->GetId()), snapshot->GetId()); if (!aim_point.has_value()) { - std::println("no valid aim point"); continue; } // failed: no valid aim point @@ -94,13 +94,12 @@ class AimingSolver { } if (!converged) { - std::println("trajectory diverse"); - return { false, std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), { }, 0 }; // failed: trajectory did not converge } + // std::println("fly_time:{}", final_trajectory.fly_time); const auto xyz = final_aim_point; const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; const double pitch = (final_trajectory.pitch + pitch_offset_); @@ -108,9 +107,9 @@ class AimingSolver { return { true, yaw, pitch, final_aim_point }; } - // auto GetArmorsToView() -> std::shared_ptr { - // return armors_to_view_; - // } + auto GetArmorsToView() -> std::shared_ptr { + return armors_to_view_; + } private: std::optional SelectPredictedAim(const EKF::XVec& ekf_x, @@ -137,7 +136,7 @@ class AimingSolver { double yaw_offset_, pitch_offset_; double bullet_speed_; const double g_; - // std::shared_ptr armors_to_view_; + std::shared_ptr armors_to_view_; std::unique_ptr aim_point_chooser_; }; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index d9e3e3d..b407511 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -50,10 +50,10 @@ class FireController::Impl { // TODO:这里不应该指针转换 const auto& aim_solution = aiming_solver_->SolveAimSolution(snapshot_manager, time_stamp, control_delay_); - // armors_to_view_ = aiming_solver_->GetArmorsToView(); + armors_to_view_ = aiming_solver_->GetArmorsToView(); if (!aim_solution.valid) { - std::println("aim solution invalid ,solver failed"); + std::println("aim solution invalid "); return data::FireControl { .time_stamp = time_stamp, .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; @@ -84,9 +84,9 @@ class FireController::Impl { void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; - // auto GetArmorsToView() -> std::shared_ptr { - // return armors_to_view_; - // } + auto GetArmorsToView() -> std::shared_ptr { + return armors_to_view_; + } private: std::chrono::milliseconds control_delay_; @@ -101,7 +101,7 @@ class FireController::Impl { std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; - // mutable std::shared_ptr armors_to_view_; + mutable std::shared_ptr armors_to_view_; }; FireController::FireController(const std::string& config_path, @@ -119,8 +119,8 @@ void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { return pimpl_->UpdateGimbalPosition(gimbal_yaw); }; -// auto FireController::GetArmorsToView() -> std::shared_ptr { -// return pimpl_->GetArmorsToView(); -// } +auto FireController::GetArmorsToView() -> std::shared_ptr { + return pimpl_->GetArmorsToView(); +} } diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp index 1f8fa89..ab65c64 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -6,6 +6,7 @@ #include #include +#include #include #include "../in_gimbal_control_armor.hpp" @@ -101,7 +102,6 @@ class CarPredictor final : public interfaces::IPredictor { auto rot = abs(ekf_->x[7]) > 0.3f; if (r_ok && l_ok && rot) return false; - // util::logger::logger()->debug("[Target] r={:.3f}, l={:.3f}", ekf_->x[8], ekf_->x[9]); return true; } auto IsAppeared() -> bool { @@ -138,6 +138,7 @@ class CarPredictor final : public interfaces::IPredictor { ekf_->x[7] = ekf_->x[7] > 0 ? max_outpost_w : -max_outpost_w; } } + std::println("r:{}", ekf_->x[8]); } data::TimeStamp time_stamp_; diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp index 13e7d1a..8bfb465 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -57,34 +57,41 @@ class CarPredictorManager::Impl { } void Update(std::shared_ptr const& data) { - last_update_timestamp_ = data->GetTimeStamp(); - - const Eigen::Affine3d transform = data->GetCameraToWorld(); - const auto armors_interface = data->GetArmors(); + Eigen::Affine3d transform_camera2world = data->GetCameraToWorld(); + const auto armors_interface_incamera = data->GetArmors(); for (int i = 0; i < 8; i++) { auto id = static_cast( static_cast(enumeration::CarIDFlag::Hero) << i); - const auto& armors = armors_interface->GetArmors(id); - if (armors.empty()) continue; + const auto& armors_in_camera = armors_interface_incamera->GetArmors(id); + if (armors_in_camera.empty()) continue; - for (const auto& armor : armors) { + for (const auto& armor : armors_in_camera) { if (!targets_map_.contains(armor.id)) { targets_map_.try_emplace(armor.id, - std::make_unique(armor.position, - util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), armor.id, - data->GetTimeStamp())); + std::make_unique(transform_camera2world * armor.position, + util::math::quaternion_to_euler( + Eigen::Quaterniond(transform_camera2world.rotation()) + * armor.orientation, + 2, 1, 0), + armor.id, data->GetTimeStamp())); } else { - targets_map_.at(armor.id)->Update(data->GetTimeStamp(), armor.position, - util::math::quaternion_to_euler(armor.orientation, 2, 1, 0), - util::math::xyz2ypd(armor.position)); + targets_map_.at(armor.id)->Update(data->GetTimeStamp(), + transform_camera2world * armor.position, + util::math::quaternion_to_euler( + Eigen::Quaterniond(transform_camera2world.rotation()) + * armor.orientation, + 2, 1, 0), + util::math::xyz2ypd(transform_camera2world * armor.position)); } } // std::erase_if(targets_map_, [](const auto& pair) { return pair.second->IsAppeared(); // }); } + + last_update_timestamp_ = data->GetTimeStamp(); } private: diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp index 4416084..6e198fe 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -131,7 +131,7 @@ class EKFModel { int best_id = 0; double min_error = 1e10; - for (int i = 0; i < std::min(3, armor_num_); ++i) { + for (int i = 0; i < std::max(3, armor_num_); ++i) { const auto& xyza = xyza_i_list[i].first; auto ypd = util::math::xyz2ypd(xyza.head(3)); double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - xyza(3))) diff --git a/tests/mock_gimbal_test.cpp b/tests/mock_gimbal_test.cpp index c9090f9..a47c89c 100644 --- a/tests/mock_gimbal_test.cpp +++ b/tests/mock_gimbal_test.cpp @@ -2,10 +2,11 @@ #include "mocks/MockArmorInCamera.hpp" #include "mocks/MockArmorInWorld.hpp" #include "mocks/mock_camera_tranform.hpp" -#include #include #include +#include +#include #include #include @@ -17,12 +18,15 @@ int main() { using namespace world_exe::util::visualization; - Eigen::Vector3d V_input(1., 0., 0.); - double Omega_Yaw = 0.1; - double Omega_Pitch = 0.05; - double Max_Pitch = M_PI / 48.0; - auto transformer = world_exe::tests::mock::Camera2GimbalTransformer( - V_input, Omega_Yaw, Omega_Pitch, Max_Pitch); + Eigen::Vector3d V_input(0., 1., 0.); + double Omega_Yaw = 0.0; + double Omega_Pitch = 0.0; + double Omega_Roll = 0.0; + double Max_Pitch = M_PI / 6.0; + double Max_Roll = M_PI / 12; + + auto transformer = world_exe::tests::mock::Camera2GimbalTransformer( + V_input, Omega_Yaw, Omega_Pitch, Omega_Roll, Max_Pitch, Max_Roll); double dt = 0.01; diff --git a/tests/mocks/mock_camera_tranform.hpp b/tests/mocks/mock_camera_tranform.hpp index a130dad..572842e 100644 --- a/tests/mocks/mock_camera_tranform.hpp +++ b/tests/mocks/mock_camera_tranform.hpp @@ -2,6 +2,8 @@ #include #include +#include +#include using namespace Eigen; using AffineTransform = Affine3d; // 仿射变换矩阵 (4x4) @@ -19,19 +21,20 @@ class Camera2GimbalTransformer { // 当前时间 double current_time; - // --- 输入参数 --- - // 1. 水平平移速度 (在世界坐标系下) const Vector3D V_horizontal; - // 2. 绕竖直轴 (Z轴) 的角速度 (在世界坐标系下) + const double Omega_Yaw; - // 3. 绕 Pitch 轴的角速度 (在世界坐标系下) + double Omega_Pitch; - // 4. 最大 Pitch 角度 (用于约束) const double Max_Pitch_Angle; // 用于追踪当前 Pitch 角度，以实现角度约束 (假设Pitch轴是Y轴) double current_pitch_angle; + double Omega_Roll; + const double Max_Roll_Angle; + double current_roll_angle; + public: /** * @brief 构造函数 @@ -40,16 +43,18 @@ class Camera2GimbalTransformer { * @param omega_pitch 绕 Pitch 轴角速度 (rad/s) * @param max_pitch_angle 绕 Pitch 轴的最大角度限制 (rad) */ - Camera2GimbalTransformer( - const Vector3D& v_horiz, double omega_yaw, double omega_pitch, double max_pitch_angle) - : V_horizontal(v_horiz) + Camera2GimbalTransformer(Vector3D v_horiz, double omega_yaw, double omega_pitch, + double omega_roll, double max_pitch_angle, double max_roll_angle) + : V_horizontal(std::move(v_horiz)) , Omega_Yaw(omega_yaw) , Omega_Pitch(omega_pitch) + , Omega_Roll(omega_roll) , Max_Pitch_Angle(std::abs(max_pitch_angle)) - , // 确保最大角度是正值 - T_current(AffineTransform::Identity()) + , Max_Roll_Angle(std::abs(max_roll_angle)) + , T_current(AffineTransform::Identity()) , current_time(0.0) - , current_pitch_angle(0.0) { } + , current_pitch_angle(0.0) + , current_roll_angle(0.0) { } /** * @brief 按照给定的时间步长进行一次状态积分，并返回更新后的 Affine 变换 @@ -65,31 +70,30 @@ class Camera2GimbalTransformer { double delta_pitch = Omega_Pitch * dt; double next_pitch_angle = current_pitch_angle + delta_pitch; if (next_pitch_angle > Max_Pitch_Angle || next_pitch_angle < -Max_Pitch_Angle) { - Omega_Pitch = -Omega_Pitch; delta_pitch = Omega_Pitch * dt; } - current_pitch_angle = current_pitch_angle + delta_pitch; - AngleAxisd R_pitch(delta_pitch, Vector3D::UnitY()); - // 2c. 组合旋转增量：R_total = R_yaw * R_pitch - // 假设 Pitch 和 Yaw 是在世界坐标系下连续作用的 (即增量也是在世界坐标系下) - // 结果是一个 AngleAxisd 或 Quaternion，需要转换为 Affine - AffineTransform T_rot_increment = AffineTransform(R_yaw * R_pitch); + double delta_roll = Omega_Roll * dt; + current_roll_angle += delta_roll; + if (current_roll_angle > Max_Roll_Angle || current_roll_angle < -Max_Roll_Angle) { + Omega_Roll = -Omega_Roll; + delta_roll = Omega_Roll * dt; + } + AngleAxisd R_roll(delta_roll, Vector3D::UnitX()); - // --- 3. 组合总增量 T_delta --- + AffineTransform T_rot_increment = AffineTransform(R_yaw * R_pitch * R_roll); // T_delta = T_平移 * T_旋转 AffineTransform T_delta = Translation3d(delta_t) * T_rot_increment; - // --- 4. 状态更新 --- - - // T_new = T_delta * T_old (因为 T_delta 是相对于世界坐标系W的增量) T_current = T_delta * T_current; current_time += dt; + // std::cout << T_current.matrix() << std::endl; + return T_current; } }; From 22e4f20b852752649f2a7fee84440f09681f75e8 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 15 Nov 2025 21:44:36 +0800 Subject: [PATCH 07/14] chore(visualization): remove visualization and debug code --- src/tongji/auto_aim_system.cpp | 6 +++--- src/tongji/fire_controller/aim_solver.hpp | 5 ++--- src/tongji/fire_controller/fire_controller.cpp | 2 +- src/tongji/predictor/car_predictor/car_predictor.hpp | 1 - 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index d60033a..592d377 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -8,7 +8,7 @@ #include #include #include -#include +// #include #include #include "../v1/sync/syncer.hpp" @@ -76,7 +76,7 @@ class AutoAimSystem::Impl { if (flag == enumeration::ArmorIdFlag::None) { state_machine_->SetLostState(); - std::println("no armors identified"); + // std::println("no armors identified"); return; } @@ -89,7 +89,7 @@ class AutoAimSystem::Impl { // TODO：等待传入真实数据 // pack.camera_capture_begin_time_stamp = // data::TimeStamp(steady_clock::now().time_since_epoch()); - std::println(" no sync data"); + // std::println(" no sync data"); return; } diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index 19914b0..dc414bc 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include @@ -80,7 +79,7 @@ class AimingSolver { const auto traj = SolveTrajectory(aim_point.value(), bullet_speed_); if (!traj.has_value()) { - std::println("trajectory unsolvable"); + // std::println("trajectory unsolvable"); continue; } @@ -127,7 +126,7 @@ class AimingSolver { auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); if (!result.solvable) { - std::println("solve trajectory failed: d={}, z={},speed={}", d, xyz.z(), bullet_speed); + // std::println("solve trajectory failed: d={}, z={},speed={}", d, xyz.z(), bullet_speed); } return result.solvable ? std::optional { result } : std::nullopt; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index b407511..f52f1b3 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -53,7 +53,7 @@ class FireController::Impl { armors_to_view_ = aiming_solver_->GetArmorsToView(); if (!aim_solution.valid) { - std::println("aim solution invalid "); + // std::println("aim solution invalid "); return data::FireControl { .time_stamp = time_stamp, .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp index ab65c64..8df5950 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -138,7 +138,6 @@ class CarPredictor final : public interfaces::IPredictor { ekf_->x[7] = ekf_->x[7] > 0 ? max_outpost_w : -max_outpost_w; } } - std::println("r:{}", ekf_->x[8]); } data::TimeStamp time_stamp_; From 2cf28bee3232c7be315344d8d011a5e8e80f3c43 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 15 Nov 2025 21:52:10 +0800 Subject: [PATCH 08/14] remove print header --- src/tongji/fire_controller/fire_controller.cpp | 1 - src/tongji/predictor/car_predictor/car_predictor.hpp | 1 - 2 files changed, 2 deletions(-) diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index f52f1b3..7668f29 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -3,7 +3,6 @@ #include #include -#include #include #include diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp index 8df5950..197f48b 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -6,7 +6,6 @@ #include #include -#include #include #include "../in_gimbal_control_armor.hpp" From 0845e5ae5bc8af9e6794405945ba991f3ed2e9e4 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Sat, 15 Nov 2025 23:01:50 +0800 Subject: [PATCH 09/14] refactor(architecture): expose coordinate transformation interface and delegate fire control to ECU --- src/tongji/auto_aim_system.cpp | 14 +---- .../fire_controller/aim_point_chooser.hpp | 4 +- src/tongji/fire_controller/aim_solver.hpp | 43 ++++++++------ .../fire_controller/fire_controller.cpp | 57 ++++++------------- .../fire_controller/fire_controller.hpp | 4 +- 5 files changed, 48 insertions(+), 74 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 592d377..0446f18 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -76,7 +77,7 @@ class AutoAimSystem::Impl { if (flag == enumeration::ArmorIdFlag::None) { state_machine_->SetLostState(); - // std::println("no armors identified"); + std::cout << "no armor identified!" << std::endl; return; } @@ -106,24 +107,15 @@ class AutoAimSystem::Impl { parameters::ParamsForSystemV1::tracker_current_armors_event, live_target_manager_->Predict(flag, pack.camera_capture_begin_time_stamp)); - const auto target_id = state_machine_->GetAllowdToFires(); - - // TODO:读编码器还是旋转矩阵？ - const auto gimbal_yaw = R_camera2gimbal.eulerAngles(2, 1, 0)[0]; - time_stamp_ = pack.camera_capture_begin_time_stamp; - fire_controller_->UpdateGimbalPosition(gimbal_yaw); /// 这里应该有一个线程进行稳定的输出之类的 /// 轨迹规划器没有实现，先不管 + fire_controller_->SetGimbal2Muzzle(pack.gimbal_to_muzzle); core::EventBus::Publish( parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); - core::EventBus::Publish>( - world_exe::parameters::ParamsForSystemV1::get_lastest_predictor_event, - fire_controller_->GetArmorsToView()); - if (!debug) [[likely]] return; diff --git a/src/tongji/fire_controller/aim_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index 63e04f5..c1120b7 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -17,9 +17,9 @@ class AimPointChooser { leaving_angle_ = yaml["leaving_angle"].as() / 57.3; // degree to rad } + template std::pair ChooseAimArmor( - const Eigen::Vector& ekf_x, - std::vector const& armors, const CarIDFlag& single_id) { + const Eigen::Vector& ekf_x, T const& armors, const CarIDFlag& single_id) { const auto armor_num = armors.size(); int chosen_id = -1; diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index dc414bc..57048d9 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -4,15 +4,16 @@ #include #include #include +#include #include #include - +#include #include -#include #include #include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.hpp" +#include "data/armor_gimbal_control_spacing.hpp" #include "data/time_stamped.hpp" #include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" #include "tongji/predictor/kalman_filter/predict_model.hpp" @@ -44,7 +45,8 @@ class AimingSolver { } AimSolution SolveAimSolution(std::shared_ptr const& snapshot, - data::TimeStamp const& time_stamp, std::chrono::milliseconds control_delay) { + Eigen::Affine3d const& transform_gimbal2muzzle, data::TimeStamp const& time_stamp, + std::chrono::milliseconds const& control_delay) { // 迭代求解飞行时间 // (最多10次，收敛条件：相邻两次fly_time差 <0.001) @@ -64,13 +66,24 @@ class AimingSolver { const auto& armors = snapshot->Predictor(time_stamp + data::TimeStamp::from_seconds(dt)); - const auto& armors_to_view = armors->GetArmors(snapshot->GetId()); + const auto& armors_in_gimbal = armors->GetArmors(snapshot->GetId()); + + auto armors_in_muzzle = armors_in_gimbal + | std::ranges::views::transform([&transform_gimbal2muzzle]( + auto const& armor_in_gimbal) { + data::ArmorGimbalControlSpacing armor_in_muzzle; + + armor_in_muzzle.id = armor_in_gimbal.id; + armor_in_muzzle.position = transform_gimbal2muzzle * armor_in_gimbal.position; + armor_in_muzzle.orientation = + Eigen::Quaterniond(transform_gimbal2muzzle.rotation()) + * armor_in_gimbal.orientation; - armors_to_view_ = std::make_shared( - armors_to_view, time_stamp + data::TimeStamp::from_seconds(dt)); + return armor_in_muzzle; + }); - const auto& aim_point = SelectPredictedAim(snapshot_derived->GetPredictedX(time_stamp), - armors->GetArmors(snapshot->GetId()), snapshot->GetId()); + const auto& aim_point = SelectPredictedAim( + snapshot_derived->GetPredictedX(time_stamp), armors_in_muzzle, snapshot->GetId()); if (!aim_point.has_value()) { continue; @@ -98,7 +111,6 @@ class AimingSolver { 0 }; // failed: trajectory did not converge } - // std::println("fly_time:{}", final_trajectory.fly_time); const auto xyz = final_aim_point; const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; const double pitch = (final_trajectory.pitch + pitch_offset_); @@ -106,13 +118,10 @@ class AimingSolver { return { true, yaw, pitch, final_aim_point }; } - auto GetArmorsToView() -> std::shared_ptr { - return armors_to_view_; - } - private: - std::optional SelectPredictedAim(const EKF::XVec& ekf_x, - const std::vector& armors, const CarIDFlag& id) const { + template + std::optional SelectPredictedAim( + const EKF::XVec& ekf_x, const T& armors, const CarIDFlag& id) const { const auto& [selectable, aim_point_in_gimbal] = aim_point_chooser_->ChooseAimArmor(ekf_x, armors, id); @@ -126,7 +135,8 @@ class AimingSolver { auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, xyz.z(), g_); if (!result.solvable) { - // std::println("solve trajectory failed: d={}, z={},speed={}", d, xyz.z(), bullet_speed); + std::cout << "solve trajectory failed: d=" << d << " z=" << xyz.z() + << "speed=" << bullet_speed << std::endl; } return result.solvable ? std::optional { result } : std::nullopt; @@ -135,7 +145,6 @@ class AimingSolver { double yaw_offset_, pitch_offset_; double bullet_speed_; const double g_; - std::shared_ptr armors_to_view_; std::unique_ptr aim_point_chooser_; }; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 7668f29..ee03e12 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -1,5 +1,6 @@ #include "fire_controller.hpp" +#include #include #include @@ -26,17 +27,12 @@ class FireController::Impl { public: Impl(const std::string& config_path, std::shared_ptr state_machine, std::shared_ptr live_target_manager) - : locked_target(CarIDFlag::None) - , firable_(false) - , aiming_solver_(std::make_unique(config_path)) - , fire_decision_(std::make_unique(config_path)) + : aiming_solver_(std::make_unique(config_path)) , state_machine_(std::move(state_machine)) - , live_target_manager_(std::move(live_target_manager)) - , control_delay_(100) { } + , live_target_manager_(std::move(live_target_manager)) { } data ::FireControl CalculateTarget(data::TimeStamp const& time_stamp) const { - if (!fire_decision_ || !state_machine_ || !live_target_manager_) - return { .fire_allowance = false }; + if (!state_machine_ || !live_target_manager_) return { .fire_allowance = false }; const auto& lockable_target = state_machine_->GetAllowdToFires(); @@ -47,9 +43,8 @@ class FireController::Impl { .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; // TODO:这里不应该指针转换 - const auto& aim_solution = - aiming_solver_->SolveAimSolution(snapshot_manager, time_stamp, control_delay_); - armors_to_view_ = aiming_solver_->GetArmorsToView(); + const auto& aim_solution = aiming_solver_->SolveAimSolution( + snapshot_manager, transform_gimbal2muzzle_, time_stamp, control_delay_); if (!aim_solution.valid) { // std::println("aim solution invalid "); @@ -60,16 +55,8 @@ class FireController::Impl { const auto gimbal_command = GimbalCommand { aim_solution.yaw, aim_solution.pitch }; const auto target_pos = Eigen::Vector3d { aim_solution.aim_point }; - auto fire_command = aim_solution.valid - ? fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_pos) - : false; - firable_ = fire_command; - data::FireControl result; - // result.fire_allowance = fire_command; - result.fire_allowance = true; - - // result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; + result.fire_allowance = aim_solution.valid; result.gimbal_dir << cos(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.pitch); result.time_stamp = time_stamp; @@ -77,30 +64,23 @@ class FireController::Impl { } CarIDFlag GetAttackCarId() const { - if (firable_) return locked_target; + if (firable_) return locked_target_; return CarIDFlag::None; } - void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; - - auto GetArmorsToView() -> std::shared_ptr { - return armors_to_view_; + void SetGimbal2Muzzle(Eigen::Affine3d const& transform_gimbal2muzzle) { + transform_gimbal2muzzle_ = transform_gimbal2muzzle; } private: - std::chrono::milliseconds control_delay_; - - double gimbal_yaw_; - - CarIDFlag locked_target; - mutable bool firable_; + std::chrono::milliseconds control_delay_ { 100 }; + CarIDFlag locked_target_ { CarIDFlag::None }; + mutable bool firable_ { false }; + Eigen::Affine3d transform_gimbal2muzzle_ { Eigen ::Affine3d::Identity() }; std::unique_ptr aiming_solver_; - std::unique_ptr fire_decision_; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; - - mutable std::shared_ptr armors_to_view_; }; FireController::FireController(const std::string& config_path, @@ -114,12 +94,7 @@ data ::FireControl FireController::CalculateTarget(data::TimeStamp const& time_s } CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId(); } -void FireController::UpdateGimbalPosition(const double& gimbal_yaw) { - return pimpl_->UpdateGimbalPosition(gimbal_yaw); -}; - -auto FireController::GetArmorsToView() -> std::shared_ptr { - return pimpl_->GetArmorsToView(); +void FireController::SetGimbal2Muzzle(Eigen::Affine3d const& transform_gimbal2muzzle) { + return pimpl_->SetGimbal2Muzzle(transform_gimbal2muzzle); } - } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 4038b5b..1cb7fea 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -18,9 +18,7 @@ class FireController final : public interfaces::IFireControl { data ::FireControl CalculateTarget(data::TimeStamp const& time_stamp) const override; enumeration ::CarIDFlag GetAttackCarId() const override; - void UpdateGimbalPosition(const double& gimbal_yaw); - - auto GetArmorsToView() -> std::shared_ptr; + void SetGimbal2Muzzle(Eigen::Affine3d const& transform_gimbal2muzzle); FireController(const FireController&) = delete; FireController& operator=(const FireController&) = delete; From 480ba1204652e50561b5d038f285911893be5261 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 17 Nov 2025 02:27:57 +0800 Subject: [PATCH 10/14] test: vehicle integration test with RMCS Successful communication at 70 FPS, PNP solver and calibration need optimization. --- CMakeLists.txt | 6 ++- include/utils/mat_triple_buffer.hpp | 43 +++++++++---------- src/tongji/auto_aim_system.cpp | 37 +++++++++++----- src/tongji/fire_controller/aim_solver.hpp | 5 ++- .../car_predictor/car_predictor_manager.cpp | 18 ++++---- src/tongji/solver/solver.cpp | 15 ++----- src/tongji/solver/solver.hpp | 1 - src/util/parameters/params_system_v1.cpp | 16 +++++-- src/util/visaulization.cpp | 3 +- 9 files changed, 82 insertions(+), 62 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 79257ee..2256435 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(OpenVINO REQUIRED) find_package(Ceres REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(ament_index_cpp REQUIRED) # 链接依赖（使用目标名，自动传递） target_link_libraries(alliance_auto_aim PUBLIC @@ -40,6 +41,7 @@ target_link_libraries(alliance_auto_aim PUBLIC openvino::runtime Ceres::ceres yaml-cpp + ament_index_cpp::ament_index_cpp ) # 安装规则 @@ -53,7 +55,9 @@ install(TARGETS alliance_auto_aim ) install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) - +install(DIRECTORY configs/ + DESTINATION share/alliance_ros_auto_aim +) # 导出配置 include(CMakePackageConfigHelpers) configure_package_config_file( diff --git a/include/utils/mat_triple_buffer.hpp b/include/utils/mat_triple_buffer.hpp index b1d9a30..1635a74 100644 --- a/include/utils/mat_triple_buffer.hpp +++ b/include/utils/mat_triple_buffer.hpp @@ -11,50 +11,47 @@ namespace world_exe::util::memory { - -template +template concept BinaryFunctor = requires(F f) { { f() } -> std::same_as; }; -template -class MatTripleBuffer { -/// SPSC only +template class MatTripleBuffer { + /// SPSC only public: - explicit MatTripleBuffer(Func timeFunc) : func_(timeFunc) {} + explicit MatTripleBuffer(Func timeFunc) + : func_(timeFunc) { } - static MatTripleBuffer default_buffer(){ - return MatTripleBuffer(time_stamp::SteadyClock{}); + static MatTripleBuffer default_buffer() { + return MatTripleBuffer(time_stamp::SteadyClock { }); } - - void set(const cv::Mat& image) - { - buffer[ptr_set_].Load(image,func_()); + + void set(const cv::Mat& image) { + buffer[ptr_set_].Load(image, func_()); size_t old = ptr_get_.exchange(ptr_set_, std::memory_order_acq_rel); data_version.fetch_add(1, std::memory_order_release); ptr_set_ = old; } - std::optional> get() - { + std::optional> get() { size_t current_version = data_version.load(std::memory_order_acquire); - size_t expected = last_read_version.load(std::memory_order_acquire); - - if (current_version == expected) return std::nullopt; + size_t expected = last_read_version.load(std::memory_order_acquire); + if (current_version == expected) return std::nullopt; + last_read_version.store(data_version); size_t old = ptr_get_.exchange(ptr_occ_, std::memory_order_acq_rel); - ptr_occ_ = old; + ptr_occ_ = old; return buffer[ptr_occ_]; } - const Func func_; - data::MatStamped buffer[3]; + const Func func_; + data::MatStamped buffer[3]; std::atomic_uint8_t ptr_occ_ = 0; std::atomic_uint8_t ptr_get_ = 1; std::atomic_uint8_t ptr_set_ = 2; - - std::atomic data_version{0}; - std::atomic last_read_version{0}; + + std::atomic data_version { 0 }; + std::atomic last_read_version { 0 }; }; } \ No newline at end of file diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 0446f18..1147acc 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -29,6 +29,7 @@ #include "utils/fps_counter.hpp" // #include "utils/visualization.hpp" #include "v1/identifier/identifier.hpp" +#include namespace world_exe::tongji { using namespace std::chrono; @@ -37,11 +38,23 @@ class AutoAimSystem::Impl { public: explicit Impl(const bool& debug) : debug(debug) - , config_path_(std::filesystem::path { __FILE__ }.parent_path().parent_path().parent_path() - / "configs" - / "example." - "yaml") + , fps_() { + + std::string package_share_directory = ament_index_cpp::get_package_share_directory("allianc" + "e_" + "ros_" + "auto_" + "aim"); + + std::filesystem::path config_fs_path = + std::filesystem::path(package_share_directory) / "example.yaml"; + std::string model_path = config_fs_path.string(); + + std::filesystem::path model_fs_path = + std::filesystem::path(package_share_directory) / "szu_identify_model.onnx"; + config_path_ = config_fs_path.string(); + identifier_ = std::make_unique( parameters::ParamsForSystemV1::szu_model_path(), parameters::ParamsForSystemV1::device(), parameters::HikCameraProfile::get_width(), @@ -73,12 +86,14 @@ class AutoAimSystem::Impl { // cv::imshow("identified", visualized); // cv::waitKey(1); // } - // if (fps_.count()) std::cout << fps_.fps() << std::endl; + if (fps_.count()) std::cout << fps_.fps() << std::endl; if (flag == enumeration::ArmorIdFlag::None) { state_machine_->SetLostState(); - std::cout << "no armor identified!" << std::endl; + // std::cout << "no armor identified!" << std::endl; return; + } else { + // std::cout << "found!" << std::endl; } // TODO:update invincible_armors @@ -88,14 +103,14 @@ class AutoAimSystem::Impl { auto [pack, check] = syncer_->get_data(raw.stamp); if (!check) { // TODO：等待传入真实数据 - // pack.camera_capture_begin_time_stamp = - // data::TimeStamp(steady_clock::now().time_since_epoch()); + pack.camera_capture_begin_time_stamp = + data::TimeStamp(steady_clock::now().time_since_epoch()); // std::println(" no sync data"); return; } - const auto R_camera2gimbal = pack.camera_to_gimbal.rotation(); - const auto t_camera2gimbal = pack.camera_to_gimbal.translation(); + const auto& R_camera2gimbal = pack.camera_to_gimbal.rotation(); + const auto& t_camera2gimbal = pack.camera_to_gimbal.translation(); pnp_solver_->SetCamera2Gimbal(R_camera2gimbal, t_camera2gimbal); const auto& armors_in_camera = pnp_solver_->SolvePnp(armors_in_image); @@ -163,7 +178,7 @@ class AutoAimSystem::Impl { private: bool debug; - const std::string config_path_; + std::string config_path_; world_exe::util::FpsCounter fps_; data::TimeStamp time_stamp_; std::unique_ptr identifier_; diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index 57048d9..d4c6e64 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -76,8 +76,9 @@ class AimingSolver { armor_in_muzzle.id = armor_in_gimbal.id; armor_in_muzzle.position = transform_gimbal2muzzle * armor_in_gimbal.position; armor_in_muzzle.orientation = - Eigen::Quaterniond(transform_gimbal2muzzle.rotation()) - * armor_in_gimbal.orientation; + (Eigen::Quaterniond(transform_gimbal2muzzle.rotation()) + * armor_in_gimbal.orientation) + .normalized(); return armor_in_muzzle; }); diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp index 8bfb465..e7025af 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -71,18 +71,20 @@ class CarPredictorManager::Impl { if (!targets_map_.contains(armor.id)) { targets_map_.try_emplace(armor.id, std::make_unique(transform_camera2world * armor.position, - util::math::quaternion_to_euler( - Eigen::Quaterniond(transform_camera2world.rotation()) - * armor.orientation, - 2, 1, 0), + (util::math::quaternion_to_euler( + Eigen::Quaterniond(transform_camera2world.rotation()) + * armor.orientation, + 2, 1, 0)) + .normalized(), armor.id, data->GetTimeStamp())); } else { targets_map_.at(armor.id)->Update(data->GetTimeStamp(), transform_camera2world * armor.position, - util::math::quaternion_to_euler( - Eigen::Quaterniond(transform_camera2world.rotation()) - * armor.orientation, - 2, 1, 0), + (util::math::quaternion_to_euler( + Eigen::Quaterniond(transform_camera2world.rotation()) + * armor.orientation, + 2, 1, 0)) + .normalized(), util::math::xyz2ypd(transform_camera2world * armor.position)); } } diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index 7d26bc9..f4d8b08 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -1,6 +1,5 @@ #include "solver.hpp" -#include #include #include #include @@ -53,12 +52,6 @@ class Solver::Impl { t_camera2gimbal_ = t_camera2gimbal; } - auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> auto { - Eigen::Vector3d xyz_in_gimbal = - R_camera2gimbal_.transpose() * xyz_in_camera + t_camera2gimbal_; - return xyz_in_gimbal; - } - auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, const double& initial_armor_yaw_in_gimbal) const -> double { @@ -68,8 +61,9 @@ class Solver::Impl { auto min_error = 1e10; auto best_yaw = initial_armor_yaw_in_gimbal; - auto pitch = - (armor_in_image.id == enumeration::ArmorIdFlag::Outpost) ? -15.0 * CV_PI / 180.0 : 15.0; + auto pitch = (armor_in_image.id == enumeration::ArmorIdFlag::Outpost) + ? -15.0 * CV_PI / 180.0 + : 15.0 * CV_PI / 180.0; for (int i = 0; i < SEARCH_RANGE; i++) { double yaw = util::math::clamp_pm_pi(yaw0 + i * CV_PI / 180.0); @@ -156,7 +150,4 @@ auto Solver::CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, armor_in_image, armor_xyz_in_gimbal, gimbal_yaw, initial_armor_yaw_in_gimbal); } -auto Solver::Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> auto { - return pimpl_->Camera2Gimbal(xyz_in_camera); -} } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 3a9ec51..0df11d3 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -15,7 +15,6 @@ class Solver final : public interfaces::IPnpSolver { void SetCamera2Gimbal( const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal); - auto Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> auto; auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, const double& initial_armor_yaw_in_gimbal) const -> double; diff --git a/src/util/parameters/params_system_v1.cpp b/src/util/parameters/params_system_v1.cpp index e9b52fa..6ad4137 100644 --- a/src/util/parameters/params_system_v1.cpp +++ b/src/util/parameters/params_system_v1.cpp @@ -4,12 +4,22 @@ #include #include +#include + namespace world_exe::parameters { struct ParamsForSystemV1::Impl { public: - std::string model_path = - std::filesystem::path { __FILE__ }.parent_path().parent_path().parent_path().parent_path() - / "models" / "szu_identify_model.onnx"; + // std::string model_path = + // std::filesystem::path { __FILE__ + // }.parent_path().parent_path().parent_path().parent_path() / "models" / + // "szu_identify_model.onnx"; + + std::string package_share_directory = ament_index_cpp::get_package_share_directory("alliance_" + "ros_auto_" + "aim"); + std::filesystem::path model_fs_path = + std::filesystem::path(package_share_directory) / "szu_identify_model.onnx"; + std::string model_path = model_fs_path.string(); std::string device = "AUTO"; double control_delay_in_second = 0.05; diff --git a/src/util/visaulization.cpp b/src/util/visaulization.cpp index e8b62a8..9de243c 100644 --- a/src/util/visaulization.cpp +++ b/src/util/visaulization.cpp @@ -95,7 +95,8 @@ void world_exe::util::visualization::draw_armor_in_gimbal( world_exe::data::ArmorCameraSpacing armor { armor_gimbal.id, gimal_to_camera * armor_gimbal.position, Eigen::Quaterniond { - gimal_to_camera.rotation() * armor_gimbal.orientation.toRotationMatrix() } }; + gimal_to_camera.rotation() * armor_gimbal.orientation.toRotationMatrix() } + .normalized() }; world_exe::util::cast::armor_3d_camera_to_armor_2d_image(armor, intrinsic_parameters, distortion_parameters, points_in_armor_spacing, img_armor); draw_armor_2d(img_armor, in_out_mat); From 12f24acc8134831cde75dcd3f301d835a146b0ca Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Tue, 18 Nov 2025 19:52:35 +0800 Subject: [PATCH 11/14] fix(system): run bugs in predictor and synchronizer modules --- src/tongji/auto_aim_system.cpp | 24 ++++++----- src/tongji/fire_controller/aim_solver.hpp | 43 +++++++++---------- .../fire_controller/fire_controller.cpp | 11 +++++ .../fire_controller/fire_controller.hpp | 2 + .../predictor/car_predictor/car_predictor.hpp | 7 +-- .../car_predictor/car_predictor_manager.cpp | 38 ++++++++-------- .../predictor/kalman_filter/predict_model.hpp | 3 +- src/tongji/solver/solver.cpp | 34 ++++++++------- src/tongji/solver/solver.hpp | 3 +- src/util/math.hpp | 2 +- src/v1/identifier/identifier.cpp | 14 ++++-- src/v1/sync/syncer.cpp | 12 +++--- 12 files changed, 109 insertions(+), 84 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 1147acc..57084f2 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -10,6 +10,7 @@ #include #include // #include +#include #include #include "../v1/sync/syncer.hpp" @@ -86,7 +87,7 @@ class AutoAimSystem::Impl { // cv::imshow("identified", visualized); // cv::waitKey(1); // } - if (fps_.count()) std::cout << fps_.fps() << std::endl; + // if (fps_.count()) std::cout << fps_.fps() << std::endl; if (flag == enumeration::ArmorIdFlag::None) { state_machine_->SetLostState(); @@ -99,23 +100,22 @@ class AutoAimSystem::Impl { // TODO:update invincible_armors state_machine_->Update(armors_in_image, enumeration::CarIDFlag::None, time_stamp_); + std::cout << "img stamp:" << raw.stamp.to_nanosec() << std::endl; // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 auto [pack, check] = syncer_->get_data(raw.stamp); if (!check) { // TODO：等待传入真实数据 - pack.camera_capture_begin_time_stamp = - data::TimeStamp(steady_clock::now().time_since_epoch()); - // std::println(" no sync data"); + // pack.camera_capture_begin_time_stamp = + // data::TimeStamp(steady_clock::now().time_since_epoch()); + std::cout << " no sync data" << std::endl; return; } - const auto& R_camera2gimbal = pack.camera_to_gimbal.rotation(); - const auto& t_camera2gimbal = pack.camera_to_gimbal.translation(); - - pnp_solver_->SetCamera2Gimbal(R_camera2gimbal, t_camera2gimbal); + pnp_solver_->SetCamera2Gimbal(pack.camera_to_gimbal); const auto& armors_in_camera = pnp_solver_->SolvePnp(armors_in_image); auto combined = std::make_shared(pack, armors_in_camera); + live_target_manager_->Update(combined); core::EventBus::Publish>( @@ -134,6 +134,9 @@ class AutoAimSystem::Impl { if (!debug) [[likely]] return; + auto target = state_machine_->GetAllowdToFires(); + // std::cout << "target:" << util::stringifier::ToString(target) << std::endl; + core::EventBus::Publish( parameters::ParamsForSystemV1::car_id_identify_event, flag); core::EventBus::Publish>( @@ -144,7 +147,9 @@ class AutoAimSystem::Impl { parameters::ParamsForSystemV1::tracker_update_event, combined); core::EventBus::Publish( parameters::ParamsForSystemV1::car_tracing_event, state_machine_->GetAllowdToFires()); - + core::EventBus::Publish>( + parameters::ParamsForSystemV1::get_lastest_predictor_event, + fire_controller_->GetArmorsSnapshot()); // if (armors_in_image) { // auto visualized = raw.mat.clone(); // util::visualization::draw_armor_in_image(*armors_in_image, visualized); @@ -171,7 +176,6 @@ class AutoAimSystem::Impl { // TODO:时间戳有待fix data::FireControl GetControlCommand() { auto attacked_id = fire_controller_->GetAttackCarId(); - return fire_controller_->CalculateTarget( data::TimeStamp(steady_clock::now().time_since_epoch())); } diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index d4c6e64..a5de67a 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -13,8 +13,8 @@ #include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.hpp" -#include "data/armor_gimbal_control_spacing.hpp" #include "data/time_stamped.hpp" +#include "interfaces/armor_in_gimbal_control.hpp" #include "tongji/predictor/kalman_filter/extended_kalman_filter.hpp" #include "tongji/predictor/kalman_filter/predict_model.hpp" #include "trajectory.hpp" @@ -68,32 +68,23 @@ class AimingSolver { const auto& armors_in_gimbal = armors->GetArmors(snapshot->GetId()); - auto armors_in_muzzle = armors_in_gimbal - | std::ranges::views::transform([&transform_gimbal2muzzle]( - auto const& armor_in_gimbal) { - data::ArmorGimbalControlSpacing armor_in_muzzle; - - armor_in_muzzle.id = armor_in_gimbal.id; - armor_in_muzzle.position = transform_gimbal2muzzle * armor_in_gimbal.position; - armor_in_muzzle.orientation = - (Eigen::Quaterniond(transform_gimbal2muzzle.rotation()) - * armor_in_gimbal.orientation) - .normalized(); - - return armor_in_muzzle; - }); + armors_view_ = std::make_shared( + armors_in_gimbal, time_stamp + data::TimeStamp::from_seconds(dt)); const auto& aim_point = SelectPredictedAim( - snapshot_derived->GetPredictedX(time_stamp), armors_in_muzzle, snapshot->GetId()); + snapshot_derived->GetPredictedX(time_stamp), armors_in_gimbal, snapshot->GetId()); if (!aim_point.has_value()) { continue; - } // failed: no valid aim point - const auto traj = SolveTrajectory(aim_point.value(), bullet_speed_); + auto fire_origin = -transform_gimbal2muzzle.linear().transpose() + * transform_gimbal2muzzle.translation(); + + auto aim_vector = *aim_point - fire_origin; + + const auto traj = SolveTrajectory(aim_vector, bullet_speed_); if (!traj.has_value()) { - // std::println("trajectory unsolvable"); continue; } @@ -119,7 +110,13 @@ class AimingSolver { return { true, yaw, pitch, final_aim_point }; } + std ::shared_ptr GetArmorsSnapshot() { + return armors_view_; + } + private: + std::shared_ptr armors_view_; + template std::optional SelectPredictedAim( const EKF::XVec& ekf_x, const T& armors, const CarIDFlag& id) const { @@ -131,12 +128,12 @@ class AimingSolver { } 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_); + const Eigen::Vector3d& vec, const double& bullet_speed) const { + double d = std::hypot(vec.x(), vec.y()); + auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, vec.z(), g_); if (!result.solvable) { - std::cout << "solve trajectory failed: d=" << d << " z=" << xyz.z() + std::cout << "solve trajectory failed: d=" << d << " z=" << vec.z() << "speed=" << bullet_speed << std::endl; } diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index ee03e12..4043969 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -43,6 +44,7 @@ class FireController::Impl { .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; // TODO:这里不应该指针转换 + std::println("calculate time:{}", time_stamp.to_seconds()); const auto& aim_solution = aiming_solver_->SolveAimSolution( snapshot_manager, transform_gimbal2muzzle_, time_stamp, control_delay_); @@ -72,6 +74,10 @@ class FireController::Impl { transform_gimbal2muzzle_ = transform_gimbal2muzzle; } + std ::shared_ptr GetArmorsSnapshot() { + return aiming_solver_->GetArmorsSnapshot(); + } + private: std::chrono::milliseconds control_delay_ { 100 }; @@ -97,4 +103,9 @@ CarIDFlag FireController::GetAttackCarId() const { return pimpl_->GetAttackCarId void FireController::SetGimbal2Muzzle(Eigen::Affine3d const& transform_gimbal2muzzle) { return pimpl_->SetGimbal2Muzzle(transform_gimbal2muzzle); } + +std ::shared_ptr FireController::GetArmorsSnapshot() { + return pimpl_->GetArmorsSnapshot(); +} + } diff --git a/src/tongji/fire_controller/fire_controller.hpp b/src/tongji/fire_controller/fire_controller.hpp index 1cb7fea..3daeb1b 100644 --- a/src/tongji/fire_controller/fire_controller.hpp +++ b/src/tongji/fire_controller/fire_controller.hpp @@ -25,6 +25,8 @@ class FireController final : public interfaces::IFireControl { FireController(FireController&&) noexcept = default; FireController& operator=(FireController&&) noexcept = default; + std ::shared_ptr GetArmorsSnapshot(); + private: class Impl; std::unique_ptr pimpl_; diff --git a/src/tongji/predictor/car_predictor/car_predictor.hpp b/src/tongji/predictor/car_predictor/car_predictor.hpp index 197f48b..b9c6cee 100644 --- a/src/tongji/predictor/car_predictor/car_predictor.hpp +++ b/src/tongji/predictor/car_predictor/car_predictor.hpp @@ -64,9 +64,10 @@ class CarPredictor final : public interfaces::IPredictor { auto xyz = model_.h_armor_xyz(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); + armor.id = model_.GetID(); + armor.position = xyz; + armor.orientation = + util::math::euler_to_quaternion(angle, 15. / 180. * CV_PI, 0).normalized(); armors.emplace_back(std::move(armor)); } return std::make_shared(armors, time_stamp); diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp index e7025af..182e54c 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -1,5 +1,6 @@ #include "car_predictor_manager.hpp" +#include #include #include #include @@ -7,7 +8,6 @@ #include #include "../in_gimbal_control_armor.hpp" - #include "car_predictor.hpp" #include "data/predictor_update_package.hpp" #include "data/time_stamped.hpp" @@ -52,13 +52,14 @@ class CarPredictorManager::Impl { const auto& it = targets_map_.find(flag); if (it == targets_map_.end()) return nullptr; const auto& [id, predictor] = *it; + return std::make_shared( predictor->GetEkf(), predictor->GetModel(), data::TimeStamp { last_update_timestamp_ }); } void Update(std::shared_ptr const& data) { - Eigen::Affine3d transform_camera2world = data->GetCameraToWorld(); - const auto armors_interface_incamera = data->GetArmors(); + Eigen::Affine3d transform_camera2gimbal = data->GetCameraToWorld(); + const auto armors_interface_incamera = data->GetArmors(); for (int i = 0; i < 8; i++) { auto id = static_cast( @@ -68,24 +69,27 @@ class CarPredictorManager::Impl { if (armors_in_camera.empty()) continue; for (const auto& armor : armors_in_camera) { + auto xyz_in_gimbal = transform_camera2gimbal * armor.position; + if (!targets_map_.contains(armor.id)) { targets_map_.try_emplace(armor.id, - std::make_unique(transform_camera2world * armor.position, - (util::math::quaternion_to_euler( - Eigen::Quaterniond(transform_camera2world.rotation()) - * armor.orientation, - 2, 1, 0)) - .normalized(), + std::make_unique(xyz_in_gimbal, + util::math::quaternion_to_euler( + Eigen::Quaterniond( + (Eigen::Quaterniond(transform_camera2gimbal.rotation()) + * armor.orientation)) + .normalized(), + 2, 1, 0), armor.id, data->GetTimeStamp())); } else { - targets_map_.at(armor.id)->Update(data->GetTimeStamp(), - transform_camera2world * armor.position, - (util::math::quaternion_to_euler( - Eigen::Quaterniond(transform_camera2world.rotation()) - * armor.orientation, - 2, 1, 0)) - .normalized(), - util::math::xyz2ypd(transform_camera2world * armor.position)); + targets_map_.at(armor.id)->Update(data->GetTimeStamp(), xyz_in_gimbal, + util::math::quaternion_to_euler( + Eigen::Quaterniond( + (Eigen::Quaterniond(transform_camera2gimbal.rotation()) + * armor.orientation)) + .normalized(), + 2, 1, 0), + util::math::xyz2ypd(transform_camera2gimbal * armor.position)); } } diff --git a/src/tongji/predictor/kalman_filter/predict_model.hpp b/src/tongji/predictor/kalman_filter/predict_model.hpp index 6e198fe..db4f753 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -114,7 +114,6 @@ class EKFModel { auto MatchArmor(const XVec& x, const Eigen::Vector3d& armor_xyz_in_gimbal, const Eigen::Vector3d& armor_ypr_in_gimbal, const Eigen::Vector3d& armor_ypd_in_gimbal) const -> int { - const auto& xyza_list = GetArmorXYZAList(x); std::vector> xyza_i_list; @@ -131,7 +130,7 @@ class EKFModel { int best_id = 0; double min_error = 1e10; - for (int i = 0; i < std::max(3, armor_num_); ++i) { + for (int i = 0; i < 3; ++i) { const auto& xyza = xyza_i_list[i].first; auto ypd = util::math::xyz2ypd(xyza.head(3)); double error = std::abs(util::math::clamp_pm_pi(armor_ypr_in_gimbal(0) - xyza(3))) diff --git a/src/tongji/solver/solver.cpp b/src/tongji/solver/solver.cpp index f4d8b08..44bc170 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -1,5 +1,6 @@ #include "solver.hpp" +#include #include #include #include @@ -26,9 +27,7 @@ namespace world_exe::tongji::solver { class Solver::Impl { public: explicit Impl() - : R_camera2gimbal_(Eigen::Matrix3d::Zero()) - , t_camera2gimbal_(Eigen::Vector3d::Zero()) - , reprojection_util_(std::make_unique()) { } + : reprojection_util_(std::make_unique()) { } static std::shared_ptr EstimateAllArmorPoses( std::shared_ptr const& armors_in_image) { @@ -46,10 +45,8 @@ class Solver::Impl { return std::make_shared(armor_plates, armors_in_image->GetTimeStamp()); } - auto SetCamera2Gimbal( - const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal) -> void { - R_camera2gimbal_ = R_camera2gimbal; - t_camera2gimbal_ = t_camera2gimbal; + auto SetCamera2Gimbal(Eigen::Affine3d const& transform_camera2gimbal) -> void { + transform_camera2gimbal_ = transform_camera2gimbal; } auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, @@ -68,9 +65,10 @@ class Solver::Impl { for (int i = 0; i < SEARCH_RANGE; i++) { double yaw = util::math::clamp_pm_pi(yaw0 + i * CV_PI / 180.0); - auto error = reprojection_util_->CalculateReprojectionError(R_camera2gimbal_, - t_camera2gimbal_, armor_in_image, armor_xyz_in_gimbal, yaw, pitch, - (i - SEARCH_RANGE / 2) * CV_PI / 180.0); + auto error = + reprojection_util_->CalculateReprojectionError(transform_camera2gimbal_.rotation(), + transform_camera2gimbal_.translation(), armor_in_image, armor_xyz_in_gimbal, + yaw, pitch, (i - SEARCH_RANGE / 2) * CV_PI / 180.0); if (error < min_error) { min_error = error; @@ -88,6 +86,13 @@ class Solver::Impl { static data::ArmorCameraSpacing EstimatePose( const world_exe::data::ArmorImageSpacing& armor_in_image) { const auto& [xyz_in_camera, R_armor2camera] = EstimatePnp(armor_in_image); + // const auto& armor_xyz_in_gimbal = transform_camera2gimbal_ * xyz_in_camera; + + // auto& gimbal_yaw = transform_gimbal2muzzle_.rotation().eulerAngles(2, 1, 0)[0]; + // auto armor_initial_yaw = util::math::matrix_to_euler( + // transform_camera2gimbal_.rotation() * R_armor2camera, 2, 1, 0)[0]; + // auto armor_yaw_optimized = CalculateOptimizeYaw( + // armor_in_image, armor_xyz_in_gimbal, gimbal_yaw, armor_initial_yaw); data::ArmorCameraSpacing pose; pose.id = armor_in_image.id; @@ -124,8 +129,8 @@ class Solver::Impl { } private: - Eigen::Matrix3d R_camera2gimbal_; - Eigen::Vector3d t_camera2gimbal_; + Eigen::Affine3d transform_camera2gimbal_ { Eigen::Affine3d::Identity() }; + // Eigen::Affine3d transform_gimbal2muzzle_ { Eigen::Affine3d::Identity() }; std::unique_ptr reprojection_util_; }; @@ -138,9 +143,8 @@ std::shared_ptr Solver::SolvePnp( std::shared_ptr armors_in_image) { return pimpl_->EstimateAllArmorPoses(armors_in_image); } -void Solver::SetCamera2Gimbal( - const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal) { - pimpl_->SetCamera2Gimbal(R_camera2gimbal, t_camera2gimbal); +void Solver::SetCamera2Gimbal(Eigen::Affine3d const& transform_camera2gimbal) { + pimpl_->SetCamera2Gimbal(transform_camera2gimbal); } auto Solver::CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 0df11d3..6844c5c 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -12,8 +12,7 @@ class Solver final : public interfaces::IPnpSolver { std::shared_ptr SolvePnp( std::shared_ptr armors) override; - void SetCamera2Gimbal( - const Eigen::Matrix3d& R_camera2gimbal, const Eigen::Vector3d& t_camera2gimbal); + void SetCamera2Gimbal(Eigen::Affine3d const& transform_camera2gimbal); auto CalculateOptimizeYaw(const data::ArmorImageSpacing& armor_in_image, const Eigen::Vector3d& armor_xyz_in_gimbal, const double& gimbal_yaw, diff --git a/src/util/math.hpp b/src/util/math.hpp index c825b15..2d9716a 100644 --- a/src/util/math.hpp +++ b/src/util/math.hpp @@ -158,7 +158,7 @@ static Eigen::Vector3d quaternion_to_euler( } static Eigen::Vector3d matrix_to_euler( - Eigen::Matrix3d R, int axis0, int axis1, int axis2, bool extrinsic = false) { + Eigen::Matrix3d const &R, int axis0, int axis1, int axis2, bool extrinsic = false) { Eigen::Quaterniond q(R); return quaternion_to_euler(q, axis0, axis1, axis2, extrinsic); } diff --git a/src/v1/identifier/identifier.cpp b/src/v1/identifier/identifier.cpp index 6343c81..da3f8d1 100644 --- a/src/v1/identifier/identifier.cpp +++ b/src/v1/identifier/identifier.cpp @@ -8,6 +8,7 @@ */ #include "identifier.hpp" +#include "enum/armor_id.hpp" #include "identifier_armor.hpp" #include "util/scanline.hpp" @@ -325,18 +326,23 @@ class Identifier::Impl { // 如果找到了两个灯条，构建装甲板 if (light_bar_size_ == 2) { - const auto& first = lightbars_[0]; - const auto& second = lightbars_[1]; + const auto& first = lightbars_[0]; + const auto& second = lightbars_[1]; + bool is_large_armor = (armor.id_ == enumeration::ArmorIdFlag::Base + || armor.id_ == enumeration::ArmorIdFlag::Hero); // 根据 x 坐标排序灯条，确保左灯条在前 if ((first.top_.x + first.bottom_.x) / 2. < (second.bottom_.x + second.top_.x) / 2.) { // 构建装甲板的四个角点：左上、右上、右下、左下 + armor_plates.emplace_back(data::ArmorImageSpacing { armor.id_, - { first.top_, second.top_, second.bottom_, first.bottom_ } }); + { first.top_, second.top_, second.bottom_, first.bottom_ }, + is_large_armor }); } else { armor_plates.emplace_back(data::ArmorImageSpacing { armor.id_, - { second.top_, first.top_, first.bottom_, second.bottom_ } }); + { second.top_, first.top_, first.bottom_, second.bottom_ }, + is_large_armor }); } // 将装甲板类型添加到检测到的车辆类型标志位中 all_car_id |= static_cast(armor.id_); diff --git a/src/v1/sync/syncer.cpp b/src/v1/sync/syncer.cpp index 1277403..3eec6ac 100644 --- a/src/v1/sync/syncer.cpp +++ b/src/v1/sync/syncer.cpp @@ -30,8 +30,7 @@ struct Syncer::Impl { std::tuple get_data(const data::TimeStamp& timestamp) { - if (data_queue_.empty()) - return {data::CameraGimbalMuzzleSyncData{}, false}; + if (data_queue_.empty()) return { data::CameraGimbalMuzzleSyncData { }, false }; for(auto rit = data_queue_.rbegin(); rit != data_queue_.rend(); ++rit) if (rit->camera_capture_begin_time_stamp <= timestamp) @@ -43,7 +42,6 @@ struct Syncer::Impl { ~Impl() = default; private: - std::list data_queue_; const std::chrono::seconds time_to_hold_; @@ -55,12 +53,12 @@ Syncer::Syncer(std::chrono::seconds time_to_hold, long tolerable_ns) Syncer::~Syncer() = default; -void Syncer::set_data(const data::CameraGimbalMuzzleSyncData& armor_pnp) { - pimpl_->set_data(armor_pnp); +void Syncer::set_data(const data::CameraGimbalMuzzleSyncData& data) { + pimpl_->set_data(data); } -std::tuple Syncer::get_data(const data::TimeStamp& timestamp) { +std::tuple Syncer::get_data( + const data::TimeStamp& timestamp) { return pimpl_->get_data(timestamp); } - } \ No newline at end of file From f63217939a2ca1605a171ee408082ca24d851ff9 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Wed, 19 Nov 2025 18:28:41 +0800 Subject: [PATCH 12/14] feat(fire_control): implement fire command validation and communication --- src/tongji/auto_aim_system.cpp | 3 +-- src/tongji/fire_controller/aim_solver.hpp | 10 +++++----- src/tongji/fire_controller/fire_controller.cpp | 12 +++++++++--- 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 57084f2..536e170 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -100,7 +100,7 @@ class AutoAimSystem::Impl { // TODO:update invincible_armors state_machine_->Update(armors_in_image, enumeration::CarIDFlag::None, time_stamp_); - std::cout << "img stamp:" << raw.stamp.to_nanosec() << std::endl; + // std::cout << "img stamp:" << raw.stamp.to_nanosec() << std::endl; // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 auto [pack, check] = syncer_->get_data(raw.stamp); if (!check) { @@ -135,7 +135,6 @@ class AutoAimSystem::Impl { return; auto target = state_machine_->GetAllowdToFires(); - // std::cout << "target:" << util::stringifier::ToString(target) << std::endl; core::EventBus::Publish( parameters::ParamsForSystemV1::car_id_identify_event, flag); diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index a5de67a..4f3fb2c 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -54,6 +54,9 @@ class AimingSolver { Eigen::Vector3d final_aim_point; TrajectoryResult final_trajectory; bool converged = false; + auto fire_origin = + -transform_gimbal2muzzle.linear().transpose() * transform_gimbal2muzzle.translation(); + // HACK:不同击打点影响飞行时间的迭代，需要根据整车的状态（转速和坐标）来选择击打点，不得已将指针转换为派生类 auto snapshot_derived = std::dynamic_pointer_cast(snapshot); if (!snapshot_derived) @@ -78,9 +81,6 @@ class AimingSolver { continue; } // failed: no valid aim point - auto fire_origin = -transform_gimbal2muzzle.linear().transpose() - * transform_gimbal2muzzle.translation(); - auto aim_vector = *aim_point - fire_origin; const auto traj = SolveTrajectory(aim_vector, bullet_speed_); @@ -103,8 +103,8 @@ class AimingSolver { 0 }; // failed: trajectory did not converge } - const auto xyz = final_aim_point; - const double yaw = std::atan2(xyz.y(), xyz.x()) + yaw_offset_; + const auto vec = final_aim_point - fire_origin; + const double yaw = std::atan2(vec.y(), vec.x()) + yaw_offset_;//TODO const double pitch = (final_trajectory.pitch + pitch_offset_); return { true, yaw, pitch, final_aim_point }; diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 4043969..ba1cdbf 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -30,7 +30,8 @@ class FireController::Impl { std::shared_ptr live_target_manager) : aiming_solver_(std::make_unique(config_path)) , state_machine_(std::move(state_machine)) - , live_target_manager_(std::move(live_target_manager)) { } + , live_target_manager_(std::move(live_target_manager)) + , fire_decision_(std::make_unique(config_path)) { } data ::FireControl CalculateTarget(data::TimeStamp const& time_stamp) const { if (!state_machine_ || !live_target_manager_) return { .fire_allowance = false }; @@ -44,7 +45,7 @@ class FireController::Impl { .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; // TODO:这里不应该指针转换 - std::println("calculate time:{}", time_stamp.to_seconds()); + // std::println("calculate time:{}", time_stamp.to_seconds()); const auto& aim_solution = aiming_solver_->SolveAimSolution( snapshot_manager, transform_gimbal2muzzle_, time_stamp, control_delay_); @@ -57,8 +58,12 @@ class FireController::Impl { const auto gimbal_command = GimbalCommand { aim_solution.yaw, aim_solution.pitch }; const auto target_pos = Eigen::Vector3d { aim_solution.aim_point }; + auto gimbal_yaw = transform_gimbal2muzzle_.inverse().rotation().eulerAngles(2, 1, 0)(0); + + auto fire_valid = fire_decision_->ShouldFire(gimbal_yaw, gimbal_command, target_pos); + data::FireControl result; - result.fire_allowance = aim_solution.valid; + result.fire_allowance = fire_valid; result.gimbal_dir << cos(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.pitch); result.time_stamp = time_stamp; @@ -87,6 +92,7 @@ class FireController::Impl { std::unique_ptr aiming_solver_; std::shared_ptr state_machine_; std::shared_ptr live_target_manager_; + std::unique_ptr fire_decision_; }; FireController::FireController(const std::string& config_path, From 30bde77c484941c70806b5fb1f7a226b9c9887b8 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 20 Nov 2025 21:33:00 +0800 Subject: [PATCH 13/14] fix(gimbal): verify gimbal communication functionality --- configs/example.yaml | 2 +- src/tongji/auto_aim_system.cpp | 5 ----- src/tongji/fire_controller/aim_solver.hpp | 17 +++++++++-------- src/tongji/fire_controller/fire_controller.cpp | 15 +++++++++++---- src/tongji/fire_controller/fire_decision.hpp | 1 + src/tongji/solver/reprojection_util.hpp | 4 ++-- src/util/cast.cpp | 8 +++----- src/util/coordinate.hpp | 8 ++++++-- 8 files changed, 33 insertions(+), 27 deletions(-) diff --git a/configs/example.yaml b/configs/example.yaml index 3683855..81f665c 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -29,7 +29,7 @@ judge_distance: 2 #距离判断阈值 #####-----aiming_solver parameters-----##### yaw_offset: 1.5 # degree pitch_offset: -0.5 # degree -bullet_speed: 20 +bullet_speed: 28 #####-----aime_point_chooser parameters-----##### comming_angle: 60 # degree diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 536e170..97b4349 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -60,8 +60,6 @@ class AutoAimSystem::Impl { parameters::ParamsForSystemV1::szu_model_path(), parameters::ParamsForSystemV1::device(), parameters::HikCameraProfile::get_width(), parameters::HikCameraProfile::get_height()); - // identifier_ = std::make_unique(config_path_, - // "."); pnp_solver_ = std::make_unique(); live_target_manager_ = std::make_shared(config_path_); state_machine_ = std::make_shared(); @@ -100,13 +98,10 @@ class AutoAimSystem::Impl { // TODO:update invincible_armors state_machine_->Update(armors_in_image, enumeration::CarIDFlag::None, time_stamp_); - // std::cout << "img stamp:" << raw.stamp.to_nanosec() << std::endl; // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 auto [pack, check] = syncer_->get_data(raw.stamp); if (!check) { // TODO：等待传入真实数据 - // pack.camera_capture_begin_time_stamp = - // data::TimeStamp(steady_clock::now().time_since_epoch()); std::cout << " no sync data" << std::endl; return; } diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index 4f3fb2c..bbabe6d 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -53,9 +53,8 @@ class AimingSolver { double prev_fly_time_s = 0; Eigen::Vector3d final_aim_point; TrajectoryResult final_trajectory; - bool converged = false; - auto fire_origin = - -transform_gimbal2muzzle.linear().transpose() * transform_gimbal2muzzle.translation(); + bool converged = false; + auto fire_origin = transform_gimbal2muzzle.inverse().translation(); // HACK:不同击打点影响飞行时间的迭代，需要根据整车的状态（转速和坐标）来选择击打点，不得已将指针转换为派生类 auto snapshot_derived = std::dynamic_pointer_cast(snapshot); @@ -82,6 +81,10 @@ class AimingSolver { } // failed: no valid aim point auto aim_vector = *aim_point - fire_origin; + // std::cout << "aim_vector:(" << aim_vector.x() << "," << aim_vector.y() << "," + // << aim_vector.z() << ")" << " fire_origin:(" << fire_origin.x() << "," + // << fire_origin.y() << "," + // << fire_origin.z() << ")" << std::endl; const auto traj = SolveTrajectory(aim_vector, bullet_speed_); if (!traj.has_value()) { @@ -98,15 +101,13 @@ class AimingSolver { } if (!converged) { - return { false, std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), { }, - 0 }; // failed: trajectory did not converge + std::cout << "trajectory did not converge" << std::endl; + return { false, { }, 0 }; // failed: trajectory did not converge } const auto vec = final_aim_point - fire_origin; - const double yaw = std::atan2(vec.y(), vec.x()) + yaw_offset_;//TODO + const double yaw = (std::atan2(vec.y(), vec.x()) + yaw_offset_); const double pitch = (final_trajectory.pitch + pitch_offset_); - return { true, yaw, pitch, final_aim_point }; } diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index ba1cdbf..3c1554c 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -4,7 +4,6 @@ #include #include -#include #include #include @@ -58,14 +57,22 @@ class FireController::Impl { const auto gimbal_command = GimbalCommand { aim_solution.yaw, aim_solution.pitch }; const auto target_pos = Eigen::Vector3d { aim_solution.aim_point }; - auto gimbal_yaw = transform_gimbal2muzzle_.inverse().rotation().eulerAngles(2, 1, 0)(0); + auto ypr = transform_gimbal2muzzle_.inverse().rotation().eulerAngles(2, 1, 0); + auto gimbal_yaw = ypr(0); + // auto gimbal_pitch = ypr(1); + // auto gimbal_roll = ypr(2); auto fire_valid = fire_decision_->ShouldFire(gimbal_yaw, gimbal_command, target_pos); + + if (!fire_valid) { + std::cout << "forbid fire" << std::endl; + } data::FireControl result; result.fire_allowance = fire_valid; - result.gimbal_dir << cos(gimbal_command.yaw) * cos(gimbal_command.pitch), - sin(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.pitch); + // result.gimbal_dir << cos(gimbal_command.yaw) * cos(gimbal_command.pitch), + // sin(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.pitch); + result.gimbal_dir << target_pos.normalized(); // TODO:not gimbal dir result.time_stamp = time_stamp; return result; } diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 39125c5..a696ce3 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -40,6 +40,7 @@ class FireDecision { last_gimbal_command_ = gimbal_command; return true; } + last_gimbal_command_ = gimbal_command; return false; } diff --git a/src/tongji/solver/reprojection_util.hpp b/src/tongji/solver/reprojection_util.hpp index 5d97812..b3c97d8 100644 --- a/src/tongji/solver/reprojection_util.hpp +++ b/src/tongji/solver/reprojection_util.hpp @@ -76,8 +76,8 @@ class ReprojectionUtil { std::vector image_points; const auto& object_points = (is_large) - ? parameters::Robomaster::LargeArmorObjectPointsOpencv - : parameters::Robomaster::NormalArmorObjectPointsOpencv; + ? parameters::Robomaster::LargeArmorObjectPointsRos + : parameters::Robomaster::NormalArmorObjectPointsRos; cv::projectPoints(object_points, rvec, tvec, parameters::HikCameraProfile::get_intrinsic_parameters(), parameters::HikCameraProfile::get_distortion_parameters(), image_points); diff --git a/src/util/cast.cpp b/src/util/cast.cpp index 35923f8..020e3b8 100644 --- a/src/util/cast.cpp +++ b/src/util/cast.cpp @@ -17,11 +17,9 @@ void world_exe::util::cast ::armor_3d_camera_to_armor_2d_image( data::ArmorImageSpacing& out_armor_2d) { out_armor_2d.id = armor3d.id; - out_armor_2d.isLargeArmor = (static_cast(armor3d.id) - & (static_cast(enumeration::ArmorIdFlag::Hero) - | static_cast(enumeration::ArmorIdFlag::Base))) - != 0; - std::vector point3ds {}; + out_armor_2d.isLargeArmor = (armor3d.id == enumeration::ArmorIdFlag::Base + || armor3d.id == enumeration::ArmorIdFlag::Hero); + std::vector point3ds { }; for (const auto& point : points_in_armor_spacing) { Eigen::Vector3d point_eigen { point.x, point.y, point.z }; diff --git a/src/util/coordinate.hpp b/src/util/coordinate.hpp index 4f632e0..c3b2554 100644 --- a/src/util/coordinate.hpp +++ b/src/util/coordinate.hpp @@ -11,7 +11,9 @@ static inline Eigen::Vector3d opencv2ros_position(const Eigen::Vector3d& positio // clangd-format off static inline Eigen::Matrix3d opencv2ros_rotation(const Eigen::Matrix3d& rotation) { Eigen::Matrix3d t; - t << 0, 0, 1, -1, 0, 0, 0, -1, 0; + t << 0, 0, 1, // + -1, 0, 0, // + 0, -1, 0; // return t * rotation * t.transpose(); } // clangd-format on @@ -23,7 +25,9 @@ static inline Eigen::Vector3d ros2opencv_position(const Eigen::Vector3d& positio static inline Eigen::Matrix3d ros2opencv_rotation(const Eigen::Matrix3d& rotation) { // clangd-format off Eigen::Matrix3d t; - t << 0, 0, 1, -1, 0, 0, 0, -1, 0; + t << 0, 0, 1, // + -1, 0, 0, // + 0, -1, 0; // // clangd-format on return t.transpose() * rotation * t; } From 7023e535bdb2ea6d8e5859758c1dbae46624fcbb Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Mon, 24 Nov 2025 00:03:22 +0800 Subject: [PATCH 14/14] fix(fire_control): repair vector calculations Correct vector operations and transformations in fire control system. --- configs/example.yaml | 6 +-- src/tongji/auto_aim_system.cpp | 17 +++++++-- src/tongji/fire_controller/aim_solver.hpp | 15 ++++---- .../fire_controller/fire_controller.cpp | 38 ++++++++++++------- src/tongji/fire_controller/fire_decision.hpp | 3 ++ src/tongji/fire_controller/trajectory.hpp | 7 +++- 6 files changed, 58 insertions(+), 28 deletions(-) diff --git a/configs/example.yaml b/configs/example.yaml index 81f665c..b5a83e5 100644 --- a/configs/example.yaml +++ b/configs/example.yaml @@ -27,9 +27,9 @@ second_tolerance: 100 # 远距离射击容差，degree judge_distance: 2 #距离判断阈值 #####-----aiming_solver parameters-----##### -yaw_offset: 1.5 # degree -pitch_offset: -0.5 # degree -bullet_speed: 28 +yaw_offset: -2.0 # degree +pitch_offset: -0.0 # degree +bullet_speed: 23 #####-----aime_point_chooser parameters-----##### comming_angle: 60 # degree diff --git a/src/tongji/auto_aim_system.cpp b/src/tongji/auto_aim_system.cpp index 97b4349..ecb356b 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -19,6 +19,7 @@ #include "data/predictor_update_package.hpp" #include "data/sync_data.hpp" #include "data/time_stamped.hpp" +#include "enum/armor_id.hpp" #include "enum/car_id.hpp" #include "interfaces/armor_in_gimbal_control.hpp" #include "parameters/params_system_v1.hpp" @@ -78,14 +79,15 @@ class AutoAimSystem::Impl { auto Solve(const data::MatStamped& raw) -> void { if (identifier_ == nullptr) std::terminate(); const auto& [armors_in_image, flag] = identifier_->identify(raw.mat); + // auto& [armors_in_image, flag] = identifier_->identify(raw.mat); // if (armors_in_image) { // auto visualized = raw.mat.clone(); - // util::visualization::draw_armor_in_image(*armors_in_image, visualized); + // // util::visualization::draw_armor_in_image(*armors_in_image, visualized); // cv::imshow("identified", visualized); // cv::waitKey(1); // } - // if (fps_.count()) std::cout << fps_.fps() << std::endl; + if (fps_.count()) std::cout << fps_.fps() << std::endl; if (flag == enumeration::ArmorIdFlag::None) { state_machine_->SetLostState(); @@ -100,6 +102,12 @@ class AutoAimSystem::Impl { // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分 auto [pack, check] = syncer_->get_data(raw.stamp); + // auto ypr = pack.gimbal_to_muzzle.inverse().rotation().eulerAngles(2, 1, 0); + // auto gimbal_yaw = ypr(0); + // auto gimbal_pitch = ypr(1); + // auto gimbal_roll = ypr(2); + // std::cout << "transform_yaw:" << gimbal_yaw << std::endl; + // return; if (!check) { // TODO：等待传入真实数据 std::cout << " no sync data" << std::endl; @@ -115,7 +123,9 @@ class AutoAimSystem::Impl { core::EventBus::Publish>( parameters::ParamsForSystemV1::tracker_current_armors_event, - live_target_manager_->Predict(flag, pack.camera_capture_begin_time_stamp)); + live_target_manager_->Predict( + enumeration::ArmorIdFlag::None, pack.camera_capture_begin_time_stamp)); + // live_target_manager_->Predict(flag, pack.camera_capture_begin_time_stamp)); time_stamp_ = pack.camera_capture_begin_time_stamp; @@ -123,6 +133,7 @@ class AutoAimSystem::Impl { /// 轨迹规划器没有实现，先不管 fire_controller_->SetGimbal2Muzzle(pack.gimbal_to_muzzle); + core::EventBus::Publish( parameters::ParamsForSystemV1::fire_control_event, GetControlCommand()); diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index bbabe6d..de38c9a 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -108,6 +108,10 @@ class AimingSolver { const auto vec = final_aim_point - fire_origin; const double yaw = (std::atan2(vec.y(), vec.x()) + yaw_offset_); const double pitch = (final_trajectory.pitch + pitch_offset_); + // std::cout << "aim point:(" << final_aim_point.x() << "," << final_aim_point.y() << "," + // << final_aim_point.z() << ") fire_origin:(" << fire_origin.x() << "," + // << fire_origin.y() << "," << fire_origin.z() << ")" << std::endl; + // TODO:not really ypr return { true, yaw, pitch, final_aim_point }; } @@ -130,14 +134,9 @@ class AimingSolver { std::optional SolveTrajectory( const Eigen::Vector3d& vec, const double& bullet_speed) const { - double d = std::hypot(vec.x(), vec.y()); - auto result = TrajectorySolver::SolveTrajectory(bullet_speed, d, vec.z(), g_); - - if (!result.solvable) { - std::cout << "solve trajectory failed: d=" << d << " z=" << vec.z() - << "speed=" << bullet_speed << std::endl; - } - + + auto result = TrajectorySolver::SolveTrajectory(bullet_speed, vec, g_); + return result.solvable ? std::optional { result } : std::nullopt; } diff --git a/src/tongji/fire_controller/fire_controller.cpp b/src/tongji/fire_controller/fire_controller.cpp index 3c1554c..75b8ff9 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -1,5 +1,6 @@ #include "fire_controller.hpp" +#include #include #include #include @@ -54,26 +55,37 @@ class FireController::Impl { .gimbal_dir = Eigen::Vector3d::Constant(std::numeric_limits::quiet_NaN()), .fire_allowance = false }; } + std::cout << "solved pitch:" << aim_solution.pitch * 57.3 << " du" << std::endl; + const auto gimbal_command = GimbalCommand { aim_solution.yaw, aim_solution.pitch }; const auto target_pos = Eigen::Vector3d { aim_solution.aim_point }; - auto ypr = transform_gimbal2muzzle_.inverse().rotation().eulerAngles(2, 1, 0); - auto gimbal_yaw = ypr(0); - // auto gimbal_pitch = ypr(1); - // auto gimbal_roll = ypr(2); + auto ypr = transform_gimbal2muzzle_.inverse().rotation().eulerAngles(2, 1, 0); + auto gimbal_yaw = ypr(0); + auto gimbal_pitch = ypr(1); + auto gimbal_roll = ypr(2); + // std::cout << "yaw:" << gimbal_yaw * 57.3 << " pitch:" << gimbal_pitch * 57.3 + // << " roll:" << gimbal_roll * 57.3 << std::endl; - auto fire_valid = fire_decision_->ShouldFire(gimbal_yaw, gimbal_command, target_pos); - - if (!fire_valid) { - std::cout << "forbid fire" << std::endl; - } + // auto fire_valid = fire_decision_->ShouldFire(gimbal_yaw, gimbal_command, target_pos); + auto fire_valid = true; data::FireControl result; - result.fire_allowance = fire_valid; - // result.gimbal_dir << cos(gimbal_command.yaw) * cos(gimbal_command.pitch), - // sin(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.pitch); - result.gimbal_dir << target_pos.normalized(); // TODO:not gimbal dir result.time_stamp = time_stamp; + + if (!fire_valid) { + result.fire_allowance = false; + firable_ = false; + result.gimbal_dir = Eigen::Vector3d::Zero(); + std::cout << "forbidden fire" << std::endl; + return result; + } + // std::cout << "fire!" << std::endl; + firable_ = true; + result.fire_allowance = true; + // result.gimbal_dir << target_pos.normalized(); // TODO:not gimbal dir + result.gimbal_dir << cos(gimbal_command.yaw) * cos(gimbal_command.pitch), + sin(gimbal_command.yaw) * cos(gimbal_command.pitch), sin(gimbal_command.pitch); return result; } diff --git a/src/tongji/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index a696ce3..c28ac2a 100644 --- a/src/tongji/fire_controller/fire_decision.hpp +++ b/src/tongji/fire_controller/fire_decision.hpp @@ -4,6 +4,7 @@ #include #include +#include #include namespace world_exe::tongji::fire_control { @@ -40,6 +41,8 @@ class FireDecision { last_gimbal_command_ = gimbal_command; return true; } + // std::cout << "gimbal_command:yaw" << gimbal_command.yaw + // << " pitch:" << gimbal_command.pitch << std::endl; last_gimbal_command_ = gimbal_command; return false; diff --git a/src/tongji/fire_controller/trajectory.hpp b/src/tongji/fire_controller/trajectory.hpp index 7c599d8..80454ea 100644 --- a/src/tongji/fire_controller/trajectory.hpp +++ b/src/tongji/fire_controller/trajectory.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include namespace world_exe::tongji::fire_control { @@ -16,8 +17,12 @@ struct TrajectorySolver { // 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) + static auto SolveTrajectory(const double v0, const Eigen::Vector3d& dir_vector, const double g) -> TrajectoryResult { + + double d = std::hypot(dir_vector.x(), dir_vector.y()); + double h = dir_vector.z(); + auto a = g * d * d / (2 * v0 * v0); auto b = -d; auto c = a + h;