diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index ce807a8c..c8de53c0 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -16,10 +16,12 @@ gen.add("wait_next_armor_delay", double_t, 0, "Delay of shooting next target whe gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.105, 0.0, 0.5) gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) -gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0) -gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.0) gen.add("max_switch_angle", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) gen.add("min_switch_angle", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) +gen.add("switch_angle_offset", double_t, 0, "Switch angle offset", 0.0, -20.0, 20) +gen.add("switch_duration_scale",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) +gen.add("switch_duration_rate",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) +gen.add("switch_duration_offset",double_t, 0,"A param of gimbal switch duration", 0.0, -99.0, 99.0) gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 0.0, 4.5, 20) gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0) 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 0f4e5a22..688c5bb0 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -56,8 +56,8 @@ namespace rm_gimbal_controllers struct Config { double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, wait_next_armor_delay, wait_diagonal_armor_delay, dt, timeout, - ban_shoot_duration, gimbal_switch_duration, max_switch_angle, min_switch_angle, min_shoot_beforehand_vel, + resistance_coff_qd_30, g, delay, wait_next_armor_delay, wait_diagonal_armor_delay, dt, timeout, max_switch_angle, + min_switch_angle, switch_duration_scale, switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay; int min_fit_switch_count; }; @@ -80,6 +80,7 @@ class BulletSolver { return -output_pitch_; } + double getGimbalSwitchDuration(double v_yaw); void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num); @@ -104,6 +105,8 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; double fly_time_; double switch_hysteresis_; + double last_yaw_{}, filtered_yaw_{}; + double gimbal_switch_duration_{}; int shoot_beforehand_cmd_{}; int selected_armor_; int count_; diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index bbb862e5..71b5b7c5 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -181,7 +181,6 @@ class Controller : public controller_interface::MultiInterfaceController(controller_nh, "shoot_beforehand_cmd", 10)); fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); - identified_target_change_sub_ = controller_nh.subscribe("/change", 10, &BulletSolver::identifiedTargetChangeCB, this); } @@ -124,6 +126,14 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d bullet_speed_ = bullet_speed; resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; + if (abs(yaw - last_yaw_) > 1.) + filtered_yaw_ = yaw; + else if (last_yaw_ != yaw) + { + filtered_yaw_ = filtered_yaw_ + (yaw - filtered_yaw_) * (0.001 / (0.01 + 0.001)); + } + last_yaw_ = yaw; + double temp_z = pos.z; double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); output_yaw_ = std::atan2(pos.y, pos.x); @@ -133,7 +143,6 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; 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; if (track_target_) { @@ -145,16 +154,24 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d 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 + - (-acos(r / target_rho) + (max_switch_angle + min_switch_angle)) * std::abs(v_yaw) / max_track_target_vel_) * - (1 - std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel) + - min_switch_angle * std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel : - min_switch_angle; - if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && - std::abs(v_yaw) >= 1.0) + double switch_armor_angle{}; + if (track_target_) + { + if (v_yaw > 0.) + switch_armor_angle = + M_PI / armors_num - (2 * rough_fly_time + getGimbalSwitchDuration(std::abs(v_yaw))) / 2 * v_yaw; + else if (v_yaw < 0.) + switch_armor_angle = + -M_PI / armors_num - (2 * rough_fly_time + getGimbalSwitchDuration(std::abs(v_yaw))) / 2 * v_yaw; + } + else + switch_armor_angle = min_switch_angle; + double yaw_subtract = filtered_yaw_ - output_yaw_; + while (yaw_subtract > M_PI) + yaw_subtract -= 2 * M_PI; + while (yaw_subtract < -M_PI) + yaw_subtract += 2 * M_PI; + if (((yaw_subtract > switch_armor_angle) && v_yaw > 1.) || ((yaw_subtract < switch_armor_angle) && v_yaw < -1.)) { count_++; if (identified_target_change_) @@ -171,14 +188,15 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d z = armors_num == 4 ? pos.z + dz : pos.z; } } - is_in_delay_before_switch_ = - (((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) > - output_yaw_ + switch_armor_angle) && - v_yaw > 0.) || - ((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) < - output_yaw_ - switch_armor_angle) && - v_yaw < 0.)) && - track_target_; + double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 2 * M_PI / armors_num; + double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_; + while (aim_yaw_subtract > M_PI) + aim_yaw_subtract -= 2 * M_PI; + while (aim_yaw_subtract < -M_PI) + aim_yaw_subtract += 2 * M_PI; + is_in_delay_before_switch_ = ((((aim_yaw_subtract + v_yaw * config_.delay) > switch_armor_angle) && v_yaw > 0.) || + (((aim_yaw_subtract + v_yaw * config_.delay) < switch_armor_angle) && v_yaw < 0.)) && + track_target_; if (track_target_) yaw += v_yaw * config_.track_rotate_target_delay; pos.x += vel.x * config_.track_move_target_delay; @@ -264,13 +282,21 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge r = r2; z = pos.z + dz; } + pos.x += vel.x * (config_.track_move_target_delay + fly_time_); + pos.y += vel.y * (config_.track_move_target_delay + fly_time_); if (track_target_) { - armor_pos.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_pos.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + + selected_armor_ * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + + selected_armor_ * 2 * M_PI / armors_num); armor_pos.z = z; - armor_vel.x = vel.x + v_yaw * r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); - armor_vel.y = vel.y - v_yaw * r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); + armor_vel.x = vel.x + v_yaw * r * + sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + + selected_armor_ * 2 * M_PI / armors_num); + armor_vel.y = vel.y - v_yaw * r * + cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + + selected_armor_ * 2 * M_PI / armors_num); armor_vel.z = vel.z; } else @@ -386,13 +412,21 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) identified_target_change_ = true; } +double BulletSolver::getGimbalSwitchDuration(double v_yaw) +{ + gimbal_switch_duration_ = + config_.switch_duration_scale * std::exp(config_.switch_duration_rate * (v_yaw * 60 / (2 * M_PI))) + + config_.switch_duration_offset; + return gimbal_switch_duration_; +} + void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) { if (!track_target_) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; - else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec()) + else if ((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw)) - config_.delay) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec()) && + else if (((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw))) && std::abs(v_yaw) > config_.min_shoot_beforehand_vel) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; else if (is_in_delay_before_switch_) @@ -424,10 +458,11 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.wait_diagonal_armor_delay = init_config.wait_diagonal_armor_delay; config.dt = init_config.dt; config.timeout = init_config.timeout; - config.ban_shoot_duration = init_config.ban_shoot_duration; - config.gimbal_switch_duration = init_config.gimbal_switch_duration; config.max_switch_angle = init_config.max_switch_angle; config.min_switch_angle = init_config.min_switch_angle; + config.switch_duration_scale = init_config.switch_duration_scale; + config.switch_duration_rate = init_config.switch_duration_rate; + config.switch_duration_offset = init_config.switch_duration_offset; config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; config.max_chassis_angular_vel = init_config.max_chassis_angular_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; @@ -446,10 +481,11 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .wait_diagonal_armor_delay = config.wait_diagonal_armor_delay, .dt = config.dt, .timeout = config.timeout, - .ban_shoot_duration = config.ban_shoot_duration, - .gimbal_switch_duration = config.gimbal_switch_duration, .max_switch_angle = config.max_switch_angle, .min_switch_angle = config.min_switch_angle, + .switch_duration_scale = config.switch_duration_scale, + .switch_duration_rate = config.switch_duration_rate, + .switch_duration_offset = config.switch_duration_offset, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, .max_chassis_angular_vel = config.max_chassis_angular_vel, .track_rotate_target_delay = config.track_rotate_target_delay, diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 1eaed2e3..48c5616d 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -434,9 +434,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { geometry_msgs::Point target_pos; geometry_msgs::Vector3 target_vel; - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, data_track_.position, data_track_.velocity, - data_track_.yaw, data_track_.v_yaw, data_track_.radius_1, - data_track_.radius_2, data_track_.dz, data_track_.armors_num); + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, data_track_.position, data_track_.velocity, yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, + data_track_.dz, data_track_.armors_num); target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); @@ -452,8 +453,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); - vel_des[2] = angles::shortest_angular_distance(last_pos_des_[2], pos_des[2]) / period.toSec(); - last_pos_des_[2] = pos_des[2]; + vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); } if (joint_urdfs_.find(1) != joint_urdfs_.end()) {