Tongji dev 修改火控模块 飞行时间的迭代的部分

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)

configs/example.yaml

@@ -26,19 +26,16 @@ first_tolerance: 100 # 近距离射击容差，degree
 second_tolerance: 100 # 远距离射击容差，degree
 judge_distance: 2 #距离判断阈值
 
-#####-----fire_controller parameters-----#####
-control_delay_s: 0.1
-
 #####-----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:
 

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<typename T>
     static inline TimeStamp from_seconds(const T time){return TimeStamp{std::chrono::seconds(static_cast<long int>(time))};}

include/interfaces/fire_controller.hpp

@@ -2,7 +2,6 @@
 
 #include "data/fire_control.hpp"
 #include "enum/car_id.hpp"
-#include <chrono>
 #include <ctime>
 
 namespace world_exe::interfaces {
@@ -16,16 +15,16 @@ class IFireControl {
      * @brief 计算云台和发射系统控制量
      *
      * @param time_duration 额外时间差 典型值：当前时刻到当前帧传感器传入内容的时间差
-     * @return const data::FireControl
+     * @return  data::FireControl
      */
-    virtual const data::FireControl CalculateTarget(const std::chrono::seconds& 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;
 };

include/interfaces/predictor.hpp

@@ -27,6 +27,8 @@ class IPredictor {
     virtual std::shared_ptr<IArmorInGimbalControl> Predictor(
         const data::TimeStamp& time_stamp) const = 0;
 
+    // virtual data::TimeStamp GetTimeStamp() const = 0;
+
     virtual ~IPredictor() = default;
 };
 }

src/tongji/auto_aim_system.cpp

@@ -4,9 +4,11 @@
 
 #include <cstdio>
 #include <exception>
+#include <filesystem>
 #include <memory>
 #include <opencv2/core/mat.hpp>
 #include <opencv2/highgui.hpp>
+#include <print>
 #include <tuple>
 
 #include "../v1/sync/syncer.hpp"
@@ -16,24 +18,28 @@
 #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"
 #include "tongji/predictor/car_predictor/car_predictor_manager.hpp"
 #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 {
 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")
+        , config_path_(std::filesystem::path { __FILE__ }.parent_path().parent_path().parent_path()
+              / "configs"
+              / "example."
+                "yaml")
         , fps_() {
         identifier_ = std::make_unique<v1::identifier::Identifier>(
             parameters::ParamsForSystemV1::szu_model_path(),
@@ -46,8 +52,7 @@ class AutoAimSystem::Impl {
         state_machine_       = std::make_shared<state_machine::StateMachine>();
         fire_controller_     = std::make_unique<fire_control::FireController>(
             config_path_, state_machine_, live_target_manager_);
-        time_stamp_ = std::chrono::steady_clock::now();
-        syncer_     = std::make_unique<world_exe::v1::Syncer>(seconds(2), 6e-6);
+        syncer_ = std::make_unique<world_exe::v1::Syncer>(seconds(2), 6e-6);
 
         core::EventBus::Subscript<world_exe::data::MatStamped>(
             parameters::ParamsForSystemV1::raw_image_event,
@@ -71,13 +76,12 @@ class AutoAimSystem::Impl {
 
         if (flag == enumeration::ArmorIdFlag::None) {
             state_machine_->SetLostState();
+            std::println("no armors identified");
             return;
         }
 
         // TODO:update invincible_armors
-        state_machine_->Update(armors_in_image, enumeration::CarIDFlag::None,
-            std::chrono::duration_cast<milliseconds>(
-                std::chrono::steady_clock::now() - time_stamp_));
+        state_machine_->Update(armors_in_image, enumeration::CarIDFlag::None, time_stamp_);
 
         // 这里使用 any_clock::now 也可以，但是时间系统的转换和同步我希望是单独的部分
         auto [pack, check] = syncer_->get_data(raw.stamp);
@@ -100,15 +104,21 @@ class AutoAimSystem::Impl {
 
         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);
 
         /// 这里应该有一个线程进行稳定的输出之类的
         /// 轨迹规划器没有实现，先不管
 
         core::EventBus::Publish<data::FireControl>(
             parameters::ParamsForSystemV1::fire_control_event, GetControlCommand());
-        time_stamp_ = std::chrono::steady_clock::now();
+
+        core::EventBus::Publish<std::shared_ptr<interfaces::IArmorInGimbalControl>>(
+            world_exe::parameters::ParamsForSystemV1::get_lastest_predictor_event,
+            fire_controller_->GetArmorsToView());
 
         if (!debug) [[likely]]
             return;
@@ -123,7 +133,7 @@ class AutoAimSystem::Impl {
             parameters::ParamsForSystemV1::tracker_update_event, combined);
         core::EventBus::Publish<enumeration::CarIDFlag>(
             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);
@@ -147,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<seconds>(std::chrono::steady_clock::now() - time_stamp_));
+            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<world_exe::v1::identifier::Identifier> identifier_;
     std::unique_ptr<solver::Solver> pnp_solver_;
     std::shared_ptr<state_machine::StateMachine> state_machine_;

src/tongji/fire_controller/aim_point_chooser.hpp

@@ -1,47 +1,48 @@
 #pragma once
+#include <yaml-cpp/yaml.h>
 
 #include "data/armor_gimbal_control_spacing.hpp"
 #include "enum/car_id.hpp"
 #include "util/math.hpp"
 
-#include <yaml-cpp/yaml.h>
-
 namespace world_exe::tongji::fire_control {
 
 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<double>() / 57.3; // degree to rad
         leaving_angle_ = yaml["leaving_angle"].as<double>() / 57.3; // degree to rad
     }
 
     std::pair<bool, data::ArmorGimbalControlSpacing> ChooseAimArmor(
         const Eigen::Vector<double, 11>& ekf_x,
-        const std::vector<data::ArmorGimbalControlSpacing> armors, const CarIDFlag& single_id) {
+        std::vector<data::ArmorGimbalControlSpacing> const& armors, const CarIDFlag& single_id) {
         const auto armor_num = armors.size();
         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<std::tuple<int, double>> delta_angle_list;
         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); });
+        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<int> id_list;
             for (const auto& [id, delta_angle] : delta_angle_list) {
-                if (std::abs(delta_angle) > 60 / 57.3) continue;
+                if (std::abs(delta_angle) > (60 / 57.3)) continue;
                 id_list.emplace_back(id);
             }
 
@@ -58,11 +59,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 +76,6 @@ class AimPointChooser {
         if (chosen_id == -1) {
             return { false, armors[std::get<0>(delta_angle_list.front())] };
         }
-
         return {
             true,
             armors[chosen_id],