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 218436d6..5ed6d081 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -86,12 +86,15 @@ class BulletSolver realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; + double max_track_target_vel_; bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; int selected_armor_; + bool track_target_; geometry_msgs::Point target_pos_{}; + double fly_time_; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; }; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index c915e1a5..e1e886ef 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -53,6 +53,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.) }; + max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); marker_desire_.header.frame_id = "odom"; @@ -114,33 +115,60 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; - if (((yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4) || ((yaw + v_yaw * rough_fly_time) < output_yaw_ - M_PI_4)) + track_target_ = std::abs(v_yaw) < max_track_target_vel_; + double switch_armor_angle = + track_target_ ? + acos(r / target_rho) - M_PI / 12 + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : + M_PI / 12; + 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.)) { - selected_armor_ = (yaw + v_yaw * rough_fly_time) > output_yaw_ + M_PI_4 ? -1 : 1; + selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; } int count{}; double error = 999; - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + if (track_target_) + { + target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + } + else + { + target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); + target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); + } target_pos_.z = z; while (error >= 0.001) { output_yaw_ = std::atan2(target_pos_.y, target_pos_.x); output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2))); target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); - double fly_time = + fly_time_ = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - - config_.g * fly_time / resistance_coff_; + (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - + config_.g * fly_time_ / resistance_coff_; - target_pos_.x = - pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = - pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.z = z + vel.z * fly_time; + if (track_target_) + { + target_pos_.x = + pos.x + vel.x * fly_time_ - r * cos(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.y = + pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); + } + else + { + double target_pos_after_fly_time[2]; + target_pos_after_fly_time[0] = pos.x + vel.x * fly_time_; + target_pos_after_fly_time[1] = pos.y + vel.y * fly_time_; + target_pos_.x = + target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + target_pos_.y = + target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + } + target_pos_.z = z + vel.z * fly_time_; double target_yaw = std::atan2(target_pos_.y, target_pos_.x); double error_theta = target_yaw - output_yaw_; @@ -165,12 +193,21 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge r = r2; z = pos.z + dz; } - 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.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.z = vel.z; + 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.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.z = vel.z; + } + else + { + armor_pos = pos; + armor_pos.z = z; + armor_vel = vel; + } } void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) @@ -226,8 +263,7 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed) { - config_ = *config_rt_buffer_.readFromRT(); - double resistance_coff = getResistanceCoefficient(bullet_speed); + double delay = track_target_ ? 0 : config_.delay; double r = r1; double z = pos.z; if (selected_armor_ != 0) @@ -235,31 +271,30 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; } - double fly_time = (-std::log(1 - std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)) * resistance_coff / - (bullet_speed * std::cos(pitch_real)))) / - resistance_coff; - double last_fly_time{}, target_rho{}; - int count{}; - geometry_msgs::Point target_pos{}; - while (std::abs(fly_time - last_fly_time) > 0.01) + double error; + if (track_target_) { - last_fly_time = fly_time; - target_pos.x = pos.x + vel.x * fly_time - r * cos(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); - target_pos.y = pos.y + vel.y * fly_time - r * sin(yaw + v_yaw * fly_time + selected_armor_ * 2 * M_PI / armors_num); - target_pos.z = z + vel.z * fly_time; - target_rho = std::sqrt(std::pow(target_pos.x, 2) + std::pow(target_pos.y, 2)); - fly_time = (-std::log(1 - target_rho * resistance_coff / (bullet_speed * std::cos(pitch_real)))) / resistance_coff; - count++; - if (count >= 20 || std::isnan(fly_time)) - return 999; + double bullet_rho = + bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + double bullet_x = bullet_rho * std::cos(yaw_real); + double bullet_y = bullet_rho * std::sin(yaw_real); + double bullet_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - + config_.g * fly_time_ / resistance_coff_; + error = std::sqrt(std::pow(target_pos_.x - bullet_x, 2) + std::pow(target_pos_.y - bullet_y, 2) + + std::pow(target_pos_.z - bullet_z, 2)); + } + else + { + geometry_msgs::Point target_pos_after_fly_time_and_delay{}; + target_pos_after_fly_time_and_delay.x = pos.x + vel.x * (fly_time_ + delay) - + r * cos(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.y = pos.y + vel.y * (fly_time_ + delay) - + r * sin(yaw + v_yaw * (fly_time_ + delay) + selected_armor_ * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_ + delay); + error = std::sqrt(std::pow(target_pos_.x - target_pos_after_fly_time_and_delay.x,2) + std::pow(target_pos_.y - target_pos_after_fly_time_and_delay.y,2) + + std::pow(target_pos_.z - target_pos_after_fly_time_and_delay.z,2)); } - double real_z = (bullet_speed * std::sin(pitch_real) + (config_.g / resistance_coff)) * - (1 - std::exp(-resistance_coff * fly_time)) / resistance_coff - - config_.g * fly_time / resistance_coff; - double target_yaw = std::atan2(target_pos.y, target_pos.x); - double error = std::sqrt(std::pow(target_rho * (std::cos(target_yaw) - std::cos(yaw_real)), 2) + - std::pow(target_rho * (std::sin(target_yaw) - std::sin(yaw_real)), 2) + - std::pow(target_pos.z - real_z, 2)); return error; }