From 9e1615413524e153ed2f80728d42fedd7d0ffd98 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 12 Jul 2025 16:55:43 +0800 Subject: [PATCH 01/19] Modify switch armor angle compute. --- .../rm_gimbal_controllers/bullet_solver.h | 1 + rm_gimbal_controllers/src/bullet_solver.cpp | 31 +++++++++---------- rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 3 files changed, 17 insertions(+), 17 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 0f4e5a22..57be70a2 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -104,6 +104,7 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; double fly_time_; double switch_hysteresis_; + double last_yaw_{}, filtered_yaw_{}; int shoot_beforehand_cmd_{}; int selected_armor_; int count_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index b4dcc8eb..9e70f12d 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -124,6 +124,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 +141,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_) { @@ -146,15 +153,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d 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) + track_target_ ? M_PI / armors_num - (2 * rough_fly_time + config_.gimbal_switch_duration) / 2 * abs(v_yaw) : + min_switch_angle; + if (((filtered_yaw_ > output_yaw_ + switch_armor_angle) && v_yaw > 1.) || + ((filtered_yaw_ < output_yaw_ - switch_armor_angle) && v_yaw < -1.)) { count_++; if (identified_target_change_) @@ -171,13 +173,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d z = armors_num == 4 ? pos.z + dz : pos.z; } } + double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 2 * M_PI / armors_num; 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.)) && + ((((filtered_aim_armor_yaw + v_yaw * config_.delay) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((filtered_aim_armor_yaw + v_yaw * config_.delay) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && track_target_; if (track_target_) yaw += v_yaw * config_.track_rotate_target_delay; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e98d628b..e88c4575 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -148,7 +148,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2base_.transform.rotation.w = 1.; cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); + data_track_sub_ = controller_nh.subscribe("/track1", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); From ab3c822c675833cc2af51393092720885cac9c35 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 12 Jul 2025 16:58:01 +0800 Subject: [PATCH 02/19] Fix track topic. --- rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e88c4575..e98d628b 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -148,7 +148,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2base_.transform.rotation.w = 1.; cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - data_track_sub_ = controller_nh.subscribe("/track1", 1, &Controller::trackCB, this); + data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); From a406beeec9cdf7697e87abff70c9957cc5628cd8 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sat, 12 Jul 2025 17:06:31 +0800 Subject: [PATCH 03/19] Fix ban shoot. --- rm_gimbal_controllers/src/bullet_solver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 9e70f12d..0f202209 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -389,9 +389,9 @@ 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() < config_.gimbal_switch_duration - 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() < config_.gimbal_switch_duration) && std::abs(v_yaw) > config_.min_shoot_beforehand_vel) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; else if (is_in_delay_before_switch_) From 2d8b6e2afdbf114fa737887d24acb764c6b8bab8 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 13 Jul 2025 15:37:03 +0800 Subject: [PATCH 04/19] Add some limit. --- rm_gimbal_controllers/src/gimbal_base.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 1eaed2e3..1d0f6e9f 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -452,8 +452,11 @@ 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] = angles::shortest_angular_distance(last_pos_des_[2], pos_des[2]) / 0.001; + vel_des[2] = + std::max(-joint_urdfs_.at(2)->limits->velocity, std::min(vel_des[2], joint_urdfs_.at(2)->limits->velocity)); + if (data_track_.v_yaw * vel_des[2] > 0) + vel_des[2] = 0; } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { @@ -471,6 +474,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ROS_WARN("%s", ex.what()); } } + last_pos_des_[2] = pos_des[2]; for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; @@ -490,6 +494,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); ctrls_.at(2)->update(time, period); + ctrls_.at(2)->joint_.setCommand( + std::max(-joint_urdfs_.at(2)->limits->effort, + std::min(ctrls_.at(2)->joint_.getCommand(), joint_urdfs_.at(2)->limits->effort))); } // publish state From 516dc6c9c55af41879ff8349cdad5e3465af2957 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 13 Jul 2025 16:04:06 +0800 Subject: [PATCH 05/19] Add decimal point. --- rm_gimbal_controllers/src/gimbal_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 1d0f6e9f..5c2ff176 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -455,8 +455,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) vel_des[2] = angles::shortest_angular_distance(last_pos_des_[2], pos_des[2]) / 0.001; vel_des[2] = std::max(-joint_urdfs_.at(2)->limits->velocity, std::min(vel_des[2], joint_urdfs_.at(2)->limits->velocity)); - if (data_track_.v_yaw * vel_des[2] > 0) - vel_des[2] = 0; + if (data_track_.v_yaw * vel_des[2] > 0.) + vel_des[2] = 0.; } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { From e43e4fafe33a7b8ec02697283b962abdf84700bd Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 13 Jul 2025 17:45:17 +0800 Subject: [PATCH 06/19] Compute vel des by target state. --- rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 5c2ff176..650c0556 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -452,7 +452,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]) / 0.001; + vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); vel_des[2] = std::max(-joint_urdfs_.at(2)->limits->velocity, std::min(vel_des[2], joint_urdfs_.at(2)->limits->velocity)); if (data_track_.v_yaw * vel_des[2] > 0.) From 4d492a3ef2288a3516b3adad91907d926636b1f7 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 13 Jul 2025 17:58:06 +0800 Subject: [PATCH 07/19] Note limit. --- rm_gimbal_controllers/src/gimbal_base.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 650c0556..158bfede 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -453,10 +453,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); - vel_des[2] = - std::max(-joint_urdfs_.at(2)->limits->velocity, std::min(vel_des[2], joint_urdfs_.at(2)->limits->velocity)); - if (data_track_.v_yaw * vel_des[2] > 0.) - vel_des[2] = 0.; + // vel_des[2] = + // std::max(-joint_urdfs_.at(2)->limits->velocity, std::min(vel_des[2], joint_urdfs_.at(2)->limits->velocity)); } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { @@ -494,9 +492,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); ctrls_.at(2)->update(time, period); - ctrls_.at(2)->joint_.setCommand( - std::max(-joint_urdfs_.at(2)->limits->effort, - std::min(ctrls_.at(2)->joint_.getCommand(), joint_urdfs_.at(2)->limits->effort))); + // ctrls_.at(2)->joint_.setCommand( + // std::max(-joint_urdfs_.at(2)->limits->effort, + // std::min(ctrls_.at(2)->joint_.getCommand(), joint_urdfs_.at(2)->limits->effort))); } // publish state From 9ac8d63a0f0c2008ccc2461da69c6383fbd241bf Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Sun, 13 Jul 2025 18:10:38 +0800 Subject: [PATCH 08/19] Delete note. --- rm_gimbal_controllers/src/gimbal_base.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 158bfede..4c21833a 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -453,8 +453,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); vel_des[2] = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); - // vel_des[2] = - // std::max(-joint_urdfs_.at(2)->limits->velocity, std::min(vel_des[2], joint_urdfs_.at(2)->limits->velocity)); } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { @@ -492,9 +490,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); ctrls_.at(2)->update(time, period); - // ctrls_.at(2)->joint_.setCommand( - // std::max(-joint_urdfs_.at(2)->limits->effort, - // std::min(ctrls_.at(2)->joint_.getCommand(), joint_urdfs_.at(2)->limits->effort))); } // publish state From 7a2e25832a92bb37c732c5dfa213329b1e04b13f Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 14 Jul 2025 13:48:47 +0800 Subject: [PATCH 09/19] Fix armor pos and vel compute. --- rm_gimbal_controllers/src/bullet_solver.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 0f202209..0532e11e 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -265,11 +265,17 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge } 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 From 2df888454cc004aba10b19cdfb99379faee63f7f Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 14 Jul 2025 14:04:24 +0800 Subject: [PATCH 10/19] Use angles lib to compute angle distance. --- rm_gimbal_controllers/src/bullet_solver.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 0532e11e..acec1106 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -124,12 +125,10 @@ 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.) + if (abs(angles::shortest_angular_distance(last_yaw_, 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; From bceabc249a53df1b4c4d31879f2d22dedfbad44f Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 14 Jul 2025 15:54:44 +0800 Subject: [PATCH 11/19] Rollback filtered yaw reset. --- rm_gimbal_controllers/src/bullet_solver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index acec1106..0532e11e 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -39,7 +39,6 @@ #include #include #include -#include namespace rm_gimbal_controllers { @@ -125,10 +124,12 @@ 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(angles::shortest_angular_distance(last_yaw_, yaw)) > 1.) + 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; From c50c6f6316c2b05789d33908bc1992ac4cf8e0f3 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 15 Jul 2025 08:50:17 +0800 Subject: [PATCH 12/19] Fix the wrong of value drop. --- rm_gimbal_controllers/src/bullet_solver.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 0532e11e..62dbe674 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -155,8 +155,12 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double switch_armor_angle = track_target_ ? M_PI / armors_num - (2 * rough_fly_time + config_.gimbal_switch_duration) / 2 * abs(v_yaw) : min_switch_angle; - if (((filtered_yaw_ > output_yaw_ + switch_armor_angle) && v_yaw > 1.) || - ((filtered_yaw_ < output_yaw_ - switch_armor_angle) && v_yaw < -1.)) + double yaw_subtract = filtered_yaw_ - output_yaw_; + while (yaw_subtract > M_PI) + yaw_subtract -= 2 * yaw_subtract; + 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_) From f0776f912e737f4a45e3d35e42644c17ac7645a8 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Thu, 17 Jul 2025 19:53:51 +0800 Subject: [PATCH 13/19] Fix wrong for switch compute and add some params for switch duration. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 6 +- .../rm_gimbal_controllers/bullet_solver.h | 8 +- rm_gimbal_controllers/src/bullet_solver.cpp | 100 ++++++++++++------ rm_gimbal_controllers/src/gimbal_base.cpp | 7 +- 4 files changed, 78 insertions(+), 43 deletions(-) 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 57be70a2..e81ba1dc 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -56,9 +56,9 @@ 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, - max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay; + resistance_coff_qd_30, g, delay, wait_next_armor_delay, wait_diagonal_armor_delay, dt, timeout, max_switch_angle, + min_switch_angle, switch_angle_offset, 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); @@ -105,6 +106,7 @@ class BulletSolver 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/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 62dbe674..f30c2fbf 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -44,26 +44,30 @@ namespace rm_gimbal_controllers { BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) { - config_ = { .resistance_coff_qd_10 = getParam(controller_nh, "resistance_coff_qd_10", 0.), - .resistance_coff_qd_15 = getParam(controller_nh, "resistance_coff_qd_15", 0.), - .resistance_coff_qd_16 = getParam(controller_nh, "resistance_coff_qd_16", 0.), - .resistance_coff_qd_18 = getParam(controller_nh, "resistance_coff_qd_18", 0.), - .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.), - .g = getParam(controller_nh, "g", 0.), - .delay = getParam(controller_nh, "delay", 0.), - .wait_next_armor_delay = getParam(controller_nh, "wait_next_armor_delay", 0.105), - .wait_diagonal_armor_delay = getParam(controller_nh, "wait_diagonal_armor_delay", 0.105), - .dt = getParam(controller_nh, "dt", 0.), - .timeout = getParam(controller_nh, "timeout", 0.), - .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), - .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0), - .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), - .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0), - .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 4.5), - .max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5), - .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), - .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), - .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3) }; + config_ = { + .resistance_coff_qd_10 = getParam(controller_nh, "resistance_coff_qd_10", 0.), + .resistance_coff_qd_15 = getParam(controller_nh, "resistance_coff_qd_15", 0.), + .resistance_coff_qd_16 = getParam(controller_nh, "resistance_coff_qd_16", 0.), + .resistance_coff_qd_18 = getParam(controller_nh, "resistance_coff_qd_18", 0.), + .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.), + .g = getParam(controller_nh, "g", 0.), + .delay = getParam(controller_nh, "delay", 0.), + .wait_next_armor_delay = getParam(controller_nh, "wait_next_armor_delay", 0.105), + .wait_diagonal_armor_delay = getParam(controller_nh, "wait_diagonal_armor_delay", 0.105), + .dt = getParam(controller_nh, "dt", 0.), + .timeout = getParam(controller_nh, "timeout", 0.), + .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), + .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0), + .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 2.0), + .switch_duration_scale = getParam(controller_nh, "switch_duration_scale", 0.), + .switch_duration_rate = getParam(controller_nh, "switch_duration_rate", 0.), + .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.08), + .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 4.5), + .max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5), + .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), + .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_); @@ -95,7 +99,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) shoot_beforehand_cmd_pub_.reset( new realtime_tools::RealtimePublisher(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); } @@ -152,12 +155,21 @@ 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_ ? M_PI / armors_num - (2 * rough_fly_time + config_.gimbal_switch_duration) / 2 * abs(v_yaw) : - min_switch_angle; + 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 * yaw_subtract; + 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.)) @@ -178,10 +190,14 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } } double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 2 * M_PI / armors_num; - is_in_delay_before_switch_ = - ((((filtered_aim_armor_yaw + v_yaw * config_.delay) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((filtered_aim_armor_yaw + v_yaw * config_.delay) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && - track_target_; + 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; @@ -267,6 +283,8 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge r = r2; z = pos.z + dz; } + pos.x += vel.x * config_.track_move_target_delay; + pos.y += vel.y * config_.track_move_target_delay; if (track_target_) { armor_pos.x = pos.x - r * cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + @@ -395,13 +413,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() < config_.gimbal_switch_duration - config_.delay) + 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() < config_.gimbal_switch_duration) && + 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_) @@ -433,10 +459,12 @@ 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_angle_offset = init_config.switch_angle_offset; + 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; @@ -455,10 +483,12 @@ 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_angle_offset = config.switch_angle_offset, + .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 4c21833a..e38c799f 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(); From 17235e9fe6c824f3a009ca565ecab7f369ca6e0a Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Fri, 18 Jul 2025 10:12:44 +0800 Subject: [PATCH 14/19] Fix pos feedforward computation. --- rm_gimbal_controllers/src/bullet_solver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index f30c2fbf..22d8df89 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -283,8 +283,8 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge r = r2; z = pos.z + dz; } - pos.x += vel.x * config_.track_move_target_delay; - pos.y += vel.y * config_.track_move_target_delay; + 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 + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + From eda2ffebde1778450d772452213d68d10baf3ad2 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Sun, 20 Jul 2025 09:22:53 +0800 Subject: [PATCH 15/19] Delete angle offset. --- .../include/rm_gimbal_controllers/bullet_solver.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 3 --- 2 files changed, 1 insertion(+), 4 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 e81ba1dc..fb4f7808 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -57,7 +57,7 @@ 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, max_switch_angle, - min_switch_angle, switch_angle_offset, switch_duration_scale, switch_duration_rate, switch_duration_offset, + 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; }; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 22d8df89..50f999eb 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -58,7 +58,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .timeout = getParam(controller_nh, "timeout", 0.), .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0), - .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 2.0), .switch_duration_scale = getParam(controller_nh, "switch_duration_scale", 0.), .switch_duration_rate = getParam(controller_nh, "switch_duration_rate", 0.), .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.08), @@ -461,7 +460,6 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.timeout = init_config.timeout; config.max_switch_angle = init_config.max_switch_angle; config.min_switch_angle = init_config.min_switch_angle; - config.switch_angle_offset = init_config.switch_angle_offset; 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; @@ -485,7 +483,6 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .timeout = config.timeout, .max_switch_angle = config.max_switch_angle, .min_switch_angle = config.min_switch_angle, - .switch_angle_offset = config.switch_angle_offset, .switch_duration_scale = config.switch_duration_scale, .switch_duration_rate = config.switch_duration_rate, .switch_duration_offset = config.switch_duration_offset, From 88f49b8ade3dca413d037d55f637d999849013bf Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Sun, 20 Jul 2025 09:23:20 +0800 Subject: [PATCH 16/19] Fix code format. --- .../include/rm_gimbal_controllers/bullet_solver.h | 4 ++-- 1 file changed, 2 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 fb4f7808..688c5bb0 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -57,8 +57,8 @@ 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, 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; + 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; }; From 5602c2bfe59ccb5600d02d25a4c4a5b428b39c2a Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Sun, 20 Jul 2025 10:02:41 +0800 Subject: [PATCH 17/19] Delete unused variable. --- .../include/rm_gimbal_controllers/gimbal_base.h | 1 - rm_gimbal_controllers/src/gimbal_base.cpp | 1 - 2 files changed, 2 deletions(-) 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 Date: Sun, 20 Jul 2025 10:52:49 +0800 Subject: [PATCH 18/19] Fix compute wrong. --- rm_gimbal_controllers/src/bullet_solver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 50f999eb..bf47b757 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -292,10 +292,10 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge selected_armor_ * 2 * M_PI / armors_num); armor_pos.z = z; armor_vel.x = vel.x + v_yaw * r * - sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + + cos(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) + + sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + selected_armor_ * 2 * M_PI / armors_num); armor_vel.z = vel.z; } From 97d9a4fd05f23aa5709c9e05b1255b18bcd44709 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Sun, 20 Jul 2025 15:25:47 +0800 Subject: [PATCH 19/19] Rollback armor pos compute. --- rm_gimbal_controllers/src/bullet_solver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index bf47b757..50f999eb 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -292,10 +292,10 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge selected_armor_ * 2 * M_PI / armors_num); armor_pos.z = z; armor_vel.x = vel.x + v_yaw * r * - cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + + 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 * - sin(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + + cos(yaw + v_yaw * (fly_time_ + config_.track_rotate_target_delay) + selected_armor_ * 2 * M_PI / armors_num); armor_vel.z = vel.z; }