diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e80351..2256435 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) @@ -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/configs/example.yaml b/configs/example.yaml index cdeb620..b5a83e5 100644 --- a/configs/example.yaml +++ b/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: 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..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,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; }; diff --git a/include/interfaces/predictor.hpp b/include/interfaces/predictor.hpp index 9824f66..2133e8d 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/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 ce640c8..ecb356b 100644 --- a/src/tongji/auto_aim_system.cpp +++ b/src/tongji/auto_aim_system.cpp @@ -4,9 +4,13 @@ #include #include +#include +#include #include #include #include +// #include +#include #include #include "../v1/sync/syncer.hpp" @@ -15,7 +19,9 @@ #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" @@ -23,31 +29,44 @@ #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 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( 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(); 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, @@ -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( - 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(pack, armors_in_camera); + live_target_manager_->Update(combined); core::EventBus::Publish>( 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( 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( parameters::ParamsForSystemV1::car_id_identify_event, flag); core::EventBus::Publish>( @@ -123,7 +152,9 @@ 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; + 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); @@ -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(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 identifier_; std::unique_ptr pnp_solver_; std::shared_ptr state_machine_; 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_point_chooser.hpp b/src/tongji/fire_controller/aim_point_chooser.hpp index 51e2c71..c1120b7 100644 --- a/src/tongji/fire_controller/aim_point_chooser.hpp +++ b/src/tongji/fire_controller/aim_point_chooser.hpp @@ -1,47 +1,48 @@ #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; 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 } + template std::pair ChooseAimArmor( - const Eigen::Vector& ekf_x, - const std::vector 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; // 整车旋转中心的球坐标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++) { 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 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], diff --git a/src/tongji/fire_controller/aim_solver.hpp b/src/tongji/fire_controller/aim_solver.hpp index cdeeb8f..de38c9a 100644 --- a/src/tongji/fire_controller/aim_solver.hpp +++ b/src/tongji/fire_controller/aim_solver.hpp @@ -1,15 +1,20 @@ #pragma once +#include #include +#include #include +#include #include #include - -#include +#include +#include #include #include "../predictor/car_predictor/car_predictor.hpp" #include "aim_point_chooser.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" @@ -20,7 +25,7 @@ struct AimSolution { bool valid; double yaw; double pitch; - Eigen::Vector3d aim_point; // 最终瞄准点（世界坐标 + 装甲板yaw） + Eigen::Vector3d aim_point; double horizon_distance = 0; // 无人机专有 }; @@ -29,7 +34,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,34 +44,52 @@ 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, + Eigen::Affine3d const& transform_gimbal2muzzle, data::TimeStamp const& time_stamp, + std::chrono::milliseconds const& control_delay) { - // 迭代求解飞行时间 (最多10次，收敛条件：相邻两次fly_time差 <0.001) - double prev_fly_time_s; + // 迭代求解飞行时间 + // (最多10次，收敛条件：相邻两次fly_time差 <0.001) + double prev_fly_time_s = 0; Eigen::Vector3d final_aim_point; TrajectoryResult final_trajectory; - bool converged = false; + bool converged = false; + auto fire_origin = transform_gimbal2muzzle.inverse().translation(); + // 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 - 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 + const auto& armors_in_gimbal = armors->GetArmors(snapshot->GetId()); + + 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_gimbal, snapshot->GetId()); + + if (!aim_point.has_value()) { + continue; + } // 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()) { + continue; + } if (i > 0 && std::abs(traj->fly_time - prev_fly_time_s) < 0.001) { final_aim_point = *aim_point; @@ -76,21 +99,32 @@ class AimingSolver { } prev_fly_time_s = traj->fly_time; } - if (!converged) - return { false, std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), { }, - 0 }; // failed: trajectory did not converge - - 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_); + + if (!converged) { + 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_); + 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 }; } + std ::shared_ptr GetArmorsSnapshot() { + return armors_view_; + } + private: - std::optional SelectPredictedAim(const EKF::XVec& ekf_x, - const std::vector& armors, const CarIDFlag& id) const { + std::shared_ptr armors_view_; + 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); @@ -99,9 +133,10 @@ 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 { + + 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 1e3a21e..75b8ff9 100644 --- a/src/tongji/fire_controller/fire_controller.cpp +++ b/src/tongji/fire_controller/fire_controller.cpp @@ -1,8 +1,11 @@ #include "fire_controller.hpp" +#include +#include #include #include +#include #include #include "../identifier/identified_armor.hpp" @@ -25,84 +28,109 @@ 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)) - , state_machine_(state_machine) - , live_target_manager_(live_target_manager) { - - auto yaml = YAML::LoadFile(config_path); - control_delay_s_ = yaml["control_delay_s"].as(); - } + : aiming_solver_(std::make_unique(config_path)) + , state_machine_(std::move(state_machine)) + , live_target_manager_(std::move(live_target_manager)) + , fire_decision_(std::make_unique(config_path)) { } - const data ::FireControl CalculateTarget( - const std::chrono::seconds& time_from_tracker_timepoint) const { + data ::FireControl CalculateTarget(data::TimeStamp const& time_stamp) const { + if (!state_machine_ || !live_target_manager_) return { .fire_allowance = false }; - 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) - 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 }; - // const auto& armors_in_gimbal = snapshot_manager->Predictor(time_from_tracker_timepoint); // TODO:这里不应该指针转换 + // std::println("calculate time:{}", time_stamp.to_seconds()); const auto& aim_solution = aiming_solver_->SolveAimSolution( - snapshot_manager, time_from_tracker_timepoint, control_delay_s_); + snapshot_manager, transform_gimbal2muzzle_, time_stamp, control_delay_); + + if (!aim_solution.valid) { + // 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 }; + } + 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 fire_command = aim_solution.valid - ? fire_decision_->ShouldFire(gimbal_yaw_, gimbal_command, target_pos) - : false; - firable_ = fire_command; + + 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); + auto fire_valid = true; data::FireControl result; - result.fire_allowance = fire_command; - result.gimbal_dir << gimbal_command.yaw, gimbal_command.pitch, 0; - result.time_stamp = data::TimeStamp { time_from_tracker_timepoint }; + 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; } - const CarIDFlag GetAttackCarId() const { - if (firable_) return locked_target; + CarIDFlag GetAttackCarId() const { + if (firable_) return locked_target_; return CarIDFlag::None; } - void UpdateGimbalPosition(const double& gimbal_yaw) { gimbal_yaw_ = gimbal_yaw; }; - -private: - double control_delay_s_; + void SetGimbal2Muzzle(Eigen::Affine3d const& transform_gimbal2muzzle) { + transform_gimbal2muzzle_ = transform_gimbal2muzzle; + } - double gimbal_yaw_; + std ::shared_ptr GetArmorsSnapshot() { + return aiming_solver_->GetArmorsSnapshot(); + } - CarIDFlag locked_target; - mutable double firable_; +private: + 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_; + std::unique_ptr fire_decision_; }; 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; -const data ::FireControl FireController::CalculateTarget( - const std::chrono::seconds& 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); -}; +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 ffa7d15..3daeb1b 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" @@ -12,21 +11,22 @@ 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( - const std::chrono::seconds& 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); + void SetGimbal2Muzzle(Eigen::Affine3d const& transform_gimbal2muzzle); FireController(const FireController&) = delete; FireController& operator=(const FireController&) = delete; 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/fire_controller/fire_decision.hpp b/src/tongji/fire_controller/fire_decision.hpp index 39125c5..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,9 @@ 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 2b9c5e1..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) - -> TrajectoryResult const { + 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; 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..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 { + 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 570e851..b9c6cee 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, @@ -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_; } @@ -58,39 +57,33 @@ 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()); 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_); + return std::make_shared(armors, time_stamp); } EKF::XVec GetEkfX() const { return ekf_->x; } - auto GetModel() const -> const PredictorModel { return model_; } - auto GetEkf() const -> const EKF { return ekf_.value(); } - - data::TimeStamp LastSeen() const { return time_stamp_; } + auto GetModel() const -> PredictorModel { return model_; } + auto GetEkf() const -> EKF { return ekf_.value(); } - auto GetPredictedXYZAList(const double& dt) -> std::vector const { - const auto [x_n, P_n] = ekf_->PredictOnce(dt); - return model_.GetArmorXYZAList(x_n); - } - - auto GetPredictedX(const double& dt) const -> 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(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) { - // 装甲板匹配 int id = model_.MatchArmor( ekf_->x, armor_xyz_in_gimbal, armor_ypr_in_gimbal, armor_ypd_in_gimbal); @@ -109,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 { @@ -118,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) { diff --git a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp index ae0d276..182e54c 100644 --- a/src/tongji/predictor/car_predictor/car_predictor_manager.cpp +++ b/src/tongji/predictor/car_predictor/car_predictor_manager.cpp @@ -1,12 +1,13 @@ #include "car_predictor_manager.hpp" +#include #include #include #include +#include #include #include "../in_gimbal_control_armor.hpp" - #include "car_predictor.hpp" #include "data/predictor_update_package.hpp" #include "data/time_stamped.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(), @@ -51,38 +52,52 @@ 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(), predictor->LastSeen()); + predictor->GetEkf(), predictor->GetModel(), data::TimeStamp { last_update_timestamp_ }); } - void Update(std::shared_ptr data) { - last_update_timestamp_ = data->GetTimeStamp(); - - const Eigen::Affine3d transform = data->GetCameraToWorld(); - const auto armors_interface = data->GetArmors(); + void Update(std::shared_ptr const& data) { + Eigen::Affine3d transform_camera2gimbal = 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_in_camera) { + auto xyz_in_gimbal = transform_camera2gimbal * armor.position; - for (const auto& armor : armors) { 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(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(), 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(), 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)); } } - // std::erase_if(targets_map_, [](const auto& pair) { return pair.second->IsAppeared(); }); + // std::erase_if(targets_map_, [](const auto& pair) { return pair.second->IsAppeared(); + // }); } + + last_update_timestamp_ = data->GetTimeStamp(); } private: @@ -105,7 +120,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..db4f753 100644 --- a/src/tongji/predictor/kalman_filter/predict_model.hpp +++ b/src/tongji/predictor/kalman_filter/predict_model.hpp @@ -31,17 +31,15 @@ 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 - || 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 (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; @@ -57,29 +55,26 @@ 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; } } - 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 +82,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 +99,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,8 +113,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; @@ -136,7 +130,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 < 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))) @@ -151,7 +145,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 +157,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 +199,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 +211,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 +229,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 +245,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 +264,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..b3c97d8 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,23 +69,23 @@ 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; 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); 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..44bc170 100644 --- a/src/tongji/solver/solver.cpp +++ b/src/tongji/solver/solver.cpp @@ -1,6 +1,6 @@ #include "solver.hpp" -#include +#include #include #include #include @@ -27,12 +27,10 @@ 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()) { } - 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++) { @@ -47,36 +45,30 @@ 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 Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> const auto { - Eigen::Vector3d xyz_in_gimbal = - R_camera2gimbal_.transpose() * xyz_in_camera + t_camera2gimbal_; - return xyz_in_gimbal; + auto SetCamera2Gimbal(Eigen::Affine3d const& transform_camera2gimbal) -> void { + transform_camera2gimbal_ = transform_camera2gimbal; } 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); 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); - 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; @@ -86,14 +78,21 @@ 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); + // 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; @@ -102,8 +101,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; @@ -130,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_; }; @@ -144,19 +143,15 @@ 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, 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 { - return pimpl_->Camera2Gimbal(xyz_in_camera); -} } diff --git a/src/tongji/solver/solver.hpp b/src/tongji/solver/solver.hpp index 5247343..6844c5c 100644 --- a/src/tongji/solver/solver.hpp +++ b/src/tongji/solver/solver.hpp @@ -12,13 +12,11 @@ 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 Camera2Gimbal(const Eigen::Vector3d& xyz_in_camera) const -> 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..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 { @@ -15,13 +16,11 @@ class StateMachine::Impl { const enumeration::CarIDFlag& GetAllowdToFires() const { return target_id_; } - void Update(std::shared_ptr armors_in_image, - const enumeration::CarIDFlag& invincible_armors, - const std::chrono::milliseconds& duration_from_last_update) { - + void Update(std::shared_ptr const& armors_in_image, + 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() { @@ -42,10 +41,9 @@ const enumeration::CarIDFlag& StateMachine::GetAllowdToFires() const { return pimpl_->GetAllowdToFires(); } -void StateMachine::Update(std::shared_ptr armors_in_image, - const enumeration::CarIDFlag& invincible_armors, - const std::chrono::milliseconds& duration_from_last_update) { - return pimpl_->Update(armors_in_image, invincible_armors, duration_from_last_update); +void StateMachine::Update(std::shared_ptr const& armors_in_image, + 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 82895da..77c92a7 100644 --- a/src/tongji/state_machine/state_machine.hpp +++ b/src/tongji/state_machine/state_machine.hpp @@ -15,9 +15,8 @@ class StateMachine final : public interfaces::ICarState { const enumeration ::CarIDFlag& GetAllowdToFires() const override; - void Update(std::shared_ptr armors_in_image, - const enumeration::CarIDFlag& invincible_armors, - const std::chrono::milliseconds& duration_from_last_update); + void Update(std::shared_ptr const& armors_in_image, + const enumeration::CarIDFlag& invincible_armors, data::TimeStamp const& time_stamp); void SetLostState(); 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; } 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/util/parameters/params_system_v1.cpp b/src/util/parameters/params_system_v1.cpp index f86d58d..6ad4137 100644 --- a/src/util/parameters/params_system_v1.cpp +++ b/src/util/parameters/params_system_v1.cpp @@ -1,14 +1,27 @@ #include "parameters/params_system_v1.hpp" +#include #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 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; double velocity_begin = 26; double gravity = 9.81; 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); 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..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,9 +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 auto& pre1 = predictor_->Predictor(fly_time + time_duration + control_delay_); + world_exe::data::FireControl CalculateTarget(data::TimeStamp const& time_stamp) { + std::chrono::seconds fly_time { 0 }; + 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; @@ -66,14 +66,14 @@ 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_)}, .fire_allowance = true }; + return { .time_stamp = data::TimeStamp { time_stamp + fly_time + control_delay_ }, + .fire_allowance = true }; } private: @@ -87,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::seconds& 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 171c0e6..1b753fe 100644 --- a/src/v1/fire_controller/fire_controller.hpp +++ b/src/v1/fire_controller/fire_controller.hpp @@ -1,15 +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::seconds& 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/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/predictor/car/car_predictor.cpp b/src/v1/predictor/car/car_predictor.cpp index 43c674f..b087fd7 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,5 @@ void CarPredictor::SetEkf(const CarPredictEkf& ekf) { return pimpl_->SetEkf(ekf) void CarPredictor::SetTimeStamp(const data::TimeStamp& time_stamp) { return pimpl_->SetTimeStamp(time_stamp); }; + } \ 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..e9776a1 100644 --- a/src/v1/predictor/car/car_predictor.hpp +++ b/src/v1/predictor/car/car_predictor.hpp @@ -6,7 +6,7 @@ #include "interfaces/predictor.hpp" namespace world_exe::v1::predictor { -class CarPredictor final: public interfaces::IPredictor { +class CarPredictor final : public interfaces::IPredictor { public: CarPredictor(); ~CarPredictor(); 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); }; 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 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; } };