From ba03e54e7f3416ab3f70a5f4b4d744352852b2f6 Mon Sep 17 00:00:00 2001 From: Rayy0527 <1668834312@qq.com> Date: Fri, 2 May 2025 18:48:10 +0800 Subject: [PATCH] Fix track target switch. --- .../include/rm_gimbal_controllers/bullet_solver.h | 3 ++- rm_gimbal_controllers/src/bullet_solver.cpp | 12 +++++++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index eb171b8b..0f4e5a22 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -103,10 +103,11 @@ class BulletSolver double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; double fly_time_; + double switch_hysteresis_; int shoot_beforehand_cmd_{}; int selected_armor_; int count_; - bool track_target_; + bool track_target_ = true; bool identified_target_change_ = true; bool is_in_delay_before_switch_{}; bool dynamic_reconfig_initialized_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 2fc23f53..b4dcc8eb 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -65,6 +65,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); + switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); config_rt_buffer_.initRT(config_); marker_desire_.header.frame_id = "odom"; @@ -134,7 +135,16 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double z = pos.z; double max_switch_angle = config_.max_switch_angle / 180 * M_PI; double min_switch_angle = config_.min_switch_angle / 180 * M_PI; - track_target_ = std::abs(v_yaw) < max_track_target_vel_; + if (track_target_) + { + if (std::abs(v_yaw) >= max_track_target_vel_ + switch_hysteresis_) + track_target_ = false; + } + else + { + if (std::abs(v_yaw) <= max_track_target_vel_ - switch_hysteresis_) + track_target_ = true; + } double switch_armor_angle = track_target_ ? (acos(r / target_rho) - max_switch_angle +