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 +