diff --git a/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp b/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp index 2578e5d50a4d0d7c8dcc4fd8690f972021f9cfc6..79e6d92dcb4dac5b5ce67f6646e639258439f449 100644 --- a/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp +++ b/rm_auto_aim/armor_solver/include/armor_solver/armor_solver.hpp @@ -101,6 +101,10 @@ private: double leaving_angle_; int lock_id_ = -1; + // Aim offset (degrees) — decoupled from camera extrinsics + double yaw_offset_; + double pitch_offset_; + // Fire decision parameters double first_tolerance_; double second_tolerance_; diff --git a/rm_auto_aim/armor_solver/src/armor_solver.cpp b/rm_auto_aim/armor_solver/src/armor_solver.cpp index 005405d4bfe6f8b0aecd837b03548fa930a9df2c..18cb3777456855a75d545178d6cea3141c570f7b 100644 --- a/rm_auto_aim/armor_solver/src/armor_solver.cpp +++ b/rm_auto_aim/armor_solver/src/armor_solver.cpp @@ -54,6 +54,10 @@ Solver::Solver(std::weak_ptr n) : node_(n) { second_tolerance_ = node->declare_parameter("solver.second_tolerance", 2.0); judge_distance_ = node->declare_parameter("solver.judge_distance", 2.0); + // 瞄准偏移量(与 launch rpy 解耦,不影响 yaw 优化) + yaw_offset_ = node->declare_parameter("solver.yaw_offset", 0.0); + pitch_offset_ = node->declare_parameter("solver.pitch_offset", 0.0); + // 初始化弹道补偿器 std::string compenstator_type = node->declare_parameter("solver.compensator_type", "ideal"); trajectory_compensator_ = CompensatorFactory::createCompensator(compenstator_type); @@ -85,6 +89,8 @@ rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &ta first_tolerance_ = node->get_parameter("solver.first_tolerance").as_double(); second_tolerance_ = node->get_parameter("solver.second_tolerance").as_double(); judge_distance_ = node->get_parameter("solver.judge_distance").as_double(); + yaw_offset_ = node->get_parameter("solver.yaw_offset").as_double(); + pitch_offset_ = node->get_parameter("solver.pitch_offset").as_double(); node.reset(); } catch (const std::runtime_error &e) { FYT_ERROR("armor_solver", "{}", e.what()); @@ -220,6 +226,10 @@ rm_interfaces::msg::GimbalCmd Solver::solve(const rm_interfaces::msg::Target &ta break; } } + // 应用瞄准偏移(与 launch rpy 解耦) + yaw += yaw_offset_ * M_PI / 180.0; + pitch += pitch_offset_ * M_PI / 180.0; + gimbal_cmd.yaw = yaw * 180 / M_PI; gimbal_cmd.pitch = pitch * 180 / M_PI; gimbal_cmd.yaw_diff = (yaw - rpy[2]) * 180 / M_PI; diff --git a/rm_bringup/config/node_params/armor_solver_params.yaml b/rm_bringup/config/node_params/armor_solver_params.yaml index 4bd363e1649ea9669725df35a7aba9fbb3887307..57627b44e1b47bebc39d4eed972f04e3f8a447ab 100644 --- a/rm_bringup/config/node_params/armor_solver_params.yaml +++ b/rm_bringup/config/node_params/armor_solver_params.yaml @@ -38,6 +38,11 @@ first_tolerance: 3.0 second_tolerance: 2.0 judge_distance: 2.0 + # 瞄准偏移量(单位:度)— 用于修正机械零位、管子歪等系统误差 + # 与 launch 中的 xyz rpy(相机物理安装角度)解耦,不影响 yaw 优化 + yaw_offset: 0.0 + pitch_offset: 0.0 + bullet_speed: 23.0 compensator_type: "ideal" gravity: 9.8035