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)
@@ -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(

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
+yaw_offset: -2.0 # degree
+pitch_offset: -0.0 # degree
+bullet_speed: 23
 
 #####-----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;
 };
 }

include/utils/mat_triple_buffer.hpp

@@ -11,50 +11,47 @@
 
 namespace world_exe::util::memory {
 
-
-template<typename F>
+template <typename F>
 concept BinaryFunctor = requires(F f) {
     { f() } -> std::same_as<data::TimeStamp>;
 };
 
-template<BinaryFunctor Func>
-class MatTripleBuffer {
-/// SPSC only
+template <BinaryFunctor Func> class MatTripleBuffer {
+    /// SPSC only
 public:
-    explicit MatTripleBuffer(Func timeFunc) : func_(timeFunc) {}
+    explicit MatTripleBuffer(Func timeFunc)
+        : func_(timeFunc) { }
 
-    static MatTripleBuffer<time_stamp::SteadyClock> default_buffer(){
-        return  MatTripleBuffer(time_stamp::SteadyClock{});
+    static MatTripleBuffer<time_stamp::SteadyClock> 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<std::reference_wrapper<data::MatStamped>> get()
-    {
+    std::optional<std::reference_wrapper<data::MatStamped>> 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<size_t> data_version{0};
-    std::atomic<size_t> last_read_version{0};
+
+    std::atomic<size_t> data_version { 0 };
+    std::atomic<size_t> last_read_version { 0 };
 };
 
 }

src/tongji/auto_aim_system.cpp

@@ -4,9 +4,13 @@
 
 #include <cstdio>
 #include <exception>
+#include <filesystem>
+#include <iostream>
 #include <memory>
 #include <opencv2/core/mat.hpp>
 #include <opencv2/highgui.hpp>
+// #include <print>
+#include <ostream>
 #include <tuple>
 
 #include "../v1/sync/syncer.hpp"
@@ -15,39 +19,54 @@
 #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"
 #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"
+#include <ament_index_cpp/get_package_share_directory.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")
+
         , 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<v1::identifier::Identifier>(
             parameters::ParamsForSystemV1::szu_model_path(),
             parameters::ParamsForSystemV1::device(), parameters::HikCameraProfile::get_width(),
             parameters::HikCameraProfile::get_height());
-        // identifier_          = std::make_unique<tongji::identifier::Identifier>(config_path_,
-        // ".");
         pnp_solver_          = std::make_unique<solver::Solver>();
         live_target_manager_ = std::make_shared<predictor::CarPredictorManager>(config_path_);
         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,
@@ -60,59 +79,69 @@ 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();
+            // std::cout << "no armor identified!" << std::endl;
             return;
+        } else {
+            // std::cout << "found!" << std::endl;
         }
 
         // 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);
-        if (!check)
-            pack.camera_capture_begin_time_stamp =
-                data::TimeStamp(steady_clock::now().time_since_epoch());
-
-        const auto R_camera2gimbal = pack.camera_to_gimbal.rotation();
-        const auto t_camera2gimbal = pack.camera_to_gimbal.translation();
+        // 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;
+            return;
+        }
 
-        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<data::PredictorUpdatePackage>(pack, armors_in_camera);
+
         live_target_manager_->Update(combined);
 
         core::EventBus::Publish<std ::shared_ptr<interfaces ::IArmorInGimbalControl>>(
             parameters::ParamsForSystemV1::tracker_current_armors_event,
-            live_target_manager_->Predict(flag, pack.camera_capture_begin_time_stamp));
-
-        const auto target_id = state_machine_->GetAllowdToFires();
+            live_target_manager_->Predict(
+                enumeration::ArmorIdFlag::None, pack.camera_capture_begin_time_stamp));
+        // live_target_manager_->Predict(flag, pack.camera_capture_begin_time_stamp));
 
-        const auto gimbal_yaw = R_camera2gimbal.eulerAngles(2, 1, 0)[0];
-        fire_controller_->UpdateGimbalPosition(gimbal_yaw);
+        time_stamp_ = pack.camera_capture_begin_time_stamp;
 
         /// 这里应该有一个线程进行稳定的输出之类的
         /// 轨迹规划器没有实现，先不管
 
+        fire_controller_->SetGimbal2Muzzle(pack.gimbal_to_muzzle);
+
         core::EventBus::Publish<data::FireControl>(
             parameters::ParamsForSystemV1::fire_control_event, GetControlCommand());
-        time_stamp_ = std::chrono::steady_clock::now();
 
         if (!debug) [[likely]]
             return;
 
+        auto target = state_machine_->GetAllowdToFires();
+
         core::EventBus::Publish<enumeration::CarIDFlag>(
             parameters::ParamsForSystemV1::car_id_identify_event, flag);
         core::EventBus::Publish<std::shared_ptr<interfaces::IArmorInImage>>(
@@ -123,7 +152,9 @@ 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;
+        core::EventBus::Publish<std ::shared_ptr<interfaces ::IArmorInGimbalControl>>(
+            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);
@@ -147,24 +178,24 @@ class AutoAimSystem::Impl {
 
     void SetTransfroms(const data::CameraGimbalMuzzleSyncData& data) { syncer_->set_data(data); }
 
+    // TODO:时间戳有待fix
     data::FireControl GetControlCommand() {
-        fire_controller_->GetAttackCarId();
+        auto attacked_id = 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_;
+    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_;
     std::shared_ptr<predictor::CarPredictorManager> live_target_manager_;
     std::unique_ptr<world_exe::v1::Syncer> syncer_;
     std::unique_ptr<fire_control::FireController> fire_controller_;
-    // std::unique_ptr<tests::mock::Camera2GimbalTransformer> mock_camera_tranform_;
 };
 
 AutoAimSystem::AutoAimSystem(const bool& debug)