From d7288d6156ade49866a5dcca361f25bc7629e22d Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 17 Mar 2026 23:43:46 +0800 Subject: [PATCH 01/31] Fix center mode . --- rm_gimbal_controllers/src/bullet_solver.cpp | 5 +++++ rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 4c45ef5d..447c12ce 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -414,6 +414,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { traject_output_yaw_ = output_yaw_; } + + if (!track_target_) + { + traject_output_yaw_ = output_yaw_; + } if (bullet_solver_pub->trylock()) { bullet_solver_pub->msg_.selected_armor_ = selected_armor_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 68eecddb..f315675d 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -540,7 +540,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } if (state_ == TRACK) { - if (bullet_solver_->getUsingtraject()) + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) { ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + From aa8544dbe99a5b12d3dde481a9e88dddc5d669eb Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Mon, 23 Mar 2026 16:57:02 +0800 Subject: [PATCH 02/31] Add the coff of target distance ,fix the bug of using joint vel,optimize the ban shoot time. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 1 + .../rm_gimbal_controllers/bullet_solver.h | 13 +-- rm_gimbal_controllers/src/bullet_solver.cpp | 82 +++++++++++++------ 3 files changed, 65 insertions(+), 31 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index cae9b458..2d2ee390 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -10,6 +10,7 @@ gen.add("resistance_coff_qd_15", double_t, 0, "Air resistance divided by mass of gen.add("resistance_coff_qd_16", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) +gen.add("resistance_coff_qd_800", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5) 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 6a4ea233..22f0e095 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -57,10 +57,10 @@ 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, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale, - switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, track_rotate_target_delay, - track_move_target_delay; + double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, + resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, center_delay, max_switch_angle, + switch_angle_offset, switch_duration_scale, switch_duration_rate, switch_duration_offset, + min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay; int min_fit_switch_count; int max_selected_armor_; double traject_ahead_; @@ -85,7 +85,7 @@ class BulletSolver double r1, double r2, double dz, int armors_num, double start_vel); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); - double getResistanceCoefficient(double bullet_speed) const; + double getResistanceCoefficient(double target_distance) const; double getYaw() const { return output_yaw_; @@ -153,6 +153,7 @@ class BulletSolver double traject_switch_time_{}; double switche_time_yaw_{}; double traject_max_acc_{}; + double last_output_yaw_{}; int shoot_num_ = 0; int shoot_beforehand_cmd_{}; int count_; @@ -177,6 +178,8 @@ class BulletSolver visualization_msgs::Marker marker_real_; ros::Time start_using_traject_time; ros::Time ban_shoot_time_; + ros::Time last_output_time_; + ros::Time ban_shoot_start_time_; TrajectoryFunctionCoefficients trajectory_function_coefficients; TrajectoryLimitParams stauts_limit_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 447c12ce..7faa6585 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -51,6 +51,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .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.), + .resistance_coff_qd_800 = getParam(controller_nh, "resistance_coff_qd_800", 1.1), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), .center_delay = getParam(controller_nh, "center_delay", 0.0), @@ -108,20 +109,30 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) &BulletSolver::heatCB, this); } -double BulletSolver::getResistanceCoefficient(double bullet_speed) const +double BulletSolver::getResistanceCoefficient(double target_distance) const { // bullet_speed have 5 value:10,15,16,18,30 double resistance_coff; - if (bullet_speed < 12.5) - resistance_coff = config_.resistance_coff_qd_10; - else if (bullet_speed < 15.5) - resistance_coff = config_.resistance_coff_qd_15; - else if (bullet_speed < 17) - resistance_coff = config_.resistance_coff_qd_16; - else if (bullet_speed < 24) - resistance_coff = config_.resistance_coff_qd_18; + if (target_distance < 3.3) + resistance_coff = config_.resistance_coff_qd_1 + (target_distance - 2.3) + + (config_.resistance_coff_qd_10 - config_.resistance_coff_qd_1); + else if (target_distance < 4.3 && target_distance > 3.3) + resistance_coff = config_.resistance_coff_qd_10 + + (target_distance - 3.3) * (config_.resistance_coff_qd_15 - config_.resistance_coff_qd_10); + else if (target_distance < 5.3) + resistance_coff = config_.resistance_coff_qd_15 + + (target_distance - 4.3) * (config_.resistance_coff_qd_16 - config_.resistance_coff_qd_15); + else if (target_distance < 6.3) + resistance_coff = config_.resistance_coff_qd_16 + + (target_distance - 5.3) * (config_.resistance_coff_qd_18 - config_.resistance_coff_qd_16); + else if (target_distance < 7.3) + resistance_coff = config_.resistance_coff_qd_18 + + (target_distance - 6.3) * (config_.resistance_coff_qd_30 - config_.resistance_coff_qd_18); + else if (target_distance < 8.3) + resistance_coff = config_.resistance_coff_qd_30 + + (target_distance - 7.3) * (config_.resistance_coff_qd_800 - config_.resistance_coff_qd_30); else - resistance_coff = config_.resistance_coff_qd_30; + resistance_coff = config_.resistance_coff_qd_800; return resistance_coff; } @@ -130,7 +141,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; - resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; + // 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) @@ -141,6 +152,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05; double temp_z = pos.z; double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); + resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; double output_yaw_central = std::atan2(pos.y, pos.x); output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); double rough_fly_time = @@ -165,8 +177,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double move_switch_time_coff_ = v_yaw * vel.x > 0.0 ? config_.move_switch_time_coff_ : -config_.move_switch_time_coff_; double move_switch_time_offset = move_switch_time_coff_ * vel.x; - traject_switch_time_ = - 1.321 * exp(-0.289 * abs(filtered_v_yaw_ + 1.0)) * config_.traject_ahead_ + move_switch_time_offset; + traject_switch_time_ = 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ + move_switch_time_offset; + // 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ * (1 + (target_rho - 2.0) * 0.05) + move_switch_time_offset ; // traject_switch_time_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_; yaw_subtract_ = filtered_yaw_ - output_yaw_central; while (yaw_subtract_ > M_PI) @@ -184,6 +196,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { selected_armor_ = 0; } + selected_armor_ = 0; if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) || (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.)) @@ -266,22 +279,22 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); - if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.5) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.5)) + if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.7)) { selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1; } if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.5) || + (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.7) || (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.5))) + (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.7))) { selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; } if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.5) || + (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.7) || (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.5))) + (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.7))) { selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; } @@ -331,7 +344,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); } stauts_limit_.start_pos = output_yaw_; - stauts_limit_.start_vel = start_vel; + stauts_limit_.start_vel = (output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + ; stauts_limit_.end_pos = std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; stauts_limit_.end_vel = stauts_limit_.start_vel; @@ -426,9 +440,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.test_number = switch_armor_time_.toSec(); + bullet_solver_pub->msg_.test_number = ban_shoot_start_time_.toSec(); bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; + bullet_solver_pub->msg_.pos_x = pos.x; + bullet_solver_pub->msg_.pos_y = pos.y; if (v_yaw > 1.0) { bullet_solver_pub->msg_.center_this_armor_ = @@ -454,6 +470,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d bullet_solver_pub->unlockAndPublish(); } last_selected_armor_ = selected_armor_; + last_output_time_ = ros::Time::now(); + last_output_yaw_ = output_yaw_; return true; } void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, @@ -613,16 +631,27 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel) { - if ((ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) && + if ((ros::Time::now().toSec() >= (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) && (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime)) + { + ban_shoot_count_++; shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (ros::Time::now().toSec() > - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime && - ros::Time::now().toSec() < - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime + config_.delay) + if (ban_shoot_count_ == 3) + { + ban_shoot_start_time_ = ros::Time::now(); + } + } + else if (ros::Time::now().toSec() > ban_shoot_start_time_.toSec() + switchtime && + ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime + config_.delay) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + ban_shoot_count_ = 0; + } else + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + ban_shoot_count_ = 0; + } } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; @@ -695,6 +724,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_16 = config.resistance_coff_qd_16, .resistance_coff_qd_18 = config.resistance_coff_qd_18, .resistance_coff_qd_30 = config.resistance_coff_qd_30, + .resistance_coff_qd_800 = config.resistance_coff_qd_800, .g = config.g, .delay = config.delay, .center_delay = config.center_delay, From 68a1d45b211eae517fe1328f3785871c2df47056 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Wed, 25 Mar 2026 19:45:46 +0800 Subject: [PATCH 03/31] Using position to start traject but not time directly. --- .../rm_gimbal_controllers/bullet_solver.h | 3 ++ rm_gimbal_controllers/src/bullet_solver.cpp | 40 +++++++++---------- 2 files changed, 23 insertions(+), 20 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 22f0e095..19d51151 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -162,6 +162,7 @@ class BulletSolver int ban_shoot_count_ = 0; int selected_armor_ = 0; int last_selected_armor_ = 0; + int vision_count_ = 0; bool track_target_ = true; bool identified_target_change_ = true; @@ -171,6 +172,8 @@ class BulletSolver bool using_traject_{}; bool last_shoot_state_{}; bool is_aheading_two_{}; + bool real_change_armor_ = false; + bool Advanced_planning_ = false; geometry_msgs::Point after_traject_output_yaw_{}; geometry_msgs::Point target_pos_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 7faa6585..5141153e 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -141,6 +141,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; + vision_count_++; // resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; if (abs(yaw - last_yaw_) > 1.) filtered_yaw_ = yaw; @@ -179,7 +180,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double move_switch_time_offset = move_switch_time_coff_ * vel.x; traject_switch_time_ = 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ + move_switch_time_offset; // 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ * (1 + (target_rho - 2.0) * 0.05) + move_switch_time_offset ; - // traject_switch_time_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_; + traject_switch_time_ = (switch_armor_angle - switche_time_yaw_) / filtered_v_yaw_ * 0.8; yaw_subtract_ = filtered_yaw_ - output_yaw_central; while (yaw_subtract_ > M_PI) yaw_subtract_ -= 2 * M_PI; @@ -197,18 +198,17 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; } selected_armor_ = 0; - + if (change_armor) + { + vision_count_ = 0; + count_ = 0; + next_count_ = 0; + change_armor = false; + } if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) || (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.)) { count_++; - if (change_armor) - { - count_ = 0; - next_count_ = 0; - change_armor = false; - switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; - } if (count_ >= config_.min_fit_switch_count) { selected_armor_ = v_yaw > 0. ? -1 : 1; @@ -258,16 +258,16 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d start_using_traject_time = ros::Time::now(); } } - double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 1.57; - double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_central; - 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 + filtered_v_yaw_ * config_.delay) > switch_armor_angle) && filtered_v_yaw_ > 1.) || - (((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) < -switch_armor_angle) && filtered_v_yaw_ < -1.)) && - track_target_; + if ((count_ == 3 && vision_count_ != 3) || (next_count_ == 3 && vision_count_ != 3) || + (vision_count_ == 3 && count_ < 3 && next_count_ < 3)) + { + real_change_armor_ = true; + switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; + } + else + { + real_change_armor_ = false; + } int count{}; double error = 999; if (track_target_) @@ -440,7 +440,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.test_number = ban_shoot_start_time_.toSec(); + bullet_solver_pub->msg_.test_number = real_change_armor_; bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; bullet_solver_pub->msg_.pos_x = pos.x; From 7853055a01f0f80372dfe06e31d78068e8fba8a5 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 27 Mar 2026 20:35:16 +0800 Subject: [PATCH 04/31] Optimize the advanced planning problem , add the debug msgs. --- .../rm_gimbal_controllers/bullet_solver.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 99 ++++++++++--------- 2 files changed, 54 insertions(+), 47 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 19d51151..3168d415 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -173,7 +173,7 @@ class BulletSolver bool last_shoot_state_{}; bool is_aheading_two_{}; bool real_change_armor_ = false; - bool Advanced_planning_ = false; + bool Advanced_planning_ = true; geometry_msgs::Point after_traject_output_yaw_{}; geometry_msgs::Point target_pos_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 5141153e..a6db767f 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -175,10 +175,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d track_target_ = true; } switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; - double move_switch_time_coff_ = - v_yaw * vel.x > 0.0 ? config_.move_switch_time_coff_ : -config_.move_switch_time_coff_; - double move_switch_time_offset = move_switch_time_coff_ * vel.x; - traject_switch_time_ = 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ + move_switch_time_offset; + // double move_switch_time_coff_ = + // v_yaw * vel.x > 0.0 ? config_.move_switch_time_coff_ : -config_.move_switch_time_coff_; + // double move_switch_time_offset = move_switch_time_coff_ * vel.x; + // traject_switch_time_ = 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ + move_switch_time_offset; // 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ * (1 + (target_rho - 2.0) * 0.05) + move_switch_time_offset ; traject_switch_time_ = (switch_armor_angle - switche_time_yaw_) / filtered_v_yaw_ * 0.8; yaw_subtract_ = filtered_yaw_ - output_yaw_central; @@ -187,16 +187,6 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d while (yaw_subtract_ < -M_PI) yaw_subtract_ += 2 * M_PI; double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; - if (((after_fly_yaw_subtract_ < switch_armor_angle - 0.1 && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ > switch_armor_angle + 0.1 && filtered_v_yaw_ < -1.)) && - track_target_) - { - selected_armor_ = 0; - } - else if (!track_target_) - { - selected_armor_ = 0; - } selected_armor_ = 0; if (change_armor) { @@ -217,6 +207,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) { selected_armor_ = -2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; next_count_++; if (next_count_ == config_.min_fit_switch_count) { @@ -227,6 +219,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < switch_armor_angle) && v_yaw < 0.) { selected_armor_ = 2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; next_count_++; if (next_count_ == config_.min_fit_switch_count) { @@ -255,13 +249,35 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (traject_count_ == 2) { using_traject_ = true; + Advanced_planning_ = true; start_using_traject_time = ros::Time::now(); } } - if ((count_ == 3 && vision_count_ != 3) || (next_count_ == 3 && vision_count_ != 3) || - (vision_count_ == 3 && count_ < 3 && next_count_ < 3)) + if (!using_traject_ && Advanced_planning_ && abs(v_yaw) > 4.0) + { + if (selected_armor_ == 0) + { + selected_armor_ = v_yaw > 0. ? -1 : 1; + z = pos.z + dz; + r = armors_num == 4 ? r2 : r1; + } + else if (selected_armor_ == -1 || selected_armor_ == 1) + { + selected_armor_ = v_yaw > 0. ? -2 : 2; + z = pos.z; + r = armors_num == 4 ? r1 : r2; + } + else if (selected_armor_ == -2 || selected_armor_ == 2) + { + selected_armor_ = v_yaw > 0. ? -3 : 3; + z = pos.z + dz; + r = armors_num == 4 ? r2 : r1; + } + } + if ((count_ == 3 && vision_count_ != 3) || (next_count_ == 3 && vision_count_ != 3)) { real_change_armor_ = true; + Advanced_planning_ = false; switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; } else @@ -272,8 +288,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double error = 999; if (track_target_) { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 1.57); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.57); + target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * M_PI / 2); + target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * M_PI / 2); } else { @@ -310,9 +326,12 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } } target_pos_.z = z; - + if (abs(filtered_v_yaw_) < 1.0) + { + selected_armor_ = 0; + } double r_trajcet_; - if (selected_armor_ == 1 || selected_armor_ == -1) + if (selected_armor_ == 1 || selected_armor_ == -1 || selected_armor_ == 3 || selected_armor_ == -3) { r_trajcet_ = r1; } @@ -345,7 +364,6 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } stauts_limit_.start_pos = output_yaw_; stauts_limit_.start_vel = (output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); - ; stauts_limit_.end_pos = std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; stauts_limit_.end_vel = stauts_limit_.start_vel; @@ -435,38 +453,27 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } if (bullet_solver_pub->trylock()) { + bullet_solver_pub->msg_.test_number = real_change_armor_; bullet_solver_pub->msg_.selected_armor_ = selected_armor_; bullet_solver_pub->msg_.yaw_subtract_ = yaw_subtract_; bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.test_number = real_change_armor_; bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; - bullet_solver_pub->msg_.pos_x = pos.x; - bullet_solver_pub->msg_.pos_y = pos.y; - if (v_yaw > 1.0) - { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 3 * 2 * M_PI / armors_num); - } - else if (v_yaw < -1.0) - { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 3 * 2 * M_PI / armors_num); - } + bullet_solver_pub->msg_.pos_distance_ = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); + bullet_solver_pub->msg_.Advanced_planning_ = Advanced_planning_; + bullet_solver_pub->msg_.traject_switch_time_ = traject_switch_time_; + bullet_solver_pub->msg_.center_this_armor_ = (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - + v_yaw / abs(v_yaw) * 0 * 2 * M_PI / armors_num); + bullet_solver_pub->msg_.center_next_armor_ = (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - + v_yaw / abs(v_yaw) * 1 * 2 * M_PI / armors_num); + bullet_solver_pub->msg_.center_diagonal_armor_ = + (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - + v_yaw / abs(v_yaw) * 2 * 2 * M_PI / armors_num); + bullet_solver_pub->msg_.center_triple_next_armor_ = + (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - + v_yaw / abs(v_yaw) * 3 * 2 * M_PI / armors_num); bullet_solver_pub->unlockAndPublish(); } last_selected_armor_ = selected_armor_; From eb120617a7be31a4fd44b747a0959d737bc1c79a Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Mon, 6 Apr 2026 13:01:16 +0800 Subject: [PATCH 05/31] Add switch time self-iteration code,fix the sign issue of trajectory time,and optimize the velocity expectation during trajectory planning. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 3 + .../rm_gimbal_controllers/bullet_solver.h | 11 +- .../rm_gimbal_controllers/gimbal_base.h | 5 +- rm_gimbal_controllers/src/bullet_solver.cpp | 171 ++++++++++-------- rm_gimbal_controllers/src/gimbal_base.cpp | 27 ++- 5 files changed, 136 insertions(+), 81 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 2d2ee390..b8eb14c7 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -25,9 +25,12 @@ gen.add("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) +gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -200.0, 200.0) gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0) gen.add("move_switch_time_coff_",double_t, 0, "The offset of the traject_switch_time", 0.0 , -1.0, 1.0) +gen.add("traject_k_effort",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) +gen.add("traject_k_vel",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) 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 3168d415..a3f6f400 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -60,12 +60,13 @@ struct Config double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale, switch_duration_rate, switch_duration_offset, - min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay; + min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay, yaw_max_acc; int min_fit_switch_count; int max_selected_armor_; double traject_ahead_; int clean_shoot_num_; double end_pos_offset, move_switch_time_coff_; + double traject_k_effort, traject_k_vel_; }; struct TrajectoryFunctionCoefficients { @@ -98,6 +99,10 @@ class BulletSolver { return traject_output_yaw_; } + double getTrajectVel() const + { + return traject_vel_; + } bool getUsingtraject() const { return using_traject_; @@ -122,7 +127,7 @@ class BulletSolver void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); - double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_); + double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw); void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg); ~BulletSolver() = default; @@ -154,6 +159,7 @@ class BulletSolver double switche_time_yaw_{}; double traject_max_acc_{}; double last_output_yaw_{}; + double traject_vel_{}; int shoot_num_ = 0; int shoot_beforehand_cmd_{}; int count_; @@ -174,6 +180,7 @@ class BulletSolver bool is_aheading_two_{}; bool real_change_armor_ = false; bool Advanced_planning_ = true; + bool need_solve_traject_ = false; geometry_msgs::Point after_traject_output_yaw_{}; geometry_msgs::Point target_pos_{}; 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 90ee90b5..7ce39863 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -56,6 +56,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -162,6 +163,8 @@ class Controller : public controller_interface::MultiInterfaceController> pid_pos_; std::unordered_map joint_urdfs_; std::unordered_map pos_des_in_limit_; + std::unordered_map>> tracking_differentiator_; + std::unordered_map last_pos_des_; bool has_imu_ = true; std::shared_ptr bullet_solver_; @@ -213,7 +216,7 @@ class Controller : public controller_interface::MultiInterfaceController 1.) filtered_yaw_ = yaw; else if (last_yaw_ != yaw) @@ -180,7 +182,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d // double move_switch_time_offset = move_switch_time_coff_ * vel.x; // traject_switch_time_ = 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ + move_switch_time_offset; // 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ * (1 + (target_rho - 2.0) * 0.05) + move_switch_time_offset ; - traject_switch_time_ = (switch_armor_angle - switche_time_yaw_) / filtered_v_yaw_ * 0.8; + traject_switch_time_ = (abs(filtered_v_yaw_) / filtered_v_yaw_ * switch_armor_angle - switche_time_yaw_) / + filtered_v_yaw_ * config_.traject_ahead_; yaw_subtract_ = filtered_yaw_ - output_yaw_central; while (yaw_subtract_ > M_PI) yaw_subtract_ -= 2 * M_PI; @@ -195,8 +198,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d next_count_ = 0; change_armor = false; } - if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.)) + if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 3.) || + (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -3.)) { count_++; if (count_ >= config_.min_fit_switch_count) @@ -216,7 +219,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d is_aheading_two_ = true; } } - else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < switch_armor_angle) && v_yaw < 0.) + else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < -switch_armor_angle) && v_yaw < 0.) { selected_armor_ = 2; r = armors_num == 4 ? r1 : r2; @@ -253,27 +256,27 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d start_using_traject_time = ros::Time::now(); } } - if (!using_traject_ && Advanced_planning_ && abs(v_yaw) > 4.0) - { - if (selected_armor_ == 0) - { - selected_armor_ = v_yaw > 0. ? -1 : 1; - z = pos.z + dz; - r = armors_num == 4 ? r2 : r1; - } - else if (selected_armor_ == -1 || selected_armor_ == 1) - { - selected_armor_ = v_yaw > 0. ? -2 : 2; - z = pos.z; - r = armors_num == 4 ? r1 : r2; - } - else if (selected_armor_ == -2 || selected_armor_ == 2) - { - selected_armor_ = v_yaw > 0. ? -3 : 3; - z = pos.z + dz; - r = armors_num == 4 ? r2 : r1; - } - } + // if (!using_traject_ && Advanced_planning_ && abs(v_yaw) > 4.0) + // { + // if (selected_armor_ == 0) + // { + // selected_armor_ = v_yaw > 0. ? -1 : 1; + // z = pos.z + dz; + // r = armors_num == 4 ? r2 : r1; + // } + // else if (selected_armor_ == -1 || selected_armor_ == 1) + // { + // selected_armor_ = v_yaw > 0. ? -2 : 2; + // z = pos.z; + // r = armors_num == 4 ? r1 : r2; + // } + // else if (selected_armor_ == -2 || selected_armor_ == 2) + // { + // selected_armor_ = v_yaw > 0. ? -3 : 3; + // z = pos.z + dz; + // r = armors_num == 4 ? r2 : r1; + // } + // } if ((count_ == 3 && vision_count_ != 3) || (next_count_ == 3 && vision_count_ != 3)) { real_change_armor_ = true; @@ -295,21 +298,21 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { target_pos_.x = pos.x - r * cos(atan2(pos.y, pos.x)); target_pos_.y = pos.y - r * sin(atan2(pos.y, pos.x)); - if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.7) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.7)) + if ((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.7)) { selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1; } - if (((filtered_v_yaw_ > 1.0 && + if (((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.7) || - (filtered_v_yaw_ < -1.0 && + (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.7))) { selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; } - if (((filtered_v_yaw_ > 1.0 && + if (((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.7) || - (filtered_v_yaw_ < -1.0 && + (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.7))) { selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; @@ -329,6 +332,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (abs(filtered_v_yaw_) < 1.0) { selected_armor_ = 0; + r = r1; } double r_trajcet_; if (selected_armor_ == 1 || selected_armor_ == -1 || selected_armor_ == 3 || selected_armor_ == -3) @@ -342,50 +346,55 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (!using_traject_) { - switchtime = 0.08; - - if (filtered_v_yaw_ > 0) + traject_max_acc_ = 200.; + switchtime = 0.05; + while (abs(traject_max_acc_) > config_.yaw_max_acc) { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - } - else - { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); - } - stauts_limit_.start_pos = output_yaw_; - stauts_limit_.start_vel = (output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); - stauts_limit_.end_pos = - std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; - stauts_limit_.end_vel = stauts_limit_.start_vel; + switchtime += 0.005; + if (filtered_v_yaw_ > 0) + { + after_traject_output_yaw_.x = + pos.x + vel.x * (fly_time_ + switchtime) - + r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = + pos.y + vel.y * (fly_time_ + switchtime) - + r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); + } + else + { + after_traject_output_yaw_.x = + pos.x + vel.x * (fly_time_ + switchtime) - + r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = + pos.y + vel.y * (fly_time_ + switchtime) - + r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); + } + stauts_limit_.start_pos = output_yaw_; + stauts_limit_.start_vel = + (output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + stauts_limit_.end_pos = + std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; + stauts_limit_.end_vel = stauts_limit_.start_vel; - if (switchtime > 0.2) - { - using_traject_ = false; - } - trajectory_function_coefficients.a0 = stauts_limit_.start_pos; - trajectory_function_coefficients.a1 = stauts_limit_.start_vel; + if (switchtime > 0.12) + { + break; + } + trajectory_function_coefficients.a0 = stauts_limit_.start_pos; + trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - Eigen::Matrix2d A; - A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); - Eigen::Vector2d B; - B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), - stauts_limit_.end_vel - stauts_limit_.start_vel; + Eigen::Matrix2d A; + A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); + Eigen::Vector2d B; + B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), + stauts_limit_.end_vel - stauts_limit_.start_vel; - Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); + Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); - trajectory_function_coefficients.a2 = X(0); - trajectory_function_coefficients.a3 = X(1); - traject_max_acc_ = 2 * trajectory_function_coefficients.a2; + trajectory_function_coefficients.a2 = X(0); + trajectory_function_coefficients.a3 = X(1); + traject_max_acc_ = 2 * trajectory_function_coefficients.a2; + } } while (error >= 0.001) @@ -436,7 +445,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (using_traject_) { ros::Time temp = ros::Time::now(); - traject_output_yaw_ = planningPoint(temp, start_using_traject_time); + traject_output_yaw_ = planningPoint(temp, start_using_traject_time, v_yaw); if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) { using_traject_ = false; @@ -464,6 +473,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d bullet_solver_pub->msg_.pos_distance_ = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); bullet_solver_pub->msg_.Advanced_planning_ = Advanced_planning_; bullet_solver_pub->msg_.traject_switch_time_ = traject_switch_time_; + bullet_solver_pub->msg_.switchtime = switchtime; + bullet_solver_pub->msg_.using_traject_ = using_traject_; + bullet_solver_pub->msg_.traject_effort_ff_ = traject_effort_ff_; + bullet_solver_pub->msg_.traject_vel_ = switch_armor_time_.toSec(); bullet_solver_pub->msg_.center_this_armor_ = (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - v_yaw / abs(v_yaw) * 0 * 2 * M_PI / armors_num); bullet_solver_pub->msg_.center_next_armor_ = (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - @@ -670,7 +683,7 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) } } -double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_) +double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw) { double a0 = trajectory_function_coefficients.a0; double a1 = trajectory_function_coefficients.a1; @@ -678,7 +691,9 @@ double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_ double a3 = trajectory_function_coefficients.a3; double n_time_ = (time - start_trajectory_time_).toSec(); double plansetpoint = a0 + a1 * n_time_ + a2 * pow(n_time_, 2) + a3 * pow(n_time_, 3); - traject_effort_ff_ = 1.4 - 1.4 * n_time_ / switchtime; + traject_effort_ff_ = (1.2 - 1.2 * n_time_ / switchtime) * abs(v_yaw) / v_yaw * config_.traject_k_effort; + // traject_effort_ff_ = (2 * a2 + 6 * a3 * n_time_) / (2 * a2) * 1.0 * config_.traject_k_effort; + traject_vel_ = (a1 + 2 * a2 * n_time_ + 3 * a3 * pow(n_time_, 2)) * config_.traject_k_vel_; return plansetpoint; } @@ -718,12 +733,15 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; + config.yaw_max_acc = init_config.yaw_max_acc; config.min_fit_switch_count = init_config.min_fit_switch_count; config.max_selected_armor_ = init_config.max_selected_armor_; config.traject_ahead_ = init_config.traject_ahead_; config.clean_shoot_num_ = init_config.clean_shoot_num_; config.end_pos_offset = init_config.end_pos_offset; config.move_switch_time_coff_ = init_config.move_switch_time_coff_; + config.traject_k_effort = init_config.traject_k_effort; + config.traject_k_vel = init_config.traject_k_vel_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -743,12 +761,15 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, + .yaw_max_acc = config.yaw_max_acc, .min_fit_switch_count = config.min_fit_switch_count, .max_selected_armor_ = config.max_selected_armor_, .traject_ahead_ = config.traject_ahead_, .clean_shoot_num_ = config.clean_shoot_num_, .end_pos_offset = config.end_pos_offset, - .move_switch_time_coff_ = config.move_switch_time_coff_ }; + .move_switch_time_coff_ = config.move_switch_time_coff_, + .traject_k_effort = config.traject_k_effort, + .traject_k_vel_ = config.traject_k_vel }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index f315675d..9c9b6d2f 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -109,7 +109,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); } - + double r = getParam(nh, "r", 999.); + tracking_differentiator_.insert( + std::make_pair(axis, std::make_unique>(r, 0.001))); ctrls_.insert(std::make_pair(axis, std::make_unique())); pid_pos_.insert(std::make_pair(axis, std::make_unique())); pos_des_in_limit_.insert(std::make_pair(axis, true)); @@ -135,6 +137,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro { ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } + if (has_imu_) + { + ROS_WARN_ONCE("has imu"); + } gimbal_des_frame_id_ = getGimbalFrameID(joint_urdfs_) + "_des"; odom2gimbal_des_.header.frame_id = "odom"; @@ -499,6 +505,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) 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); } + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + { + vel_des[2] = bullet_solver_->getTrajectVel(); + } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, @@ -520,6 +530,15 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (!in_limit.second) vel_des[in_limit.first] = 0.; + // for (auto& td : tracking_differentiator_) + // { + // td.second->update(last_pos_des_[td.first] + + // angles::shortest_angular_distance(last_pos_des_[td.first], pos_des[td.first]), + // vel_des[td.first],0.5); + // last_pos_des_[td.first] += angles::shortest_angular_distance(last_pos_des_[td.first], pos_des[td.first]); + // pos_des_temp[td.first] = std::remainder(td.second->getX1(), 2 * M_PI); + // angle_error[td.first] = angles::shortest_angular_distance(pos_real[td.first], pos_des_temp[td.first]); + // } if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) { pid_pos_.at(1)->computeCommand(angle_error[1], period); @@ -544,8 +563,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z + - bullet_solver_->getTrajectEffortff()); + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd += bullet_solver_->getTrajectEffortff(); + ctrls_.at(2)->joint_.setCommand(cmd); } else { From cdaff9bbc4da5871b848840e7c9572aec38c3ef5 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 12 Apr 2026 21:47:56 +0800 Subject: [PATCH 06/31] Write the mpc trajectory planning class . --- .../rm_gimbal_controllers/bullet_solver.h | 2 +- .../rm_gimbal_controllers/gimbal_base.h | 5 + .../mpc_trajectory_planning.h | 144 ++++++++++++++++++ rm_gimbal_controllers/src/bullet_solver.cpp | 16 +- 4 files changed, 158 insertions(+), 9 deletions(-) create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/mpc_trajectory_planning.h 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 a3f6f400..874c7272 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -83,7 +83,7 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel); + double r1, double r2, double dz, int armors_num, double start_vel, double next_n); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); double getResistanceCoefficient(double target_distance) const; 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 7ce39863..c7c2738e 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -57,6 +57,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -168,6 +169,10 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; + std::vector> bullet_solver_MPC_; + std::vector bullet_solver_MPC_solve_success_; + Eigen::VectorXd bullet_pos_des = Eigen::VectorXd::Zero(201); + std::shared_ptr mpc_trajectory_planning_; // ROS Interface ros::Time last_publish_time_{}; diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/mpc_trajectory_planning.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/mpc_trajectory_planning.h new file mode 100644 index 00000000..8c3d0c10 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/mpc_trajectory_planning.h @@ -0,0 +1,144 @@ +// +// Created by xuncheng on 2026/4/11. +// + +#ifndef RM_WS_MPC_TRAJERTORY_PLANNING_H +#define RM_WS_MPC_TRAJERTORY_PLANNING_H +#include +#include + +namespace rm_gimbal_controllers +{ +class MPC_Trajectory_Planning +{ +public: + // Eigen -> OSQP 转换函数 + OSQPCscMatrix* eigen2osqp(const Eigen::SparseMatrix& mat) + { + Eigen::SparseMatrix mat_csc = mat; + mat_csc.makeCompressed(); + + OSQPInt m = mat_csc.rows(); + OSQPInt n = mat_csc.cols(); + OSQPInt nz = mat_csc.nonZeros(); + + std::vector inner(mat_csc.innerIndexPtr(), mat_csc.innerIndexPtr() + nz); + std::vector outer(mat_csc.outerIndexPtr(), mat_csc.outerIndexPtr() + n + 1); + + OSQPCscMatrix* osqp_mat = OSQPCscMatrix_new(m, n, nz, mat_csc.valuePtr(), inner.data(), outer.data()); + + return osqp_mat; + } + + void initMatrix(Eigen::Vector2d x_self) + { + A << 1.0, 0.001, 0.0, 1.0; + B << 0.0, 0.001; + f << 0.0, 0.0; + Q << 10e6, 0.0, 0.0, 0.0; + R << 1.0; + I << 1.0, 0.0, 0.0, 1.0; + for (int i = 0; i < (N + 1); i++) + { + P_des.block(nx * i, nx * i, nx, nx) = Q; + P_des.block(nu * i + nx * (N + 1), nu * i + nx * N, nu, nu) = R; + } + //转化成稀疏矩阵 + P_osqp = P_des.sparseView(); + P_csc = eigen2osqp(P_osqp); + + //初始状态不可以更改 + A_(0, 0) = 1; + A_(1, 1) = 1; + l_(0) = x_self(0); + u_(0) = x_self(0); + l_(1) = x_self(1); + u_(1) = x_self(1); + x = x_self; + for (int i = 0; i < N; i++) + { + //动力学解算 + A_.block(nx * i + 2, nx * i, nx, nx) = -A; + A_.block(nx * i + 2, nx * (N + 1) + nu * i, nx, nu) = -B; + A_.block(nx * i + 2, nx * (i + 1), 2, 2) = I; + //速度限制 + A_(nx * N + 2 + i, nx * (i + 1) + 1) = 1; + l_(nx * N + 2 + i) = -max_vel_; + u_(nx * N + 2 + i) = max_vel_; + //加速度限制 + A_(nx * N + 2 + N + i, nx * (N + 1) + nu * i) = 1; + l_(nx * N + 2 + N + i) = -max_acc_; + u_(nx * N + 2 + N + i) = max_acc_; + } + A_osqp = A_.sparseView(); + A_csc = eigen2osqp(A_osqp); + } + void osqpSolver(Eigen::VectorXd bullet_pos_des) + { + for (int i = 0; i < N + 1; i++) + { + q_osqp(nx * i) = -Q(0, 0) * bullet_pos_des(i); + } + OSQPSettings* settings = OSQPSettings_new(); + osqp_set_default_settings(settings); + + OSQPSolver* solver = NULL; + osqp_setup(&solver, P_csc, q_osqp.data(), A_csc, l_.data(), u_.data(), N * 4 + 2, (nx + nu) * (N + 1), settings); + + osqp_solve(solver); + + OSQPSolution sol = OSQPSolution(); + osqp_get_solution(solver, &sol); + pos_des_ = sol.x[3]; + vel_des_ = sol.x[4]; + acc_des_ = sol.x[2]; + OSQPSettings_free(settings); + osqp_cleanup(solver); + }; + double getPosDes() + { + return pos_des_; + } + double getVelDes() + { + return vel_des_; + } + double getAccDes() + { + return acc_des_; + } + +private: + //自身状态和控制量 + Eigen::Vector2d x; + Eigen::VectorXd u; + //状态变化矩阵 + Eigen::Matrix2d A; + Eigen::Vector2d B; + //常数扰动 + Eigen::Vector2d f; + //权重矩阵 + Eigen::Matrix2d Q; + Eigen::MatrixXd R; + Eigen::Matrix2d I; + double pos_des_ = 0.0; + double vel_des_ = 0.0; + double acc_des_ = 0.0; + double max_acc_ = 40; + double max_vel_ = 8; + int N = 200; + int nx = 2; + int nu = 1; + int num = (nx + nu) * (N + 1); + Eigen::MatrixXd P_des = Eigen::MatrixXd::Zero(num, num); + Eigen::VectorXd q_osqp = Eigen::VectorXd::Zero(num); + Eigen::SparseMatrix P_osqp; + Eigen::MatrixXd A_ = Eigen::MatrixXd::Zero(N * 4 + 2, num); + Eigen::VectorXd l_ = Eigen::VectorXd::Zero(N * 4 + 2); + Eigen::VectorXd u_ = Eigen::VectorXd::Zero(N * 4 + 2); + Eigen::SparseMatrix A_osqp; + const OSQPCscMatrix* A_csc; + const OSQPCscMatrix* P_csc; +}; +} // namespace rm_gimbal_controllers +#endif // RM_WS_MPC_TRAJERTORY_PLANNING_H diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index d7d5a99c..f8d97aa9 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -140,7 +140,7 @@ double BulletSolver::getResistanceCoefficient(double target_distance) const } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel) + double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel, double next_n) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; @@ -163,9 +163,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double r = r1; double z = pos.z; if (track_target_) - yaw += filtered_v_yaw_ * config_.track_rotate_target_delay; - pos.x += vel.x * config_.track_move_target_delay; - pos.y += vel.y * config_.track_move_target_delay; + yaw += filtered_v_yaw_ * (config_.track_rotate_target_delay + next_n); + pos.x += vel.x * (config_.track_move_target_delay + next_n); + pos.y += vel.y * (config_.track_move_target_delay + next_n); if (track_target_) { if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) @@ -198,15 +198,15 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d next_count_ = 0; change_armor = false; } - if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 3.) || - (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -3.)) + if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -2.)) { count_++; if (count_ >= config_.min_fit_switch_count) { selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; - z = armors_num == 4 ? pos.z + dz : pos.z; + z = pos.z + dz; if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) { selected_armor_ = -2; @@ -594,7 +594,7 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec else { r = armors_num == 4 ? r2 : r1; - z = armors_num == 4 ? pos.z + dz : pos.z; + z = pos.z + dz; } double error; if (track_target_) From c3bf106b66fa84eee27ca46d37a9f3286f184cec Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 26 Apr 2026 21:46:06 +0800 Subject: [PATCH 07/31] Completed the time window logic, modified the firing logic, improved the speed input, and fixed the firing judgment in standard.cpp. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 5 - .../rm_gimbal_controllers/bullet_solver.h | 46 +- .../rm_gimbal_controllers/gimbal_base.h | 6 +- rm_gimbal_controllers/src/bullet_solver.cpp | 550 ++++++++---------- rm_gimbal_controllers/src/gimbal_base.cpp | 8 +- rm_shooter_controllers/src/standard.cpp | 3 +- 6 files changed, 259 insertions(+), 359 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index b8eb14c7..076cdc37 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -17,11 +17,7 @@ gen.add("center_delay", double_t, 0, "Delay of shooting this target when in cent 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, 1, 99) -gen.add("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) @@ -29,7 +25,6 @@ gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -200.0, 20 gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0) -gen.add("move_switch_time_coff_",double_t, 0, "The offset of the traject_switch_time", 0.0 , -1.0, 1.0) gen.add("traject_k_effort",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) gen.add("traject_k_vel",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.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 874c7272..d88d4570 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -59,13 +59,11 @@ struct Config { double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, center_delay, max_switch_angle, - switch_angle_offset, switch_duration_scale, switch_duration_rate, switch_duration_offset, - min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay, yaw_max_acc; + switch_angle_offset, min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay, yaw_max_acc; int min_fit_switch_count; - int max_selected_armor_; double traject_ahead_; int clean_shoot_num_; - double end_pos_offset, move_switch_time_coff_; + double end_pos_offset; double traject_k_effort, traject_k_vel_; }; struct TrajectoryFunctionCoefficients @@ -83,17 +81,17 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel, double next_n); + double r1, double r2, double dz, int armors_num, double start_vel); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); double getResistanceCoefficient(double target_distance) const; double getYaw() const { - return output_yaw_; + return output_yaw_[0]; } double getPitch() const { - return -output_pitch_; + return -output_pitch_[0]; } double getTrajectYaw() const { @@ -119,7 +117,6 @@ class BulletSolver { return track_target_; } - 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); @@ -143,10 +140,11 @@ class BulletSolver realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; + double yaw_[150], pos_x[150], pos_y[150]; double max_track_target_vel_; - double output_yaw_{}, output_pitch_{}, traject_output_yaw_{}; + double output_yaw_[150], output_pitch_[150], traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; - double fly_time_; + double fly_time_[150]; double switch_hysteresis_; double last_yaw_{}, filtered_yaw_{}; double gimbal_switch_duration_{}; @@ -160,30 +158,26 @@ class BulletSolver double traject_max_acc_{}; double last_output_yaw_{}; double traject_vel_{}; - int shoot_num_ = 0; - int shoot_beforehand_cmd_{}; - int count_; - int next_count_; - int traject_count_; - int ban_shoot_count_ = 0; - int selected_armor_ = 0; - int last_selected_armor_ = 0; - int vision_count_ = 0; + double r_traject_{}; + int shoot_num_{}; + int shoot_beforehand_cmd_{}; + int count_[150]{}; + int next_count_[150]{}; + int ban_shoot_count_{}; + int selected_armor_[150] = {}; + int last_selected_armor_ = {}; bool track_target_ = true; bool identified_target_change_ = true; - bool is_in_delay_before_switch_{}; bool dynamic_reconfig_initialized_{}; - bool change_armor = false; bool using_traject_{}; bool last_shoot_state_{}; - bool is_aheading_two_{}; - bool real_change_armor_ = false; - bool Advanced_planning_ = true; - bool need_solve_traject_ = false; + bool is_aheading_two_[150]{}; + bool start_traject_{}; + double filtered_vel_des{}; geometry_msgs::Point after_traject_output_yaw_{}; - geometry_msgs::Point target_pos_{}; + geometry_msgs::Point target_pos_[150]{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; ros::Time start_using_traject_time; 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 c7c2738e..e46a7d3f 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -57,7 +57,6 @@ #include #include #include -#include namespace rm_gimbal_controllers { @@ -169,10 +168,7 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; - std::vector> bullet_solver_MPC_; - std::vector bullet_solver_MPC_solve_success_; - Eigen::VectorXd bullet_pos_des = Eigen::VectorXd::Zero(201); - std::shared_ptr mpc_trajectory_planning_; + double gimbal_real_z_vel_{}; // ROS Interface ros::Time last_publish_time_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index f8d97aa9..5fb2f17e 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -40,6 +40,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -57,21 +58,16 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .center_delay = getParam(controller_nh, "center_delay", 0.0), .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.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.1), .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 3.0), .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), - .max_selected_armor_ = getParam(controller_nh, "max_selecteed_armor", 3), .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1), .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.), - .move_switch_time_coff_ = getParam(controller_nh, "move_switch_time_coff_", 0.001), - .traject_k_effort = getParam(controller_nh, "traject_k_effort", 1.0), - .traject_k_vel_ = getParam(controller_nh, "traject_k_vel", 0.), + .traject_k_effort = getParam(controller_nh, "traject_k_effort", 0.0), + .traject_k_vel_ = getParam(controller_nh, "traject_k_vel", 0.65), }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); @@ -140,32 +136,17 @@ double BulletSolver::getResistanceCoefficient(double target_distance) const } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel, double next_n) + double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; - vision_count_++; 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; filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05; - double temp_z = pos.z; - double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); - resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; - double output_yaw_central = std::atan2(pos.y, pos.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); - double rough_fly_time = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double r = r1; - double z = pos.z; - if (track_target_) - yaw += filtered_v_yaw_ * (config_.track_rotate_target_delay + next_n); - pos.x += vel.x * (config_.track_move_target_delay + next_n); - pos.y += vel.y * (config_.track_move_target_delay + next_n); + if (track_target_) { if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) @@ -176,175 +157,188 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) track_target_ = true; } - switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; - // double move_switch_time_coff_ = - // v_yaw * vel.x > 0.0 ? config_.move_switch_time_coff_ : -config_.move_switch_time_coff_; - // double move_switch_time_offset = move_switch_time_coff_ * vel.x; - // traject_switch_time_ = 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ + move_switch_time_offset; - // 1.1399 * std::pow(v_yaw, -0.9126) * config_.traject_ahead_ * (1 + (target_rho - 2.0) * 0.05) + move_switch_time_offset ; - traject_switch_time_ = (abs(filtered_v_yaw_) / filtered_v_yaw_ * switch_armor_angle - switche_time_yaw_) / - filtered_v_yaw_ * config_.traject_ahead_; - yaw_subtract_ = filtered_yaw_ - output_yaw_central; - while (yaw_subtract_ > M_PI) - yaw_subtract_ -= 2 * M_PI; - while (yaw_subtract_ < -M_PI) - yaw_subtract_ += 2 * M_PI; - double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; - selected_armor_ = 0; - if (change_armor) + for (int i = 0; i < 150; i++) { - vision_count_ = 0; - count_ = 0; - next_count_ = 0; - change_armor = false; - } - if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || - (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -2.)) - { - count_++; - if (count_ >= config_.min_fit_switch_count) + double temp_z = pos.z; + if (track_target_) + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); + else + yaw_[i] = yaw; + pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.005); + pos_y[i] = pos.y + vel.y * (config_.track_move_target_delay + i * 0.005); + double target_rho = std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2)); + resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; + double output_yaw_central = std::atan2(pos_y[i], pos_x[i]); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2))); + double rough_fly_time = + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + double r = r1; + double z = pos.z; + + switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; + yaw_subtract_ = yaw_[i] - output_yaw_central; + while (yaw_subtract_ > M_PI) + yaw_subtract_ -= 2 * M_PI; + while (yaw_subtract_ < -M_PI) + yaw_subtract_ += 2 * M_PI; + double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; + if (abs(v_yaw) < 2.) + selected_armor_[i] = 0; + else if ((after_fly_yaw_subtract_ < switch_armor_angle - 0.1 && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ > -switch_armor_angle + 0.1 && filtered_v_yaw_ < -2.)) + selected_armor_[i] = 0; + + if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -2.)) { - selected_armor_ = v_yaw > 0. ? -1 : 1; - r = armors_num == 4 ? r2 : r1; - z = pos.z + dz; - if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) + count_[i]++; + if (count_[i] >= config_.min_fit_switch_count) { - selected_armor_ = -2; - r = armors_num == 4 ? r1 : r2; - z = pos.z; - next_count_++; - if (next_count_ == config_.min_fit_switch_count) + selected_armor_[i] = v_yaw > 0. ? -1 : 1; + r = armors_num == 4 ? r2 : r1; + z = pos.z + dz; + if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) { - switch_armor_time_ = ros::Time::now(); - is_aheading_two_ = true; + selected_armor_[i] = -2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } } - } - else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < -switch_armor_angle) && v_yaw < 0.) - { - selected_armor_ = 2; - r = armors_num == 4 ? r1 : r2; - z = pos.z; - next_count_++; - if (next_count_ == config_.min_fit_switch_count) - { - switch_armor_time_ = ros::Time::now(); - is_aheading_two_ = true; - } - } - if (count_ == config_.min_fit_switch_count) - { - if (is_aheading_two_) + else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < -switch_armor_angle) && v_yaw < 0.) { - is_aheading_two_ = false; + selected_armor_[i] = 2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } } - else + if (count_[i] == config_.min_fit_switch_count) { - switch_armor_time_ = ros::Time::now(); + if (is_aheading_two_[i]) + { + is_aheading_two_[i] = false; + } + else + { + switch_armor_time_ = ros::Time::now(); + } } - traject_count_ = 0; } } - } - if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 2. && - count_ > 2) - { - traject_count_++; - if (traject_count_ == 2) - { - using_traject_ = true; - Advanced_planning_ = true; - start_using_traject_time = ros::Time::now(); - } - } - // if (!using_traject_ && Advanced_planning_ && abs(v_yaw) > 4.0) - // { - // if (selected_armor_ == 0) - // { - // selected_armor_ = v_yaw > 0. ? -1 : 1; - // z = pos.z + dz; - // r = armors_num == 4 ? r2 : r1; - // } - // else if (selected_armor_ == -1 || selected_armor_ == 1) - // { - // selected_armor_ = v_yaw > 0. ? -2 : 2; - // z = pos.z; - // r = armors_num == 4 ? r1 : r2; - // } - // else if (selected_armor_ == -2 || selected_armor_ == 2) - // { - // selected_armor_ = v_yaw > 0. ? -3 : 3; - // z = pos.z + dz; - // r = armors_num == 4 ? r2 : r1; - // } - // } - if ((count_ == 3 && vision_count_ != 3) || (next_count_ == 3 && vision_count_ != 3)) - { - real_change_armor_ = true; - Advanced_planning_ = false; - switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; - } - else - { - real_change_armor_ = false; - } - int count{}; - double error = 999; - if (track_target_) - { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * M_PI / 2); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * M_PI / 2); - } - 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)); - if ((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.7) || - (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.7)) - { - selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1; - } - if (((filtered_v_yaw_ > 4.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.7) || - (filtered_v_yaw_ < -4.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.7))) - { - selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; - } - if (((filtered_v_yaw_ > 4.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.7) || - (filtered_v_yaw_ < -4.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.7))) + int count{}; + double error = 999; + if (track_target_) { - selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; + target_pos_[i].x = pos_x[i] - r * cos(yaw_[i] + selected_armor_[i] * M_PI / 2); + target_pos_[i].y = pos_y[i] - r * sin(yaw_[i] + selected_armor_[i] * M_PI / 2); } - if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || - (selected_armor_ < 0 && selected_armor_ < -config_.max_selected_armor_)) + else { - selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; + target_pos_[i].x = pos_x[i] - r * cos(atan2(pos_y[i], pos_x[i])); + target_pos_[i].y = pos_y[i] - r * sin(atan2(pos_y[i], pos_x[i])); + if ((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 1.1) || + (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -1.1)) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -1 : 1; + } + if (((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 1 * 2 * M_PI / armors_num) > 1.1) || + (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 1 * 2 * M_PI / armors_num) < -1.1))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -2 : 2; + } + if (((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 2 * 2 * M_PI / armors_num) > 1.1) || + (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 2 * 2 * M_PI / armors_num) < -1.1))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -3 : 3; + } + + if (selected_armor_[i] % 2 == 0) + { + r = r1; + z = pos.z; + } } - if (selected_armor_ % 2 == 0) + target_pos_[i].z = z; + while (error >= 0.001) { - r = r1; - z = pos.z; + output_yaw_[i] = std::atan2(target_pos_[i].y, target_pos_[i].x); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2))); + target_rho = std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2)); + fly_time_[i] = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + double real_z = (bullet_speed_ * std::sin(output_pitch_[i]) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time_[i])) / resistance_coff_ - + config_.g * fly_time_[i] / resistance_coff_; + if (track_target_) + { + target_pos_[i].x = pos_x[i] + vel.x * fly_time_[i] - + r * cos(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] + vel.y * fly_time_[i] - + r * sin(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + } + else + { + double target_pos_after_fly_time[2]; + target_pos_after_fly_time[0] = pos_x[i] + vel.x * fly_time_[i]; + target_pos_after_fly_time[1] = pos_y[i] + vel.y * fly_time_[i]; + target_pos_[i].x = + target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + target_pos_[i].y = + target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + } + target_pos_[i].z = z + vel.z * fly_time_[i]; + + double target_yaw = std::atan2(target_pos_[i].y, target_pos_[i].x); + double error_theta = target_yaw - output_yaw_[i]; + double error_z = target_pos_[i].z - real_z; + temp_z += error_z; + error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); + count++; + + if (count >= 20 || std::isnan(error)) + return false; } } - target_pos_.z = z; - if (abs(filtered_v_yaw_) < 1.0) + + if (fly_time_pub_->trylock()) { - selected_armor_ = 0; - r = r1; + fly_time_pub_->msg_.data = fly_time_[0]; + fly_time_pub_->unlockAndPublish(); } - double r_trajcet_; - if (selected_armor_ == 1 || selected_armor_ == -1 || selected_armor_ == 3 || selected_armor_ == -3) + if (selected_armor_[0] == 1 || selected_armor_[0] == -1 || selected_armor_[0] == 3 || selected_armor_[0] == -3) { - r_trajcet_ = r1; + r_traject_ = r1; } else { - r_trajcet_ = r2; + r_traject_ = r2; + } + for (int i = 1; i <= 55; i++) + { + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false) + { + start_traject_ = true; + using_traject_ = true; + start_using_traject_time = ros::Time::now(); + break; + } } - if (!using_traject_) + if (start_traject_ && using_traject_) { traject_max_acc_ = 200.; switchtime = 0.05; @@ -353,30 +347,29 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d switchtime += 0.005; if (filtered_v_yaw_ > 0) { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); } else { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); } - stauts_limit_.start_pos = output_yaw_; - stauts_limit_.start_vel = - (output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + stauts_limit_.start_pos = output_yaw_[0]; + stauts_limit_.start_vel = start_vel; stauts_limit_.end_pos = std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; stauts_limit_.end_vel = stauts_limit_.start_vel; - if (switchtime > 0.12) + if (switchtime > 0.15) { break; } @@ -393,54 +386,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d trajectory_function_coefficients.a2 = X(0); trajectory_function_coefficients.a3 = X(1); - traject_max_acc_ = 2 * trajectory_function_coefficients.a2; - } - } - - 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)); - 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_; - - 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); + traject_max_acc_ = abs(2 * trajectory_function_coefficients.a2); } - 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_; - double error_z = target_pos_.z - real_z; - temp_z += error_z; - error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); - count++; - - if (count >= 20 || std::isnan(error)) - return false; - } - if (fly_time_pub_->trylock()) - { - fly_time_pub_->msg_.data = fly_time_; - fly_time_pub_->unlockAndPublish(); + start_traject_ = false; } if (using_traject_) { @@ -451,74 +399,57 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d using_traject_ = false; } } - else + if (!using_traject_ || !track_target_) { - traject_output_yaw_ = output_yaw_; - } - - if (!track_target_) - { - traject_output_yaw_ = output_yaw_; + traject_output_yaw_ = output_yaw_[0]; } + double vel_des = (traject_output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + filtered_vel_des = filtered_vel_des + (vel_des - filtered_vel_des) * (0.001 / (0.01 + 0.001)); + last_output_time_ = ros::Time::now(); + last_output_yaw_ = traject_output_yaw_; if (bullet_solver_pub->trylock()) { - bullet_solver_pub->msg_.test_number = real_change_armor_; - bullet_solver_pub->msg_.selected_armor_ = selected_armor_; - bullet_solver_pub->msg_.yaw_subtract_ = yaw_subtract_; - bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; - bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; - bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; - bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; - bullet_solver_pub->msg_.pos_distance_ = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); - bullet_solver_pub->msg_.Advanced_planning_ = Advanced_planning_; - bullet_solver_pub->msg_.traject_switch_time_ = traject_switch_time_; + for (int i = 0; i < 150; ++i) + { + bullet_solver_pub->msg_.selected_armor_[i] = static_cast(selected_armor_[i]); + bullet_solver_pub->msg_.count_number_[i] = count_[i]; + } + bullet_solver_pub->msg_.start_vel = stauts_limit_.start_vel; bullet_solver_pub->msg_.switchtime = switchtime; + bullet_solver_pub->msg_.traject_output_yaw_ = traject_output_yaw_; + bullet_solver_pub->msg_.traject_acc_ = traject_effort_ff_; + bullet_solver_pub->msg_.traject_vel_ = filtered_vel_des; bullet_solver_pub->msg_.using_traject_ = using_traject_; - bullet_solver_pub->msg_.traject_effort_ff_ = traject_effort_ff_; - bullet_solver_pub->msg_.traject_vel_ = switch_armor_time_.toSec(); - bullet_solver_pub->msg_.center_this_armor_ = (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - - v_yaw / abs(v_yaw) * 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - - v_yaw / abs(v_yaw) * 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - - v_yaw / abs(v_yaw) * 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - - v_yaw / abs(v_yaw) * 3 * 2 * M_PI / armors_num); bullet_solver_pub->unlockAndPublish(); } - last_selected_armor_ = selected_armor_; - last_output_time_ = ros::Time::now(); - last_output_yaw_ = output_yaw_; return true; } + void BulletSolver::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) { double r = r1, z = pos.z; - if (armors_num == 4 && selected_armor_ != 0) + if (armors_num == 4 && selected_armor_[0] != 0) { 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_); + pos.x += vel.x * (config_.track_move_target_delay + fly_time_[0]); + pos.y += vel.y * (config_.track_move_target_delay + fly_time_[0]); if (track_target_) { - 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.x = pos.x - r * cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 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) + - selected_armor_ * 2 * M_PI / armors_num); + sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 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); + cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.z = vel.z; } else @@ -536,18 +467,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi double roll{}, pitch{}, yaw{}; quatToRPY(odom2pitch.transform.rotation, roll, pitch, yaw); geometry_msgs::Point point_desire{}, point_real{}; - double target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); + double target_rho = std::sqrt(std::pow(target_pos_[0].x, 2) + std::pow(target_pos_[0].y, 2)); int point_num = int(target_rho * 20); for (int i = 0; i <= point_num; i++) { double rt_bullet_rho = target_rho * i / point_num; - double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / + double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[0])))) / resistance_coff_; - double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * + double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_[0]) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - point_desire.x = rt_bullet_rho * std::cos(output_yaw_) + odom2pitch.transform.translation.x; - point_desire.y = rt_bullet_rho * std::sin(output_yaw_) + odom2pitch.transform.translation.y; + point_desire.x = rt_bullet_rho * std::cos(output_yaw_[0]) + odom2pitch.transform.translation.x; + point_desire.y = rt_bullet_rho * std::sin(output_yaw_[0]) + odom2pitch.transform.translation.y; point_desire.z = rt_bullet_z + odom2pitch.transform.translation.z; marker_desire_.points.push_back(point_desire); } @@ -586,7 +517,7 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec double delay; delay = track_target_ ? 0. : config_.center_delay; double r, z; - if (selected_armor_ % 2 == 0) + if (selected_armor_[0] % 2 == 0) { r = r1; z = pos.z; @@ -600,28 +531,28 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec if (track_target_) { double bullet_rho = - bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_[0])) / 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)); + (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_ - + config_.g * fly_time_[0] / resistance_coff_; + error = std::sqrt(std::pow(target_pos_[0].x - bullet_x, 2) + std::pow(target_pos_[0].y - bullet_y, 2) + + std::pow(target_pos_[0].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); + pos.x + vel.x * (fly_time_[0] + delay) - + r * cos(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 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)); + pos.y + vel.y * (fly_time_[0] + delay) - + r * sin(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_[0] + delay); + error = std::sqrt(std::pow(target_pos_[0].x - target_pos_after_fly_time_and_delay.x, 2) + + std::pow(target_pos_[0].y - target_pos_after_fly_time_and_delay.y, 2) + + std::pow(target_pos_[0].z - target_pos_after_fly_time_and_delay.z, 2)); } return error; } @@ -632,35 +563,31 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) identified_target_change_ = true; else if (!msg->data && identified_target_change_) { - change_armor = true; identified_target_change_ = false; } } -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 (std::abs(v_yaw) > config_.min_shoot_beforehand_vel) { - if ((ros::Time::now().toSec() >= (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) && - (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime)) + int ban_shoot_delay_number = static_cast(55 + config_.delay * 1000); + ban_shoot_delay_number = ban_shoot_delay_number > 149 ? 149 : ban_shoot_delay_number; + if (selected_armor_[ban_shoot_delay_number] != selected_armor_[0]) { ban_shoot_count_++; shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - if (ban_shoot_count_ == 3) + if (ban_shoot_count_ == 1) { ban_shoot_start_time_ = ros::Time::now(); } } + else if (using_traject_ && ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime) + { + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + } else if (ros::Time::now().toSec() > ban_shoot_start_time_.toSec() + switchtime && ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime + config_.delay) { @@ -691,8 +618,7 @@ double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_ double a3 = trajectory_function_coefficients.a3; double n_time_ = (time - start_trajectory_time_).toSec(); double plansetpoint = a0 + a1 * n_time_ + a2 * pow(n_time_, 2) + a3 * pow(n_time_, 3); - traject_effort_ff_ = (1.2 - 1.2 * n_time_ / switchtime) * abs(v_yaw) / v_yaw * config_.traject_k_effort; - // traject_effort_ff_ = (2 * a2 + 6 * a3 * n_time_) / (2 * a2) * 1.0 * config_.traject_k_effort; + traject_effort_ff_ = (2 * a2 + 6 * a3 * n_time_) / (2 * a2) * 1.0 * config_.traject_k_effort; traject_vel_ = (a1 + 2 * a2 * n_time_ + 3 * a3 * pow(n_time_, 2)) * config_.traject_k_vel_; return plansetpoint; } @@ -727,19 +653,14 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.center_delay = init_config.center_delay; config.max_switch_angle = init_config.max_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.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; config.yaw_max_acc = init_config.yaw_max_acc; config.min_fit_switch_count = init_config.min_fit_switch_count; - config.max_selected_armor_ = init_config.max_selected_armor_; config.traject_ahead_ = init_config.traject_ahead_; config.clean_shoot_num_ = init_config.clean_shoot_num_; config.end_pos_offset = init_config.end_pos_offset; - config.move_switch_time_coff_ = init_config.move_switch_time_coff_; config.traject_k_effort = init_config.traject_k_effort; config.traject_k_vel = init_config.traject_k_vel_; dynamic_reconfig_initialized_ = true; @@ -755,19 +676,14 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .center_delay = config.center_delay, .max_switch_angle = config.max_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, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, .yaw_max_acc = config.yaw_max_acc, .min_fit_switch_count = config.min_fit_switch_count, - .max_selected_armor_ = config.max_selected_armor_, .traject_ahead_ = config.traject_ahead_, .clean_shoot_num_ = config.clean_shoot_num_, .end_pos_offset = config.end_pos_offset, - .move_switch_time_coff_ = config.move_switch_time_coff_, .traject_k_effort = config.traject_k_effort, .traject_k_vel_ = config.traject_k_vel }; config_rt_buffer_.writeFromNonRT(config_non_rt); diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 9c9b6d2f..bf39faaf 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -312,7 +312,7 @@ void Controller::track(const ros::Time& time) target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity()); + data_track_.armors_num, gimbal_real_z_vel_); bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) @@ -456,6 +456,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrls_.find(2) != ctrls_.end()) angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } + gimbal_real_z_vel_ = angular_vel.z; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); @@ -549,7 +550,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) { - if (bullet_solver_->getUsingtraject()) + if (state_ == TRACK) { pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); } @@ -564,9 +565,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); - auto cmd = ctrls_.at(2)->joint_.getCommand(); - cmd += bullet_solver_->getTrajectEffortff(); - ctrls_.at(2)->joint_.setCommand(cmd); } else { diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index a68f830c..24bcf6cf 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -345,7 +345,8 @@ void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& pe double friction_change_speed_derivative = friction_change_speed - last_friction_change_speed_; if (state_ != STOP) { - if (friction_change_speed_derivative > 0 && has_shoot_) + if (friction_change_speed_derivative > 0 && has_shoot_ && + friction_change_speed > config_.wheel_speed_raise_threshold) has_shoot_ = false; if (friction_change_speed < -config_.wheel_speed_drop_threshold && !has_shoot_ && friction_change_speed_derivative < 0) From 089600ef60af7b7ca123bb1cb3d2626f1dd28bd3 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 26 Apr 2026 21:52:34 +0800 Subject: [PATCH 08/31] Delete the mpc class. --- .../mpc_trajectory_planning.h | 144 ------------------ 1 file changed, 144 deletions(-) delete mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/mpc_trajectory_planning.h diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/mpc_trajectory_planning.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/mpc_trajectory_planning.h deleted file mode 100644 index 8c3d0c10..00000000 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/mpc_trajectory_planning.h +++ /dev/null @@ -1,144 +0,0 @@ -// -// Created by xuncheng on 2026/4/11. -// - -#ifndef RM_WS_MPC_TRAJERTORY_PLANNING_H -#define RM_WS_MPC_TRAJERTORY_PLANNING_H -#include -#include - -namespace rm_gimbal_controllers -{ -class MPC_Trajectory_Planning -{ -public: - // Eigen -> OSQP 转换函数 - OSQPCscMatrix* eigen2osqp(const Eigen::SparseMatrix& mat) - { - Eigen::SparseMatrix mat_csc = mat; - mat_csc.makeCompressed(); - - OSQPInt m = mat_csc.rows(); - OSQPInt n = mat_csc.cols(); - OSQPInt nz = mat_csc.nonZeros(); - - std::vector inner(mat_csc.innerIndexPtr(), mat_csc.innerIndexPtr() + nz); - std::vector outer(mat_csc.outerIndexPtr(), mat_csc.outerIndexPtr() + n + 1); - - OSQPCscMatrix* osqp_mat = OSQPCscMatrix_new(m, n, nz, mat_csc.valuePtr(), inner.data(), outer.data()); - - return osqp_mat; - } - - void initMatrix(Eigen::Vector2d x_self) - { - A << 1.0, 0.001, 0.0, 1.0; - B << 0.0, 0.001; - f << 0.0, 0.0; - Q << 10e6, 0.0, 0.0, 0.0; - R << 1.0; - I << 1.0, 0.0, 0.0, 1.0; - for (int i = 0; i < (N + 1); i++) - { - P_des.block(nx * i, nx * i, nx, nx) = Q; - P_des.block(nu * i + nx * (N + 1), nu * i + nx * N, nu, nu) = R; - } - //转化成稀疏矩阵 - P_osqp = P_des.sparseView(); - P_csc = eigen2osqp(P_osqp); - - //初始状态不可以更改 - A_(0, 0) = 1; - A_(1, 1) = 1; - l_(0) = x_self(0); - u_(0) = x_self(0); - l_(1) = x_self(1); - u_(1) = x_self(1); - x = x_self; - for (int i = 0; i < N; i++) - { - //动力学解算 - A_.block(nx * i + 2, nx * i, nx, nx) = -A; - A_.block(nx * i + 2, nx * (N + 1) + nu * i, nx, nu) = -B; - A_.block(nx * i + 2, nx * (i + 1), 2, 2) = I; - //速度限制 - A_(nx * N + 2 + i, nx * (i + 1) + 1) = 1; - l_(nx * N + 2 + i) = -max_vel_; - u_(nx * N + 2 + i) = max_vel_; - //加速度限制 - A_(nx * N + 2 + N + i, nx * (N + 1) + nu * i) = 1; - l_(nx * N + 2 + N + i) = -max_acc_; - u_(nx * N + 2 + N + i) = max_acc_; - } - A_osqp = A_.sparseView(); - A_csc = eigen2osqp(A_osqp); - } - void osqpSolver(Eigen::VectorXd bullet_pos_des) - { - for (int i = 0; i < N + 1; i++) - { - q_osqp(nx * i) = -Q(0, 0) * bullet_pos_des(i); - } - OSQPSettings* settings = OSQPSettings_new(); - osqp_set_default_settings(settings); - - OSQPSolver* solver = NULL; - osqp_setup(&solver, P_csc, q_osqp.data(), A_csc, l_.data(), u_.data(), N * 4 + 2, (nx + nu) * (N + 1), settings); - - osqp_solve(solver); - - OSQPSolution sol = OSQPSolution(); - osqp_get_solution(solver, &sol); - pos_des_ = sol.x[3]; - vel_des_ = sol.x[4]; - acc_des_ = sol.x[2]; - OSQPSettings_free(settings); - osqp_cleanup(solver); - }; - double getPosDes() - { - return pos_des_; - } - double getVelDes() - { - return vel_des_; - } - double getAccDes() - { - return acc_des_; - } - -private: - //自身状态和控制量 - Eigen::Vector2d x; - Eigen::VectorXd u; - //状态变化矩阵 - Eigen::Matrix2d A; - Eigen::Vector2d B; - //常数扰动 - Eigen::Vector2d f; - //权重矩阵 - Eigen::Matrix2d Q; - Eigen::MatrixXd R; - Eigen::Matrix2d I; - double pos_des_ = 0.0; - double vel_des_ = 0.0; - double acc_des_ = 0.0; - double max_acc_ = 40; - double max_vel_ = 8; - int N = 200; - int nx = 2; - int nu = 1; - int num = (nx + nu) * (N + 1); - Eigen::MatrixXd P_des = Eigen::MatrixXd::Zero(num, num); - Eigen::VectorXd q_osqp = Eigen::VectorXd::Zero(num); - Eigen::SparseMatrix P_osqp; - Eigen::MatrixXd A_ = Eigen::MatrixXd::Zero(N * 4 + 2, num); - Eigen::VectorXd l_ = Eigen::VectorXd::Zero(N * 4 + 2); - Eigen::VectorXd u_ = Eigen::VectorXd::Zero(N * 4 + 2); - Eigen::SparseMatrix A_osqp; - const OSQPCscMatrix* A_csc; - const OSQPCscMatrix* P_csc; -}; -} // namespace rm_gimbal_controllers -#endif // RM_WS_MPC_TRAJERTORY_PLANNING_H From d9303ec8f74222149b85a530dab7996901a3574d Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 28 Apr 2026 21:35:22 +0800 Subject: [PATCH 09/31] Add the dynamic param and add the v_yaw limit of trajectory. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 1 + .../rm_gimbal_controllers/bullet_solver.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 19 +++++++++++-------- 3 files changed, 13 insertions(+), 9 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 076cdc37..0f81d933 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -18,6 +18,7 @@ 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("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99) +gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of switch", 0, 1, 99) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.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 d88d4570..1eb3af3c 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -60,7 +60,7 @@ struct Config double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, center_delay, max_switch_angle, switch_angle_offset, min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay, yaw_max_acc; - int min_fit_switch_count; + int min_fit_switch_count, traject_start_fit_; double traject_ahead_; int clean_shoot_num_; double end_pos_offset; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 5fb2f17e..7ec9b161 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -63,6 +63,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), + .traject_start_fit_ = getParam(controller_nh, "traject_start_fit_", 45), .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1), .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.), @@ -246,22 +247,22 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { target_pos_[i].x = pos_x[i] - r * cos(atan2(pos_y[i], pos_x[i])); target_pos_[i].y = pos_y[i] - r * sin(atan2(pos_y[i], pos_x[i])); - if ((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 1.1) || - (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -1.1)) + if ((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -0.7)) { selected_armor_[i] = filtered_v_yaw_ > 0. ? -1 : 1; } if (((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - - 1 * 2 * M_PI / armors_num) > 1.1) || + 1 * 2 * M_PI / armors_num) > 0.7) || (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + - 1 * 2 * M_PI / armors_num) < -1.1))) + 1 * 2 * M_PI / armors_num) < -0.7))) { selected_armor_[i] = filtered_v_yaw_ > 0. ? -2 : 2; } if (((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - - 2 * 2 * M_PI / armors_num) > 1.1) || + 2 * 2 * M_PI / armors_num) > 0.7) || (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + - 2 * 2 * M_PI / armors_num) < -1.1))) + 2 * 2 * M_PI / armors_num) < -0.7))) { selected_armor_[i] = filtered_v_yaw_ > 0. ? -3 : 3; } @@ -327,9 +328,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { r_traject_ = r2; } - for (int i = 1; i <= 55; i++) + for (int i = 1; i <= config_.traject_start_fit_; i++) { - if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false) + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 2.5) { start_traject_ = true; using_traject_ = true; @@ -658,6 +659,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.track_move_target_delay = init_config.track_move_target_delay; config.yaw_max_acc = init_config.yaw_max_acc; config.min_fit_switch_count = init_config.min_fit_switch_count; + config.traject_start_fit_ = init_config.traject_start_fit_; config.traject_ahead_ = init_config.traject_ahead_; config.clean_shoot_num_ = init_config.clean_shoot_num_; config.end_pos_offset = init_config.end_pos_offset; @@ -681,6 +683,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .track_move_target_delay = config.track_move_target_delay, .yaw_max_acc = config.yaw_max_acc, .min_fit_switch_count = config.min_fit_switch_count, + .traject_start_fit_ = config.traject_start_fit_, .traject_ahead_ = config.traject_ahead_, .clean_shoot_num_ = config.clean_shoot_num_, .end_pos_offset = config.end_pos_offset, From 3efc776706c17b9fa3992b122a3c8c660c62ae61 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 28 Apr 2026 21:40:13 +0800 Subject: [PATCH 10/31] Fix the bug of time. --- 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 7ec9b161..c5537323 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -165,8 +165,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); else yaw_[i] = yaw; - pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.005); - pos_y[i] = pos.y + vel.y * (config_.track_move_target_delay + i * 0.005); + pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.001); + pos_y[i] = pos.y + vel.y * (config_.track_move_target_delay + i * 0.001); double target_rho = std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2)); resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; double output_yaw_central = std::atan2(pos_y[i], pos_x[i]); From aa11882dcbac4ca8552d047774e1afe262442987 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 1 May 2026 15:32:04 +0800 Subject: [PATCH 11/31] Add the dynamic param. --- .../rm_gimbal_controllers/bullet_solver.h | 22 +++++++++---------- rm_gimbal_controllers/src/bullet_solver.cpp | 4 ++-- 2 files changed, 13 insertions(+), 13 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 1eb3af3c..8ea0c38c 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -113,7 +113,7 @@ class BulletSolver { return traject_effort_ff_; } - bool getTrackTarget() + bool getTrackTarget() const { return track_target_; } @@ -141,14 +141,14 @@ class BulletSolver dynamic_reconfigure::Server* d_srv_{}; Config config_{}; double yaw_[150], pos_x[150], pos_y[150]; - double max_track_target_vel_; + double max_track_target_vel_{}; double output_yaw_[150], output_pitch_[150], traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; double fly_time_[150]; - double switch_hysteresis_; + double switch_hysteresis_{}; double last_yaw_{}, filtered_yaw_{}; double gimbal_switch_duration_{}; - double yaw_subtract_; + double yaw_subtract_{}; double switch_armor_angle{}; double filtered_v_yaw_{}; double switchtime{}; @@ -180,14 +180,14 @@ class BulletSolver geometry_msgs::Point target_pos_[150]{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; - ros::Time start_using_traject_time; - ros::Time ban_shoot_time_; - ros::Time last_output_time_; - ros::Time ban_shoot_start_time_; + ros::Time start_using_traject_time{}; + ros::Time ban_shoot_time_{}; + ros::Time last_output_time_{}; + ros::Time ban_shoot_start_time_{}; - TrajectoryFunctionCoefficients trajectory_function_coefficients; - TrajectoryLimitParams stauts_limit_; + TrajectoryFunctionCoefficients trajectory_function_coefficients{}; + TrajectoryLimitParams stauts_limit_{}; - mutable std::mutex heat_mutex_; + mutable std::mutex heat_mutex_{}; }; } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index c5537323..47a1058b 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -330,7 +330,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } for (int i = 1; i <= config_.traject_start_fit_; i++) { - if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 2.5) + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 3.0) { start_traject_ = true; using_traject_ = true; @@ -413,7 +413,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d for (int i = 0; i < 150; ++i) { bullet_solver_pub->msg_.selected_armor_[i] = static_cast(selected_armor_[i]); - bullet_solver_pub->msg_.count_number_[i] = count_[i]; + bullet_solver_pub->msg_.count_number_[i] = static_cast(count_[i]); } bullet_solver_pub->msg_.start_vel = stauts_limit_.start_vel; bullet_solver_pub->msg_.switchtime = switchtime; From c7909160f28471377aaa1929da20ec3a5fc7f2e1 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sat, 2 May 2026 16:48:03 +0800 Subject: [PATCH 12/31] Add the outpost delay , delete the td. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 1 + .../rm_gimbal_controllers/bullet_solver.h | 5 +- .../rm_gimbal_controllers/gimbal_base.h | 3 - rm_gimbal_controllers/src/bullet_solver.cpp | 60 ++++++++++++------- rm_gimbal_controllers/src/gimbal_base.cpp | 11 +--- 5 files changed, 43 insertions(+), 37 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 0f81d933..129faa57 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -23,6 +23,7 @@ gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforeha gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -200.0, 200.0) +gen.add("track_rotate_outpost_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.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 8ea0c38c..1a17cbfe 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -59,7 +59,8 @@ struct Config { double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, center_delay, max_switch_angle, - switch_angle_offset, min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay, yaw_max_acc; + switch_angle_offset, min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay, yaw_max_acc, + track_rotate_outpost_delay; int min_fit_switch_count, traject_start_fit_; double traject_ahead_; int clean_shoot_num_; @@ -81,7 +82,7 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel); + double r1, double r2, double dz, int armors_num, double start_vel, double track_id); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); double getResistanceCoefficient(double target_distance) const; 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 da64c3b7..abe6ecd7 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -56,7 +56,6 @@ #include #include #include -#include namespace rm_gimbal_controllers { @@ -165,8 +164,6 @@ class Controller : public controller_interface::MultiInterfaceController> pid_pos_; std::unordered_map joint_urdfs_; std::unordered_map pos_des_in_limit_; - std::unordered_map>> tracking_differentiator_; - std::unordered_map last_pos_des_; bool has_imu_ = true; std::shared_ptr bullet_solver_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 47a1058b..49603c31 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -62,6 +62,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), + .track_rotate_outpost_delay = getParam(controller_nh, "track_rotate_outpost_delay", 0.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), .traject_start_fit_ = getParam(controller_nh, "traject_start_fit_", 45), .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), @@ -113,31 +114,35 @@ double BulletSolver::getResistanceCoefficient(double target_distance) const { // bullet_speed have 5 value:10,15,16,18,30 double resistance_coff; - if (target_distance < 3.3) - resistance_coff = config_.resistance_coff_qd_1 + (target_distance - 2.3) + - (config_.resistance_coff_qd_10 - config_.resistance_coff_qd_1); - else if (target_distance < 4.3 && target_distance > 3.3) - resistance_coff = config_.resistance_coff_qd_10 + - (target_distance - 3.3) * (config_.resistance_coff_qd_15 - config_.resistance_coff_qd_10); - else if (target_distance < 5.3) - resistance_coff = config_.resistance_coff_qd_15 + - (target_distance - 4.3) * (config_.resistance_coff_qd_16 - config_.resistance_coff_qd_15); - else if (target_distance < 6.3) - resistance_coff = config_.resistance_coff_qd_16 + - (target_distance - 5.3) * (config_.resistance_coff_qd_18 - config_.resistance_coff_qd_16); - else if (target_distance < 7.3) - resistance_coff = config_.resistance_coff_qd_18 + - (target_distance - 6.3) * (config_.resistance_coff_qd_30 - config_.resistance_coff_qd_18); - else if (target_distance < 8.3) - resistance_coff = config_.resistance_coff_qd_30 + - (target_distance - 7.3) * (config_.resistance_coff_qd_800 - config_.resistance_coff_qd_30); - else - resistance_coff = config_.resistance_coff_qd_800; + // if (target_distance < 3.3) + // resistance_coff = config_.resistance_coff_qd_1 + (target_distance - 2.3) + + // (config_.resistance_coff_qd_10 - config_.resistance_coff_qd_1); + // else if (target_distance < 4.3 && target_distance > 3.3) + // resistance_coff = config_.resistance_coff_qd_10 + + // (target_distance - 3.3) * (config_.resistance_coff_qd_15 - config_.resistance_coff_qd_10); + // else if (target_distance < 5.3) + // resistance_coff = config_.resistance_coff_qd_15 + + // (target_distance - 4.3) * (config_.resistance_coff_qd_16 - config_.resistance_coff_qd_15); + // else if (target_distance < 6.3) + // resistance_coff = config_.resistance_coff_qd_16 + + // (target_distance - 5.3) * (config_.resistance_coff_qd_18 - config_.resistance_coff_qd_16); + // else if (target_distance < 7.3) + // resistance_coff = config_.resistance_coff_qd_18 + + // (target_distance - 6.3) * (config_.resistance_coff_qd_30 - config_.resistance_coff_qd_18); + // else if (target_distance < 8.3) + // resistance_coff = config_.resistance_coff_qd_30 + + // (target_distance - 7.3) * (config_.resistance_coff_qd_800 - config_.resistance_coff_qd_30); + // else + resistance_coff = config_.resistance_coff_qd_800; + + // hero + resistance_coff = config_.resistance_coff_qd_800; return resistance_coff; } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel) + double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel, + double track_id) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; @@ -162,7 +167,16 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { double temp_z = pos.z; if (track_target_) - yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); + { + if (track_id == 6) + { + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_outpost_delay + i * 0.001); + } + else + { + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); + } + } else yaw_[i] = yaw; pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.001); @@ -658,6 +672,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; config.yaw_max_acc = init_config.yaw_max_acc; + config.track_rotate_outpost_delay = init_config.track_rotate_outpost_delay; config.min_fit_switch_count = init_config.min_fit_switch_count; config.traject_start_fit_ = init_config.traject_start_fit_; config.traject_ahead_ = init_config.traject_ahead_; @@ -682,6 +697,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, .yaw_max_acc = config.yaw_max_acc, + .track_rotate_outpost_delay = config.track_rotate_outpost_delay, .min_fit_switch_count = config.min_fit_switch_count, .traject_start_fit_ = config.traject_start_fit_, .traject_ahead_ = config.traject_ahead_, diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index bf39faaf..de4fa484 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -312,7 +312,7 @@ void Controller::track(const ros::Time& time) target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, gimbal_real_z_vel_); + data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) @@ -531,15 +531,6 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (!in_limit.second) vel_des[in_limit.first] = 0.; - // for (auto& td : tracking_differentiator_) - // { - // td.second->update(last_pos_des_[td.first] + - // angles::shortest_angular_distance(last_pos_des_[td.first], pos_des[td.first]), - // vel_des[td.first],0.5); - // last_pos_des_[td.first] += angles::shortest_angular_distance(last_pos_des_[td.first], pos_des[td.first]); - // pos_des_temp[td.first] = std::remainder(td.second->getX1(), 2 * M_PI); - // angle_error[td.first] = angles::shortest_angular_distance(pos_real[td.first], pos_des_temp[td.first]); - // } if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) { pid_pos_.at(1)->computeCommand(angle_error[1], period); From b3c1c821acc89f6f4bc884c46fcf1020f916657c Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sat, 2 May 2026 17:19:29 +0800 Subject: [PATCH 13/31] Delete the td param. --- rm_gimbal_controllers/src/gimbal_base.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index de4fa484..2f31c024 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -109,9 +109,6 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); } - double r = getParam(nh, "r", 999.); - tracking_differentiator_.insert( - std::make_pair(axis, std::make_unique>(r, 0.001))); ctrls_.insert(std::make_pair(axis, std::make_unique())); pid_pos_.insert(std::make_pair(axis, std::make_unique())); pos_des_in_limit_.insert(std::make_pair(axis, true)); From b5e6ba22f539815a3a2c2ec95f0781f15fff43dd Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sat, 2 May 2026 17:32:49 +0800 Subject: [PATCH 14/31] Fix the next armor yaw . --- rm_gimbal_controllers/src/bullet_solver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 49603c31..95589392 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -58,7 +58,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .center_delay = getParam(controller_nh, "center_delay", 0.0), .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.0), - .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 3.0), + .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 2.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.), .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), @@ -254,8 +254,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double error = 999; if (track_target_) { - target_pos_[i].x = pos_x[i] - r * cos(yaw_[i] + selected_armor_[i] * M_PI / 2); - target_pos_[i].y = pos_y[i] - r * sin(yaw_[i] + selected_armor_[i] * M_PI / 2); + target_pos_[i].x = pos_x[i] - r * cos(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] - r * sin(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); } else { From 800abbce4d496130a8b866c494df34e73a949761 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 24 May 2026 16:27:11 +0800 Subject: [PATCH 15/31] Independent outpost parameters, optimized some error logic, and added initial central mode logic. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 + .../rm_gimbal_controllers/bullet_solver.h | 22 ++-- rm_gimbal_controllers/src/bullet_solver.cpp | 122 +++++++++++------- rm_gimbal_controllers/src/gimbal_base.cpp | 3 +- 4 files changed, 93 insertions(+), 56 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 129faa57..05a6ea24 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -13,10 +13,12 @@ gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of gen.add("resistance_coff_qd_800", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) +gen.add("outpost_delay", double_t, 0, "Delay of bullet firing of outpost", 0.0, 0, 0.5) gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5) 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("outpost_switch_angle_offset", double_t, 0,"Switch angle offset", 0.0, -20.0, 20) gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99) gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of switch", 0, 1, 99) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) 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 1a17cbfe..53440a0d 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -58,9 +58,9 @@ namespace rm_gimbal_controllers struct Config { double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, - resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, center_delay, max_switch_angle, - switch_angle_offset, min_shoot_beforehand_vel, track_rotate_target_delay, track_move_target_delay, yaw_max_acc, - track_rotate_outpost_delay; + resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, outpost_delay, center_delay, + max_switch_angle, switch_angle_offset, outpost_switch_angle_offset, min_shoot_beforehand_vel, + track_rotate_target_delay, track_move_target_delay, yaw_max_acc, track_rotate_outpost_delay; int min_fit_switch_count, traject_start_fit_; double traject_ahead_; int clean_shoot_num_; @@ -82,9 +82,10 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel, double track_id); + double r1, double r2, double dz, double armors_num, double start_vel, double track_id); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, - double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); + double r2, double dz, double armors_num, double yaw_real, double pitch_real, + double bullet_speed); double getResistanceCoefficient(double target_distance) const; double getYaw() const { @@ -118,10 +119,14 @@ class BulletSolver { return track_target_; } + void CleanTrackCount() + { + track_count_ = 0; + } 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); - void judgeShootBeforehand(const ros::Time& time, double v_yaw); + double r1, double r2, double dz, double armors_num); + void judgeShootBeforehand(const ros::Time& time, double v_yaw, int id); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); @@ -168,7 +173,8 @@ class BulletSolver int ban_shoot_count_{}; int selected_armor_[150] = {}; int last_selected_armor_ = {}; - bool track_target_ = true; + bool track_target_ = false; + int track_count_{}; bool identified_target_change_ = true; bool dynamic_reconfig_initialized_{}; bool using_traject_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 95589392..5e4d49cf 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -52,12 +52,13 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .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.), - .resistance_coff_qd_800 = getParam(controller_nh, "resistance_coff_qd_800", 1.1), + .resistance_coff_qd_800 = getParam(controller_nh, "resistance_coff_qd_800", 0.001), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), + .outpost_delay = getParam(controller_nh, "outpost_delay", 0.), .center_delay = getParam(controller_nh, "center_delay", 0.0), - .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.0), + .outpost_switch_angle_offset = getParam(controller_nh, "outpost_switch_angle_offset", 0.0), .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 2.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.), @@ -112,38 +113,20 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) double BulletSolver::getResistanceCoefficient(double target_distance) const { - // bullet_speed have 5 value:10,15,16,18,30 double resistance_coff; - // if (target_distance < 3.3) - // resistance_coff = config_.resistance_coff_qd_1 + (target_distance - 2.3) + - // (config_.resistance_coff_qd_10 - config_.resistance_coff_qd_1); - // else if (target_distance < 4.3 && target_distance > 3.3) - // resistance_coff = config_.resistance_coff_qd_10 + - // (target_distance - 3.3) * (config_.resistance_coff_qd_15 - config_.resistance_coff_qd_10); - // else if (target_distance < 5.3) - // resistance_coff = config_.resistance_coff_qd_15 + - // (target_distance - 4.3) * (config_.resistance_coff_qd_16 - config_.resistance_coff_qd_15); - // else if (target_distance < 6.3) - // resistance_coff = config_.resistance_coff_qd_16 + - // (target_distance - 5.3) * (config_.resistance_coff_qd_18 - config_.resistance_coff_qd_16); - // else if (target_distance < 7.3) - // resistance_coff = config_.resistance_coff_qd_18 + - // (target_distance - 6.3) * (config_.resistance_coff_qd_30 - config_.resistance_coff_qd_18); - // else if (target_distance < 8.3) - // resistance_coff = config_.resistance_coff_qd_30 + - // (target_distance - 7.3) * (config_.resistance_coff_qd_800 - config_.resistance_coff_qd_30); - // else - resistance_coff = config_.resistance_coff_qd_800; - // hero resistance_coff = config_.resistance_coff_qd_800; return resistance_coff; } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel, + double v_yaw, double r1, double r2, double dz, double armors_num, double start_vel, double track_id) { + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; if (abs(yaw - last_yaw_) > 1.) @@ -152,16 +135,27 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d filtered_yaw_ = filtered_yaw_ + (yaw - filtered_yaw_) * (0.001 / (0.01 + 0.001)); last_yaw_ = yaw; filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05; - - if (track_target_) + if (track_count_ <= 300) + { + track_target_ = false; + track_count_++; + } + if (track_count_ > 300) { if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) track_target_ = false; + else if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) + track_target_ = true; } - else + + if (identified_target_change_) { - if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) - track_target_ = true; + for (int i = 0; i < 150; i++) + { + count_[i] = 0; + next_count_[i] = 0; + } + identified_target_change_ = false; } for (int i = 0; i < 150; i++) { @@ -188,10 +182,19 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double rough_fly_time = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / resistance_coff_; + if (bullet_speed == 0.0) + { + ROS_ERROR("solve bullet speed is zero"); + return false; + } double r = r1; double z = pos.z; switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; + if (track_id == 6) + { + switch_armor_angle = acos(r1 / target_rho) - config_.outpost_switch_angle_offset; + } yaw_subtract_ = yaw_[i] - output_yaw_central; while (yaw_subtract_ > M_PI) yaw_subtract_ -= 2 * M_PI; @@ -200,8 +203,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; if (abs(v_yaw) < 2.) selected_armor_[i] = 0; - else if ((after_fly_yaw_subtract_ < switch_armor_angle - 0.1 && filtered_v_yaw_ > 2.) || - (after_fly_yaw_subtract_ > -switch_armor_angle + 0.1 && filtered_v_yaw_ < -2.)) + else if ((after_fly_yaw_subtract_ < switch_armor_angle - 0.4 && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ > -switch_armor_angle + 0.4 && filtered_v_yaw_ < -2.)) selected_armor_[i] = 0; if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || @@ -261,21 +264,22 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { target_pos_[i].x = pos_x[i] - r * cos(atan2(pos_y[i], pos_x[i])); target_pos_[i].y = pos_y[i] - r * sin(atan2(pos_y[i], pos_x[i])); - if ((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 0.7) || - (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -0.7)) + selected_armor_[i] = 0; + if ((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -0.7)) { selected_armor_[i] = filtered_v_yaw_ > 0. ? -1 : 1; } - if (((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.7) || - (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.7))) { selected_armor_[i] = filtered_v_yaw_ > 0. ? -2 : 2; } - if (((filtered_v_yaw_ > 4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.7) || - (filtered_v_yaw_ < -4.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.7))) { selected_armor_[i] = filtered_v_yaw_ > 0. ? -3 : 3; @@ -286,6 +290,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d r = r1; z = pos.z; } + else + { + r = r2; + z = pos.z + dz; + } } target_pos_[i].z = z; while (error >= 0.001) @@ -323,7 +332,6 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d temp_z += error_z; error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); count++; - if (count >= 20 || std::isnan(error)) return false; } @@ -344,7 +352,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } for (int i = 1; i <= config_.traject_start_fit_; i++) { - if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 3.0) + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 4.5) { start_traject_ = true; using_traject_ = true; @@ -382,6 +390,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d stauts_limit_.start_vel = start_vel; stauts_limit_.end_pos = std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; + stauts_limit_.end_pos = + stauts_limit_.start_pos + angles::shortest_angular_distance(stauts_limit_.start_pos, stauts_limit_.end_pos); stauts_limit_.end_vel = stauts_limit_.start_vel; if (switchtime > 0.15) @@ -409,6 +419,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { ros::Time temp = ros::Time::now(); traject_output_yaw_ = planningPoint(temp, start_using_traject_time, v_yaw); + while (traject_output_yaw_ > M_PI) + traject_output_yaw_ -= 2 * M_PI; + while (traject_output_yaw_ < -M_PI) + traject_output_yaw_ += 2 * M_PI; if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) { using_traject_ = false; @@ -442,10 +456,14 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d void BulletSolver::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) + double v_yaw, double r1, double r2, double dz, double armors_num) { + if (armors_num == 3) + { + armors_num = 3.6; + } double r = r1, z = pos.z; - if (armors_num == 4 && selected_armor_[0] != 0) + if (armors_num == 4 && selected_armor_[0] % 2 != 0) { r = r2; z = pos.z + dz; @@ -525,9 +543,13 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, - double bullet_speed) + double r1, double r2, double dz, double armors_num, double yaw_real, + double pitch_real, double bullet_speed) { + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); double delay; delay = track_target_ ? 0. : config_.center_delay; @@ -582,13 +604,17 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) } } -void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) +void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw, int id) { if (!track_target_) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel) { int ban_shoot_delay_number = static_cast(55 + config_.delay * 1000); + if (id == 6) + { + ban_shoot_delay_number = static_cast(55 + config_.outpost_delay * 1000); + } ban_shoot_delay_number = ban_shoot_delay_number > 149 ? 149 : ban_shoot_delay_number; if (selected_armor_[ban_shoot_delay_number] != selected_armor_[0]) { @@ -665,9 +691,10 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30; config.g = init_config.g; config.delay = init_config.delay; + config.outpost_delay = init_config.outpost_delay; config.center_delay = init_config.center_delay; - config.max_switch_angle = init_config.max_switch_angle; config.switch_angle_offset = init_config.switch_angle_offset; + config.outpost_switch_angle_offset = init_config.outpost_switch_angle_offset; config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; @@ -690,9 +717,10 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_800 = config.resistance_coff_qd_800, .g = config.g, .delay = config.delay, + .outpost_delay = config.outpost_delay, .center_delay = config.center_delay, - .max_switch_angle = config.max_switch_angle, .switch_angle_offset = config.switch_angle_offset, + .outpost_switch_angle_offset = config.outpost_switch_angle_offset, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 2f31c024..bf110f9f 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -240,6 +240,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, void Controller::rate(const ros::Time& time, const ros::Duration& period) { + bullet_solver_->CleanTrackCount(); if (state_changed_) { // on enter state_changed_ = false; @@ -310,7 +311,7 @@ void Controller::track(const ros::Time& time) bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); - bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); + bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw, data_track_.id); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { From f055c6222ff3fb1ba4bff304c99c0f6e65f72274 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 14 Jun 2026 19:55:31 +0800 Subject: [PATCH 16/31] Adapt to sp_track. --- .../rm_gimbal_controllers/bullet_solver.h | 8 +- .../rm_gimbal_controllers/gimbal_base.h | 26 ++- rm_gimbal_controllers/src/bullet_solver.cpp | 15 +- rm_gimbal_controllers/src/gimbal_base.cpp | 209 ++++++++++++++---- 4 files changed, 197 insertions(+), 61 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 53440a0d..d0b3cb32 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -123,10 +123,14 @@ class BulletSolver { track_count_ = 0; } + double getFlyTime() + { + return fly_time_[0]; + } 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, double armors_num); - void judgeShootBeforehand(const ros::Time& time, double v_yaw, int id); + uint8_t judgeShootBeforehand(double v_yaw, int id); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); @@ -137,7 +141,6 @@ class BulletSolver private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> shoot_beforehand_cmd_pub_; std::shared_ptr> fly_time_pub_; std::shared_ptr> bullet_solver_pub; ros::Subscriber identified_target_change_sub_; @@ -165,6 +168,7 @@ class BulletSolver double last_output_yaw_{}; double traject_vel_{}; double r_traject_{}; + double intital_yaw_{}; int shoot_num_{}; int shoot_beforehand_cmd_{}; 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 abe6ecd7..7fb57281 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -139,47 +140,64 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs); std::string getBaseFrameID(std::unordered_map joint_urdfs); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; + bool has_imu_ = true; std::unordered_map> ctrls_; std::unordered_map> pid_pos_; std::unordered_map joint_urdfs_; std::unordered_map pos_des_in_limit_; - bool has_imu_ = true; std::shared_ptr bullet_solver_; double gimbal_real_z_vel_{}; + enum class TrackSolverType + { + BULLET_SOLVER, + EXTERNAL_MPC_AIM + }; + TrackSolverType track_solver_type_{ TrackSolverType::BULLET_SOLVER }; + double external_aim_timeout_{ 0.1 }; + bool external_aim_active_{ false }; // ROS Interface ros::Time last_publish_time_{}; + ros::Time last_track_time_{}; std::unordered_map>> pos_state_pub_; std::shared_ptr> error_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; + ros::Subscriber mpc_gimbal_aim_sub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; + realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; + rm_msgs::MPCGimbalAimCmd gimbal_aim_; std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_; double publish_rate_{}; bool state_changed_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 5e4d49cf..09952dc6 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -100,8 +100,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - 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)); bullet_solver_pub.reset( new realtime_tools::RealtimePublisher(controller_nh, "bullet_solver_data", 10)); @@ -123,6 +121,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double v_yaw, double r1, double r2, double dz, double armors_num, double start_vel, double track_id) { + while (yaw > M_PI / 2) + yaw -= M_PI; + while (yaw < -M_PI / 2) + yaw += M_PI; if (armors_num == 3) { armors_num = 3.6; @@ -604,7 +606,7 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) } } -void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw, int id) +uint8_t BulletSolver::judgeShootBeforehand(double v_yaw, int id) { if (!track_target_) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; @@ -643,12 +645,7 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw, int } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; - if (shoot_beforehand_cmd_pub_->trylock()) - { - shoot_beforehand_cmd_pub_->msg_.stamp = time; - shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_; - shoot_beforehand_cmd_pub_->unlockAndPublish(); - } + return static_cast(shoot_beforehand_cmd_); } double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index bf110f9f..61c4c857 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -36,6 +36,7 @@ // #include "rm_gimbal_controllers/gimbal_base.h" +#include #include #include #include @@ -66,6 +67,26 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro chassis_vel_ = std::make_shared(chassis_vel_nh); ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); bullet_solver_ = std::make_shared(nh_bullet_solver); + std::string track_solver_type; + controller_nh.param("track_solver_type", track_solver_type, "bullet_solver"); + if (track_solver_type == "external_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_WARN("[Gimbal] track_solver_type=external_aim is deprecated, using external_mpc_aim instead"); + } + else if (track_solver_type == "external_mpc_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_INFO("[Gimbal] TRACK mode uses external MPC aim solver"); + } + else if (track_solver_type != "bullet_solver") + { + track_solver_type_ = TrackSolverType::BULLET_SOLVER; + ROS_WARN("[Gimbal] Unknown track_solver_type '%s', falling back to bullet_solver", track_solver_type.c_str()); + } + controller_nh.param("external_aim_timeout", external_aim_timeout_, 0.1); + std::string external_mpc_aim_topic; + controller_nh.param("external_mpc_aim_topic", external_mpc_aim_topic, "/sp_vision/mpc_gimbal_aim"); config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), @@ -154,10 +175,21 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_; odom2gimbal_traject_des_.transform.rotation.w = 1.; + rm_msgs::GimbalCmd default_cmd; + rm_msgs::TrackData default_track; + rm_msgs::MPCGimbalAimCmd default_aim; + cmd_rt_buffer_.initRT(default_cmd); + track_rt_buffer_.initRT(default_track); + gimbal_aim_rt_buffer_.initRT(default_aim); + 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("/sp_vision/track", 1, &Controller::trackCB, this); + mpc_gimbal_aim_sub_ = + controller_nh.subscribe(external_mpc_aim_topic, 1, &Controller::mpcGimbalAimCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); return true; } @@ -173,6 +205,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); period_ = period; time_ = time; @@ -198,7 +231,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) rate(time, period); break; case TRACK: - track(time); + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) + externalAimTrack(time); + else + track(time); break; case DIRECT: direct(time); @@ -270,6 +306,14 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) void Controller::track(const ros::Time& time) { + external_aim_active_ = false; + if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) + { + state_ = RATE; + state_changed_ = true; + rate(time, period_); + return; + } if (state_changed_) { // on enter state_changed_ = false; @@ -311,7 +355,7 @@ void Controller::track(const ros::Time& time) bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); - bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw, data_track_.id); + publishShootBeforehand(time, bullet_solver_->judgeShootBeforehand(data_track_.v_yaw, data_track_.id)); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { @@ -338,6 +382,58 @@ void Controller::track(const ros::Time& time) } } +bool Controller::externalAimIsFresh(const ros::Time& time) const +{ + if (!gimbal_aim_.valid || gimbal_aim_.header.stamp.isZero()) + return false; + const double age = (time - gimbal_aim_.header.stamp).toSec(); + return age >= -external_aim_timeout_ && age <= external_aim_timeout_; +} + +void Controller::externalAimTrack(const ros::Time& time) +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Gimbal] Enter TRACK with external aim"); + } + + external_aim_active_ = externalAimIsFresh(time); + if (external_aim_active_) + { + setDes(time, gimbal_aim_.target_yaw, gimbal_aim_.pitch, gimbal_aim_.yaw); + publishShootBeforehand(time, gimbal_aim_.shoot_cmd == 1 ? rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT : + rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + else + { + odom2gimbal_des_.header.stamp = time; + robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); + publishShootBeforehand(time, rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + { + if (error_pub_->trylock()) + { + error_pub_->msg_.stamp = time; + error_pub_->msg_.error = external_aim_active_ && std::isfinite(gimbal_aim_.error) ? gimbal_aim_.error : 1.0; + error_pub_->unlockAndPublish(); + } + last_publish_time_ = time; + } +} + +void Controller::publishShootBeforehand(const ros::Time& time, uint8_t cmd) +{ + if (shoot_beforehand_cmd_pub_ && shoot_beforehand_cmd_pub_->trylock()) + { + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = cmd; + shoot_beforehand_cmd_pub_->unlockAndPublish(); + } +} + void Controller::direct(const ros::Time& time) { if (state_changed_) @@ -469,59 +565,75 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - if (data_track_.id != 12) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) { - geometry_msgs::Point pos = data_track_.position; - double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); - pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); - pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); - pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, - data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, - data_track_.dz, data_track_.armors_num); + if (external_aim_active_ && externalAimIsFresh(time)) + { + vel_des[2] = gimbal_aim_.yaw_rate; + vel_des[1] = gimbal_aim_.pitch_rate; + } + else + { + vel_des[2] = 0.; + vel_des[1] = 0.; + } } else { - target_pos = data_track_.position; - target_vel = data_track_.velocity; - } - target_vel.x -= chassis_vel_->linear_->x(); - target_vel.y -= chassis_vel_->linear_->y(); - target_vel.z -= chassis_vel_->linear_->z(); - tf2::Vector3 target_pos_tf, target_vel_tf; - try - { - geometry_msgs::TransformStamped transform; - if (joint_urdfs_.find(2) != joint_urdfs_.end()) + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + if (data_track_.id != 12) { - transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - 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); + geometry_msgs::Point pos = data_track_.position; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); + pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); + pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, + data_track_.dz, data_track_.armors_num); } - if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + else { - vel_des[2] = bullet_solver_->getTrajectVel(); + target_pos = data_track_.position; + target_vel = data_track_.velocity; } - if (joint_urdfs_.find(1) != joint_urdfs_.end()) + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); + tf2::Vector3 target_pos_tf, target_vel_tf; + try { - transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + geometry_msgs::TransformStamped transform; + if (joint_urdfs_.find(2) != joint_urdfs_.end()) + { + transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, + data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + 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); + } + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + { + vel_des[2] = bullet_solver_->getTrajectVel(); + } + if (joint_urdfs_.find(1) != joint_urdfs_.end()) + { + transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, + data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); } - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); } } @@ -678,6 +790,11 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } +void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) +{ + gimbal_aim_rt_buffer_.writeFromNonRT(*msg); +} + void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) { ROS_INFO("[Gimbal Base] Dynamic params change"); From d45900135a0fe29da401644f5a360575c38f962b Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sat, 27 Jun 2026 16:13:12 +0800 Subject: [PATCH 17/31] Adapt to sp_vison. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 05a6ea24..0abda770 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -24,7 +24,7 @@ gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of swit gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) -gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -200.0, 200.0) +gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -400.0, 400.0) gen.add("track_rotate_outpost_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 09952dc6..33ac7a18 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -129,6 +129,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { armors_num = 3.6; } + config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; if (abs(yaw - last_yaw_) > 1.) @@ -148,6 +149,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d track_target_ = false; else if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) track_target_ = true; + if (sqrt(pow(pos.x, 2) + pow(pos.y, 2)) < 1.2) + track_target_ = false; } if (identified_target_change_) @@ -365,7 +368,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (start_traject_ && using_traject_) { - traject_max_acc_ = 200.; + traject_max_acc_ = 400.; switchtime = 0.05; while (abs(traject_max_acc_) > config_.yaw_max_acc) { From 7ebaf09ae540aba8b34ee6e49e0ae76dfdfb2a87 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 17 Jul 2026 16:05:09 +0800 Subject: [PATCH 18/31] Add new state change srv . --- .../rm_gimbal_controllers/bullet_solver.h | 2 + .../rm_gimbal_controllers/gimbal_base.h | 56 ++-- rm_gimbal_controllers/src/bullet_solver.cpp | 10 +- rm_gimbal_controllers/src/gimbal_base.cpp | 257 +++++++++++------- 4 files changed, 204 insertions(+), 121 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 d0b3cb32..c8b1d396 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -169,6 +169,8 @@ class BulletSolver double traject_vel_{}; double r_traject_{}; double intital_yaw_{}; + geometry_msgs::Vector3 pos_acc_{}; + geometry_msgs::Vector3 filter_vel_[3]{}; int shoot_num_{}; int shoot_beforehand_cmd_{}; 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 7fb57281..42b5ea17 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -57,15 +57,18 @@ #include #include #include +#include +#include +#include namespace rm_gimbal_controllers { struct GimbalConfig { - double yaw_k_v_, pitch_k_v_; - double chassis_comp_a_, chassis_comp_b_, chassis_comp_c_, - chassis_comp_d_; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d - double accel_pitch_{}, accel_yaw_{}; + double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; + double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, + chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double moment_of_inertia_{}; }; class ChassisVel @@ -126,8 +129,8 @@ class ChassisVel } private: - bool is_debug_; - int loop_count_; + bool is_debug_{}; + int loop_count_{}; std::shared_ptr> real_pub_{}, filtered_pub_{}; }; @@ -143,16 +146,34 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); + virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); + virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]); + virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], + const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]); + void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, + double k_v, const double vel_des[3], const double angle_error[3], + const double traject_angle_error[3]); void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time); + void track(const ros::Time& time, const ros::Duration& period); void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); - double feedForward(const ros::Time& time); + double gravityFeedForward(const ros::Time& time); void updateChassisVel(); double updateCompensation(double chassis_vel_angular_z); void publishShootBeforehand(const ros::Time& time, uint8_t cmd); @@ -160,16 +181,16 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); + bool TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res); + void publishTargetIsArmor(); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; - bool has_imu_ = true; std::unordered_map> ctrls_; std::unordered_map> pid_pos_; std::unordered_map joint_urdfs_; std::unordered_map pos_des_in_limit_; + bool has_imu_ = true; std::shared_ptr bullet_solver_; double gimbal_real_z_vel_{}; @@ -191,6 +212,8 @@ class Controller : public controller_interface::MultiInterfaceController cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; @@ -208,12 +231,12 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; - double chassis_compensation_; + double chassis_compensation_{}; bool dynamic_reconfig_initialized_{}; GimbalConfig config_{}; @@ -230,12 +253,11 @@ class Controller : public controller_interface::MultiInterfaceController 1.) @@ -201,10 +200,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d switch_armor_angle = acos(r1 / target_rho) - config_.outpost_switch_angle_offset; } yaw_subtract_ = yaw_[i] - output_yaw_central; - while (yaw_subtract_ > M_PI) - yaw_subtract_ -= 2 * M_PI; - while (yaw_subtract_ < -M_PI) - yaw_subtract_ += 2 * M_PI; + // while (yaw_subtract_ > M_PI) + // yaw_subtract_ -= 2 * M_PI; + // while (yaw_subtract_ < -M_PI) + // yaw_subtract_ += 2 * M_PI; double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; if (abs(v_yaw) < 2.) selected_armor_[i] = 0; @@ -454,6 +453,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d bullet_solver_pub->msg_.traject_acc_ = traject_effort_ff_; bullet_solver_pub->msg_.traject_vel_ = filtered_vel_des; bullet_solver_pub->msg_.using_traject_ = using_traject_; + bullet_solver_pub->msg_.test_number[0] = yaw_[0]; bullet_solver_pub->unlockAndPublish(); } return true; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 61c4c857..eac54149 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -50,8 +50,7 @@ namespace rm_gimbal_controllers bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { XmlRpc::XmlRpcValue xml_rpc_value; - bool enable_feedforward; - enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); + const bool enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); if (enable_feedforward) { ROS_ASSERT(xml_rpc_value.hasMember("mass_origin")); @@ -88,48 +87,55 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro std::string external_mpc_aim_topic; controller_nh.param("external_mpc_aim_topic", external_mpc_aim_topic, "/sp_vision/mpc_gimbal_aim"); - config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), - .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), - .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), - .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), - .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), - .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), - .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.) }; + config_ = { + .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), + .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), + .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), + .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), + .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), + .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), + .moment_of_inertia_ = getParam(controller_nh, "controllers/moment_of_inertia", 0.), + }; + config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; d_srv_->setCallback(cb); + std::string target_is_armor_topic; + controller_nh.param("target_is_armor_topic", target_is_armor_topic, "/vision/target_is_armor"); + controller_nh.param("initial_target_is_armor", target_is_armor_, true); + target_is_armor_pub_ = root_nh.advertise(target_is_armor_topic, 1, true); + publishTargetIsArmor(); + TrackSolver_type_srv_ = root_nh.advertiseService("/Processor/status_change", &Controller::TrackSolver_typeCB, this); hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { + // Get URDF info about joint + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } for (const auto& it : xml_rpc_value) { ros::NodeHandle nh = ros::NodeHandle(controller_nh, "controllers/" + it.first); ros::NodeHandle nh_pid_pos = ros::NodeHandle(controller_nh, "controllers/" + it.first + "/pid_pos"); - - // Get URDF info about joint - urdf::Model urdf; - int axis; - if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) - { - ROS_ERROR("Failed to parse urdf file"); - return false; - } auto joint_urdf = urdf.getJoint(getParam(nh, "joint", std::string())); if (!joint_urdf) { ROS_ERROR("Could not find joint in urdf"); return false; } - else - { - axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); - } + const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; + if (!shouldInitializeController(it.first, joint_urdf, axis)) + continue; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); ctrls_.insert(std::make_pair(axis, std::make_unique())); pid_pos_.insert(std::make_pair(axis, std::make_unique())); pos_des_in_limit_.insert(std::make_pair(axis, true)); @@ -142,23 +148,19 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro } robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); - if (!controller_nh.hasParam("imu_name")) - has_imu_ = false; + has_imu_ = controller_nh.hasParam("imu_name"); if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); hardware_interface::ImuSensorInterface* imu_sensor_interface = robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); + ROS_WARN_ONCE("has imu"); } else { ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } - if (has_imu_) - { - ROS_WARN_ONCE("has imu"); - } gimbal_des_frame_id_ = getGimbalFrameID(joint_urdfs_) + "_des"; odom2gimbal_des_.header.frame_id = "odom"; @@ -207,8 +209,6 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) data_track_ = *track_rt_buffer_.readFromNonRT(); gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); - period_ = period; - time_ = time; try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); @@ -231,10 +231,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) rate(time, period); break; case TRACK: - if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) externalAimTrack(time); else - track(time); + track(time, period); break; case DIRECT: direct(time); @@ -246,6 +246,17 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } +bool Controller::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const +{ + return true; +} + +bool Controller::shouldApplyJointLimit(int /*axis*/) const +{ + return true; +} + void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { @@ -263,7 +274,10 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, for (const auto& it : joint_urdfs_) { bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + if (shouldApplyJointLimit(it.first)) + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + else + pos_des_in_limit_[it.first] = true; } base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); @@ -293,25 +307,23 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) { double roll{}, pitch{}, yaw{}; quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); - + // determine whether to update yaw and pitch bool pitch_needs_update = std::abs(cmd_gimbal_.rate_pitch) > 1e-6; bool yaw_needs_update = std::abs(cmd_gimbal_.rate_yaw) > 1e-6; - double new_yaw = yaw_needs_update ? (yaw + period.toSec() * cmd_gimbal_.rate_yaw) : yaw; double new_pitch = pitch_needs_update ? (pitch + period.toSec() * cmd_gimbal_.rate_pitch) : pitch; - setDes(time, new_yaw, new_pitch, new_yaw, yaw_needs_update, pitch_needs_update); } } -void Controller::track(const ros::Time& time) +void Controller::track(const ros::Time& time, const ros::Duration& period) { external_aim_active_ = false; if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) { state_ = RATE; state_changed_ = true; - rate(time, period_); + rate(time, period); return; } if (state_changed_) @@ -495,16 +507,13 @@ void Controller::traj(const ros::Time& time) bool Controller::setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle) { - // 如果不需要更新该轴，保持当前值不变 if (!update) { angle = current_angle; return true; } - double upper_limit = joint_urdf->limits ? joint_urdf->limits->upper : 1e16; double lower_limit = joint_urdf->limits ? joint_urdf->limits->lower : -1e16; - if ((angle <= upper_limit && angle >= lower_limit) || (angles::two_pi_complement(angle) <= upper_limit && angles::two_pi_complement(angle) >= lower_limit)) { @@ -512,7 +521,6 @@ bool Controller::setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& } else { - // 超限时，钳位到最近的限位值 angle = std::abs(angles::shortest_angular_distance(angle, upper_limit)) < std::abs(angles::shortest_angular_distance(angle, lower_limit)) ? upper_limit : @@ -555,9 +563,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); for (int i = 0; i < 3; i++) + { angle_error[i] = angles::shortest_angular_distance(pos_real[i], pos_des[i]); - for (int i = 0; i < 3; i++) traject_angle_error[i] = angles::shortest_angular_distance(pos_real[i], traject_pos_des[i]); + } if (state_ == RATE) { vel_des[2] = cmd_gimbal_.rate_yaw; @@ -565,7 +574,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) { if (external_aim_active_ && externalAimIsFresh(time)) { @@ -604,11 +613,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::Vector3 target_pos_tf, target_vel_tf; try { - geometry_msgs::TransformStamped transform; if (joint_urdfs_.find(2) != joint_urdfs_.end()) { - transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); @@ -621,8 +629,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { - transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); @@ -640,49 +648,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + feedForward(time)); - } - if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) - { - if (state_ == TRACK) - { - pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); - } - else - { - pid_pos_.at(2)->computeCommand(angle_error[2], period); - } - if (state_ == TRACK) - { - if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); - } - else - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); - } - } - else - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - 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); - } + updatePitchJoint(time, period, angular_vel, vel_des, angle_error); + updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, + traject_angle_error); // publish state if (loop_count_ % 10 == 0) @@ -706,7 +674,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) loop_count_++; } -double Controller::feedForward(const ros::Time& time) +double Controller::gravityFeedForward(const ros::Time& time) { if (joint_urdfs_.find(1) != joint_urdfs_.end()) { @@ -748,7 +716,78 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]) +{ + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) + { + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); + } +} + +void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], + const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], + const double angle_error[3], const double /*traject_pos_des*/[3], + const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) + return; + + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); +} + +void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, + control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], + const double angle_error[3], const double traject_angle_error[3]) +{ + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) + { + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); + ctrls_.at(2)->joint_.setCommand(cmd); + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + } + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); +} + +std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) return joint_urdfs.at(1)->child_link_name.c_str(); @@ -759,7 +798,7 @@ std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) return joint_urdfs.at(2)->parent_link_name.c_str(); @@ -795,6 +834,24 @@ void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) gimbal_aim_rt_buffer_.writeFromNonRT(*msg); } +bool Controller::TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res) +{ + const bool previous = target_is_armor_; + target_is_armor_ = req.target == rm_msgs::StatusChangeRequest::ARMOR; + publishTargetIsArmor(); + if (previous != target_is_armor_) + ROS_INFO("[Gimbal] armor vision pipeline %s", target_is_armor_ ? "enabled" : "disabled"); + res.switch_is_success = true; + return true; +} + +void Controller::publishTargetIsArmor() +{ + std_msgs::Bool msg; + msg.data = target_is_armor_; + target_is_armor_pub_.publish(msg); +} + void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) { ROS_INFO("[Gimbal Base] Dynamic params change"); @@ -809,16 +866,18 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin config.chassis_comp_d_ = init_config.chassis_comp_d_; config.accel_pitch_ = init_config.accel_pitch_; config.accel_yaw_ = init_config.accel_yaw_; + config.moment_of_inertia_ = init_config.moment_of_inertia_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, .pitch_k_v_ = config.pitch_k_v_, + .accel_pitch_ = config.accel_pitch_, + .accel_yaw_ = config.accel_yaw_, .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, .chassis_comp_d_ = config.chassis_comp_d_, - .accel_pitch_ = config.accel_pitch_, - .accel_yaw_ = config.accel_yaw_ }; + .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From 5bc083126b2a4db69ed0d67d2c198a27dfa62c7b Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 17 Jul 2026 16:07:30 +0800 Subject: [PATCH 19/31] Save the old controller. --- .../rm_gimbal_controllers/gimbal_base.h | 55 ++-- rm_gimbal_controllers/src/gimbal_base.cpp | 234 +++++++----------- 2 files changed, 115 insertions(+), 174 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 42b5ea17..0ca332bf 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -58,17 +58,16 @@ #include #include #include -#include -#include namespace rm_gimbal_controllers { struct GimbalConfig { - double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; - double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, - chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d - double moment_of_inertia_{}; + double yaw_k_v_, pitch_k_v_; + double chassis_comp_a_, chassis_comp_b_, chassis_comp_c_, + chassis_comp_d_; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double accel_pitch_{}, accel_yaw_{}; + double moment_of_inertia_; }; class ChassisVel @@ -129,8 +128,8 @@ class ChassisVel } private: - bool is_debug_{}; - int loop_count_{}; + bool is_debug_; + int loop_count_; std::shared_ptr> real_pub_{}, filtered_pub_{}; }; @@ -146,34 +145,16 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); - virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); - virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, - const geometry_msgs::Vector3& angular_vel, const double vel_des[3], - const double angle_error[3]); - virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, - const geometry_msgs::Vector3& angular_vel, const double pos_real[3], - const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], - const double angle_error[3], const double traject_pos_des[3], - const double traject_angle_error[3]); - void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, - const geometry_msgs::Vector3& angular_vel, - effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, - double k_v, const double vel_des[3], const double angle_error[3], - const double traject_angle_error[3]); +private: void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time, const ros::Duration& period); + void track(const ros::Time& time); void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); - double gravityFeedForward(const ros::Time& time); + double feedForward(const ros::Time& time); void updateChassisVel(); double updateCompensation(double chassis_vel_angular_z); void publishShootBeforehand(const ros::Time& time, uint8_t cmd); @@ -182,15 +163,16 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs); + std::string getBaseFrameID(std::unordered_map joint_urdfs); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; + bool has_imu_ = true; std::unordered_map> ctrls_; std::unordered_map> pid_pos_; std::unordered_map joint_urdfs_; std::unordered_map pos_des_in_limit_; - bool has_imu_ = true; std::shared_ptr bullet_solver_; double gimbal_real_z_vel_{}; @@ -213,7 +195,6 @@ class Controller : public controller_interface::MultiInterfaceController cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; @@ -231,12 +212,12 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; - double chassis_compensation_{}; + double chassis_compensation_; bool dynamic_reconfig_initialized_{}; GimbalConfig config_{}; @@ -253,11 +234,13 @@ class Controller : public controller_interface::MultiInterfaceController(controller_nh); dynamic_reconfigure::Server::CallbackType cb = [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; d_srv_->setCallback(cb); - std::string target_is_armor_topic; - controller_nh.param("target_is_armor_topic", target_is_armor_topic, "/vision/target_is_armor"); - controller_nh.param("initial_target_is_armor", target_is_armor_, true); - target_is_armor_pub_ = root_nh.advertise(target_is_armor_topic, 1, true); - publishTargetIsArmor(); TrackSolver_type_srv_ = root_nh.advertiseService("/Processor/status_change", &Controller::TrackSolver_typeCB, this); hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { - // Get URDF info about joint - urdf::Model urdf; - if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) - { - ROS_ERROR("Failed to parse urdf file"); - return false; - } for (const auto& it : xml_rpc_value) { ros::NodeHandle nh = ros::NodeHandle(controller_nh, "controllers/" + it.first); ros::NodeHandle nh_pid_pos = ros::NodeHandle(controller_nh, "controllers/" + it.first + "/pid_pos"); + + // Get URDF info about joint + urdf::Model urdf; + int axis; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } auto joint_urdf = urdf.getJoint(getParam(nh, "joint", std::string())); if (!joint_urdf) { ROS_ERROR("Could not find joint in urdf"); return false; } - const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - if (!shouldInitializeController(it.first, joint_urdf, axis)) - continue; - joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + else + { + axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + } ctrls_.insert(std::make_pair(axis, std::make_unique())); pid_pos_.insert(std::make_pair(axis, std::make_unique())); pos_des_in_limit_.insert(std::make_pair(axis, true)); @@ -148,19 +146,23 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro } robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); - has_imu_ = controller_nh.hasParam("imu_name"); + if (!controller_nh.hasParam("imu_name")) + has_imu_ = false; if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); hardware_interface::ImuSensorInterface* imu_sensor_interface = robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); - ROS_WARN_ONCE("has imu"); } else { ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } + if (has_imu_) + { + ROS_WARN_ONCE("has imu"); + } gimbal_des_frame_id_ = getGimbalFrameID(joint_urdfs_) + "_des"; odom2gimbal_des_.header.frame_id = "odom"; @@ -209,6 +211,8 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) data_track_ = *track_rt_buffer_.readFromNonRT(); gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); + period_ = period; + time_ = time; try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); @@ -234,7 +238,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) externalAimTrack(time); else - track(time, period); + track(time); break; case DIRECT: direct(time); @@ -246,17 +250,6 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } -bool Controller::shouldInitializeController(const std::string& /*name*/, - const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const -{ - return true; -} - -bool Controller::shouldApplyJointLimit(int /*axis*/) const -{ - return true; -} - void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { @@ -274,10 +267,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, for (const auto& it : joint_urdfs_) { bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); - if (shouldApplyJointLimit(it.first)) - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); - else - pos_des_in_limit_[it.first] = true; + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); } base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); @@ -307,23 +297,25 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) { double roll{}, pitch{}, yaw{}; quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); - // determine whether to update yaw and pitch + bool pitch_needs_update = std::abs(cmd_gimbal_.rate_pitch) > 1e-6; bool yaw_needs_update = std::abs(cmd_gimbal_.rate_yaw) > 1e-6; + double new_yaw = yaw_needs_update ? (yaw + period.toSec() * cmd_gimbal_.rate_yaw) : yaw; double new_pitch = pitch_needs_update ? (pitch + period.toSec() * cmd_gimbal_.rate_pitch) : pitch; + setDes(time, new_yaw, new_pitch, new_yaw, yaw_needs_update, pitch_needs_update); } } -void Controller::track(const ros::Time& time, const ros::Duration& period) +void Controller::track(const ros::Time& time) { external_aim_active_ = false; if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) { state_ = RATE; state_changed_ = true; - rate(time, period); + rate(time, period_); return; } if (state_changed_) @@ -507,13 +499,16 @@ void Controller::traj(const ros::Time& time) bool Controller::setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle) { + // 如果不需要更新该轴，保持当前值不变 if (!update) { angle = current_angle; return true; } + double upper_limit = joint_urdf->limits ? joint_urdf->limits->upper : 1e16; double lower_limit = joint_urdf->limits ? joint_urdf->limits->lower : -1e16; + if ((angle <= upper_limit && angle >= lower_limit) || (angles::two_pi_complement(angle) <= upper_limit && angles::two_pi_complement(angle) >= lower_limit)) { @@ -521,6 +516,7 @@ bool Controller::setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& } else { + // 超限时，钳位到最近的限位值 angle = std::abs(angles::shortest_angular_distance(angle, upper_limit)) < std::abs(angles::shortest_angular_distance(angle, lower_limit)) ? upper_limit : @@ -563,10 +559,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); for (int i = 0; i < 3; i++) - { angle_error[i] = angles::shortest_angular_distance(pos_real[i], pos_des[i]); + for (int i = 0; i < 3; i++) traject_angle_error[i] = angles::shortest_angular_distance(pos_real[i], traject_pos_des[i]); - } if (state_ == RATE) { vel_des[2] = cmd_gimbal_.rate_yaw; @@ -613,10 +608,11 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::Vector3 target_pos_tf, target_vel_tf; try { + geometry_msgs::TransformStamped transform; if (joint_urdfs_.find(2) != joint_urdfs_.end()) { - geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); + transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, + data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); @@ -629,8 +625,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { - geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, + data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); @@ -648,9 +644,53 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - updatePitchJoint(time, period, angular_vel, vel_des, angle_error); - updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, - traject_angle_error); + + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) + { + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + feedForward(time)); + } + if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) + { + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) + { + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ctrls_.at(2)->joint_.setCommand(cmd); + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); + } + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); + } // publish state if (loop_count_ % 10 == 0) @@ -674,7 +714,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) loop_count_++; } -double Controller::gravityFeedForward(const ros::Time& time) +double Controller::feedForward(const ros::Time& time) { if (joint_urdfs_.find(1) != joint_urdfs_.end()) { @@ -716,78 +756,7 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, - const geometry_msgs::Vector3& angular_vel, const double vel_des[3], - const double angle_error[3]) -{ - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); - } -} - -void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, - const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], - const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], - const double angle_error[3], const double /*traject_pos_des*/[3], - const double traject_angle_error[3]) -{ - if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) - return; - - updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, - angle_error, traject_angle_error); -} - -void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, - const geometry_msgs::Vector3& angular_vel, - effort_controllers::JointVelocityController& ctrl, - control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], - const double angle_error[3], const double traject_angle_error[3]) -{ - if (state_ == TRACK) - { - pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); - } - else - { - pid_pos_.at(2)->computeCommand(angle_error[2], period); - } - if (state_ == TRACK) - { - if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - 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); - auto cmd = ctrls_.at(2)->joint_.getCommand(); - cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; - ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); - ctrls_.at(2)->joint_.setCommand(cmd); - } - else - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); - } - } - else - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - 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); -} - -std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) +std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) return joint_urdfs.at(1)->child_link_name.c_str(); @@ -798,7 +767,7 @@ std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) +std::string Controller::getBaseFrameID(std::unordered_map joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) return joint_urdfs.at(2)->parent_link_name.c_str(); @@ -836,22 +805,11 @@ void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) bool Controller::TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res) { - const bool previous = target_is_armor_; - target_is_armor_ = req.target == rm_msgs::StatusChangeRequest::ARMOR; - publishTargetIsArmor(); - if (previous != target_is_armor_) - ROS_INFO("[Gimbal] armor vision pipeline %s", target_is_armor_ ? "enabled" : "disabled"); + target_is_armor_ = (req.target == rm_msgs::StatusChangeRequest::ARMOR); res.switch_is_success = true; return true; } -void Controller::publishTargetIsArmor() -{ - std_msgs::Bool msg; - msg.data = target_is_armor_; - target_is_armor_pub_.publish(msg); -} - void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) { ROS_INFO("[Gimbal Base] Dynamic params change"); @@ -871,12 +829,12 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, .pitch_k_v_ = config.pitch_k_v_, - .accel_pitch_ = config.accel_pitch_, - .accel_yaw_ = config.accel_yaw_, .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, .chassis_comp_d_ = config.chassis_comp_d_, + .accel_pitch_ = config.accel_pitch_, + .accel_yaw_ = config.accel_yaw_, .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From 0bc40e6d3a76317f96acc2f8d70d6ae62a405857 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 13 Oct 2026 14:27:47 +0800 Subject: [PATCH 20/31] Add the final code and tidy up the formatting. --- rm_gimbal_controllers/CMakeLists.txt | 10 +- .../rm_gimbal_controllers/bullet_solver.h | 2 - .../rm_gimbal_controllers/gimbal_base.h | 55 ++-- rm_gimbal_controllers/package.xml | 1 + .../rm_gimbal_controllers_plugins.xml | 7 + rm_gimbal_controllers/src/gimbal_base.cpp | 236 +++++++++++------- .../orientation_controller.h | 5 + .../src/orientation_controller.cpp | 50 ++++ 8 files changed, 242 insertions(+), 124 deletions(-) diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index fb4183bd..50971382 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -14,9 +14,8 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp - + std_msgs rm_common - controller_interface effort_controllers tf2_eigen @@ -27,6 +26,7 @@ find_package(catkin REQUIRED ) generate_dynamic_reconfigure_options( + # cfg/BallisticSolver.cfg cfg/BulletSolver.cfg cfg/GimbalBase.cfg ) @@ -46,9 +46,8 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp - + std_msgs rm_common - effort_controllers tf2_eigen tf2_geometry_msgs @@ -68,8 +67,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp - ) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 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 c8b1d396..d0b3cb32 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -169,8 +169,6 @@ class BulletSolver double traject_vel_{}; double r_traject_{}; double intital_yaw_{}; - geometry_msgs::Vector3 pos_acc_{}; - geometry_msgs::Vector3 filter_vel_[3]{}; int shoot_num_{}; int shoot_beforehand_cmd_{}; 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 0ca332bf..42b5ea17 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -58,16 +58,17 @@ #include #include #include +#include +#include namespace rm_gimbal_controllers { struct GimbalConfig { - double yaw_k_v_, pitch_k_v_; - double chassis_comp_a_, chassis_comp_b_, chassis_comp_c_, - chassis_comp_d_; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d - double accel_pitch_{}, accel_yaw_{}; - double moment_of_inertia_; + double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; + double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, + chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double moment_of_inertia_{}; }; class ChassisVel @@ -128,8 +129,8 @@ class ChassisVel } private: - bool is_debug_; - int loop_count_; + bool is_debug_{}; + int loop_count_{}; std::shared_ptr> real_pub_{}, filtered_pub_{}; }; @@ -145,16 +146,34 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); + virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); + virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]); + virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], + const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]); + void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, + double k_v, const double vel_des[3], const double angle_error[3], + const double traject_angle_error[3]); void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time); + void track(const ros::Time& time, const ros::Duration& period); void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); - double feedForward(const ros::Time& time); + double gravityFeedForward(const ros::Time& time); void updateChassisVel(); double updateCompensation(double chassis_vel_angular_z); void publishShootBeforehand(const ros::Time& time, uint8_t cmd); @@ -163,16 +182,15 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); + void publishTargetIsArmor(); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; - bool has_imu_ = true; std::unordered_map> ctrls_; std::unordered_map> pid_pos_; std::unordered_map joint_urdfs_; std::unordered_map pos_des_in_limit_; + bool has_imu_ = true; std::shared_ptr bullet_solver_; double gimbal_real_z_vel_{}; @@ -195,6 +213,7 @@ class Controller : public controller_interface::MultiInterfaceController cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; @@ -212,12 +231,12 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; - double chassis_compensation_; + double chassis_compensation_{}; bool dynamic_reconfig_initialized_{}; GimbalConfig config_{}; @@ -234,13 +253,11 @@ class Controller : public controller_interface::MultiInterfaceControllercatkin roscpp + std_msgs rm_common diff --git a/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml b/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml index 8f436457..b12da6bd 100644 --- a/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml +++ b/rm_gimbal_controllers/rm_gimbal_controllers_plugins.xml @@ -8,4 +8,11 @@ + + + The DualYawController controls base_yaw, yaw and pitch from one GimbalCmd source. + + + diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index ac726718..0c5d5b75 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -50,8 +50,7 @@ namespace rm_gimbal_controllers bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { XmlRpc::XmlRpcValue xml_rpc_value; - bool enable_feedforward; - enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); + const bool enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); if (enable_feedforward) { ROS_ASSERT(xml_rpc_value.hasMember("mass_origin")); @@ -91,49 +90,52 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), - .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), - .moment_of_inertia_ = getParam(controller_nh, "controllers/moment_of_inertia", 0.), + .moment_of_inertia_ = getParam(controller_nh, "moment_of_inertia", 0.), }; + config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; d_srv_->setCallback(cb); + std::string target_is_armor_topic; + controller_nh.param("target_is_armor_topic", target_is_armor_topic, "/vision/target_is_armor"); + controller_nh.param("initial_target_is_armor", target_is_armor_, true); + target_is_armor_pub_ = root_nh.advertise(target_is_armor_topic, 1, true); + publishTargetIsArmor(); TrackSolver_type_srv_ = root_nh.advertiseService("/Processor/status_change", &Controller::TrackSolver_typeCB, this); hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { + // Get URDF info about joint + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } for (const auto& it : xml_rpc_value) { ros::NodeHandle nh = ros::NodeHandle(controller_nh, "controllers/" + it.first); ros::NodeHandle nh_pid_pos = ros::NodeHandle(controller_nh, "controllers/" + it.first + "/pid_pos"); - - // Get URDF info about joint - urdf::Model urdf; - int axis; - if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) - { - ROS_ERROR("Failed to parse urdf file"); - return false; - } auto joint_urdf = urdf.getJoint(getParam(nh, "joint", std::string())); if (!joint_urdf) { ROS_ERROR("Could not find joint in urdf"); return false; } - else - { - axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); - } + const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; + if (!shouldInitializeController(it.first, joint_urdf, axis)) + continue; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); ctrls_.insert(std::make_pair(axis, std::make_unique())); pid_pos_.insert(std::make_pair(axis, std::make_unique())); pos_des_in_limit_.insert(std::make_pair(axis, true)); @@ -146,23 +148,19 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro } robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); - if (!controller_nh.hasParam("imu_name")) - has_imu_ = false; + has_imu_ = controller_nh.hasParam("imu_name"); if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); hardware_interface::ImuSensorInterface* imu_sensor_interface = robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); + ROS_WARN_ONCE("has imu"); } else { ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } - if (has_imu_) - { - ROS_WARN_ONCE("has imu"); - } gimbal_des_frame_id_ = getGimbalFrameID(joint_urdfs_) + "_des"; odom2gimbal_des_.header.frame_id = "odom"; @@ -211,8 +209,6 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) data_track_ = *track_rt_buffer_.readFromNonRT(); gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); - period_ = period; - time_ = time; try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); @@ -238,7 +234,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) externalAimTrack(time); else - track(time); + track(time, period); break; case DIRECT: direct(time); @@ -250,6 +246,17 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } +bool Controller::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const +{ + return true; +} + +bool Controller::shouldApplyJointLimit(int /*axis*/) const +{ + return true; +} + void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { @@ -267,7 +274,10 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, for (const auto& it : joint_urdfs_) { bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + if (shouldApplyJointLimit(it.first)) + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + else + pos_des_in_limit_[it.first] = true; } base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); @@ -297,25 +307,23 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) { double roll{}, pitch{}, yaw{}; quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); - + // determine whether to update yaw and pitch bool pitch_needs_update = std::abs(cmd_gimbal_.rate_pitch) > 1e-6; bool yaw_needs_update = std::abs(cmd_gimbal_.rate_yaw) > 1e-6; - double new_yaw = yaw_needs_update ? (yaw + period.toSec() * cmd_gimbal_.rate_yaw) : yaw; double new_pitch = pitch_needs_update ? (pitch + period.toSec() * cmd_gimbal_.rate_pitch) : pitch; - setDes(time, new_yaw, new_pitch, new_yaw, yaw_needs_update, pitch_needs_update); } } -void Controller::track(const ros::Time& time) +void Controller::track(const ros::Time& time, const ros::Duration& period) { external_aim_active_ = false; if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) { state_ = RATE; state_changed_ = true; - rate(time, period_); + rate(time, period); return; } if (state_changed_) @@ -499,16 +507,13 @@ void Controller::traj(const ros::Time& time) bool Controller::setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle) { - // 如果不需要更新该轴，保持当前值不变 if (!update) { angle = current_angle; return true; } - double upper_limit = joint_urdf->limits ? joint_urdf->limits->upper : 1e16; double lower_limit = joint_urdf->limits ? joint_urdf->limits->lower : -1e16; - if ((angle <= upper_limit && angle >= lower_limit) || (angles::two_pi_complement(angle) <= upper_limit && angles::two_pi_complement(angle) >= lower_limit)) { @@ -516,7 +521,6 @@ bool Controller::setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& } else { - // 超限时，钳位到最近的限位值 angle = std::abs(angles::shortest_angular_distance(angle, upper_limit)) < std::abs(angles::shortest_angular_distance(angle, lower_limit)) ? upper_limit : @@ -559,9 +563,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); for (int i = 0; i < 3; i++) + { angle_error[i] = angles::shortest_angular_distance(pos_real[i], pos_des[i]); - for (int i = 0; i < 3; i++) traject_angle_error[i] = angles::shortest_angular_distance(pos_real[i], traject_pos_des[i]); + } if (state_ == RATE) { vel_des[2] = cmd_gimbal_.rate_yaw; @@ -608,11 +613,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) tf2::Vector3 target_pos_tf, target_vel_tf; try { - geometry_msgs::TransformStamped transform; if (joint_urdfs_.find(2) != joint_urdfs_.end()) { - transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); @@ -625,8 +629,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } if (joint_urdfs_.find(1) != joint_urdfs_.end()) { - transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); @@ -644,53 +648,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + feedForward(time)); - } - if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) - { - if (state_ == TRACK) - { - pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); - } - else - { - pid_pos_.at(2)->computeCommand(angle_error[2], period); - } - if (state_ == TRACK) - { - if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - 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); - auto cmd = ctrls_.at(2)->joint_.getCommand(); - cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; - ctrls_.at(2)->joint_.setCommand(cmd); - } - else - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z); - } - } - else - { - ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - - 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); - } + updatePitchJoint(time, period, angular_vel, vel_des, angle_error); + updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, + traject_angle_error); // publish state if (loop_count_ % 10 == 0) @@ -714,7 +674,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) loop_count_++; } -double Controller::feedForward(const ros::Time& time) +double Controller::gravityFeedForward(const ros::Time& time) { if (joint_urdfs_.find(1) != joint_urdfs_.end()) { @@ -756,7 +716,78 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]) +{ + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) + { + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); + } +} + +void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], + const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], + const double angle_error[3], const double /*traject_pos_des*/[3], + const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) + return; + + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); +} + +void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, + control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], + const double angle_error[3], const double traject_angle_error[3]) +{ + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) + { + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); + ctrls_.at(2)->joint_.setCommand(cmd); + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); + } + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); + } +} + +std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) return joint_urdfs.at(1)->child_link_name.c_str(); @@ -767,7 +798,7 @@ std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) return joint_urdfs.at(2)->parent_link_name.c_str(); @@ -805,11 +836,22 @@ void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) bool Controller::TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res) { - target_is_armor_ = (req.target == rm_msgs::StatusChangeRequest::ARMOR); + const bool previous = target_is_armor_; + target_is_armor_ = req.target == rm_msgs::StatusChangeRequest::ARMOR; + publishTargetIsArmor(); + if (previous != target_is_armor_) + ROS_INFO("[Gimbal] armor vision pipeline %s", target_is_armor_ ? "enabled" : "disabled"); res.switch_is_success = true; return true; } +void Controller::publishTargetIsArmor() +{ + std_msgs::Bool msg; + msg.data = target_is_armor_; + target_is_armor_pub_.publish(msg); +} + void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) { ROS_INFO("[Gimbal Base] Dynamic params change"); @@ -829,12 +871,12 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, .pitch_k_v_ = config.pitch_k_v_, + .accel_pitch_ = config.accel_pitch_, + .accel_yaw_ = config.accel_yaw_, .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, .chassis_comp_d_ = config.chassis_comp_d_, - .accel_pitch_ = config.accel_pitch_, - .accel_yaw_ = config.accel_yaw_, .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } diff --git a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h index b03ac226..ef653261 100644 --- a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h +++ b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h @@ -10,6 +10,8 @@ #include #include #include +#include +#include namespace rm_orientation_controller { @@ -25,6 +27,7 @@ class Controller : public controller_interface::MultiInterfaceController> assembly_error_pub_; bool receive_imu_msg_ = false; + int loop_count_{}; }; } // namespace rm_orientation_controller diff --git a/rm_orientation_controller/src/orientation_controller.cpp b/rm_orientation_controller/src/orientation_controller.cpp index 35640f02..1db309ed 100644 --- a/rm_orientation_controller/src/orientation_controller.cpp +++ b/rm_orientation_controller/src/orientation_controller.cpp @@ -23,6 +23,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro tf_broadcaster_.init(root_nh); imu_data_sub_ = root_nh.subscribe("data", 1, &Controller::imuDataCallback, this); + assembly_error_pub_.reset( + new realtime_tools::RealtimePublisher(root_nh, "imu_assembly_error", 10)); source2target_msg_.header.frame_id = frame_source_; source2target_msg_.child_frame_id = frame_target_; source2target_msg_.transform.rotation.w = 1.0; @@ -48,6 +50,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) if (!receive_imu_msg_) tf_broadcaster_.sendTransform(source2target_msg_); } + AssemblyErrorPub(time); } bool Controller::getTransform(const ros::Time& time, geometry_msgs::TransformStamped& source2target, const double x, @@ -90,6 +93,53 @@ void Controller::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) tf_broadcaster_.sendTransform(source2target); } +void Controller::AssemblyErrorPub(const ros::Time& time) +{ + double roll_error, pitch_error, yaw_error, roll_imu, pitch_imu, yaw_imu; + geometry_msgs::TransformStamped tf_source2target, tf_target2imu; + tf2::Transform source2target, target2imu; + Eigen::Matrix3d roll_eigen, pitch_eigen, yaw_eigen, R_eigen; + Eigen::Vector3d error_eigen, assembly_error_eigen; + try + { + tf_source2target = robot_state_.lookupTransform(frame_source_, frame_target_, time); + tf_target2imu = robot_state_.lookupTransform(frame_target_, imu_sensor_.getFrameId(), time); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + } + tf2::fromMsg(tf_source2target.transform, source2target); + tf2::Matrix3x3(source2target.getRotation()).getRPY(roll_error, pitch_error, yaw_error); + tf2::fromMsg(tf_target2imu.transform, target2imu); + tf2::Matrix3x3(target2imu.getRotation()).getRPY(roll_imu, pitch_imu, yaw_imu); + roll_imu = static_cast(roll_imu * 2 / M_PI); + pitch_imu = static_cast(pitch_imu * 2 / M_PI); + yaw_imu = static_cast(yaw_imu * 2 / M_PI); + roll_eigen << 1, 0, 0, 0, cos(roll_imu * M_PI / 2), -sin(roll_imu * M_PI / 2), 0, sin(roll_imu * M_PI / 2), + cos(roll_imu * M_PI / 2); + pitch_eigen << cos(pitch_imu * M_PI / 2), 0, sin(pitch_imu * M_PI / 2), 0, 1, 0, -sin(pitch_imu * M_PI / 2), 0, + cos(pitch_imu * M_PI / 2); + yaw_eigen << cos(yaw_imu * M_PI / 2), -sin(yaw_imu * M_PI / 2), 0, sin(yaw_imu * M_PI / 2), cos(yaw_imu * M_PI / 2), + 0, 0, 0, 1; + R_eigen = roll_eigen * pitch_eigen * yaw_eigen; + error_eigen << roll_error, pitch_error, 0.0; + assembly_error_eigen = R_eigen * error_eigen; + if (loop_count_ % 100 == 0) + { + if (assembly_error_pub_->trylock()) + { + assembly_error_pub_->msg_.header.stamp = time; + assembly_error_pub_->msg_.roll_error = assembly_error_eigen[0]; + assembly_error_pub_->msg_.pitch_error = assembly_error_eigen[1]; + assembly_error_pub_->msg_.yaw_error = assembly_error_eigen[2]; + assembly_error_pub_->unlockAndPublish(); + } + else + ROS_WARN_THROTTLE(1, "Can't publish assembly error data"); + } + loop_count_++; +} } // namespace rm_orientation_controller PLUGINLIB_EXPORT_CLASS(rm_orientation_controller::Controller, controller_interface::ControllerBase) From 5d210f094c16cf2ea90b1ec690782bd17d86733b Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 13 Oct 2026 14:31:43 +0800 Subject: [PATCH 21/31] Remove redundant code. --- .../test/gimbal_config_template.yaml | 32 ------------------- .../test/load_controllers.launch | 13 -------- 2 files changed, 45 deletions(-) delete mode 100644 rm_gimbal_controllers/test/gimbal_config_template.yaml delete mode 100644 rm_gimbal_controllers/test/load_controllers.launch diff --git a/rm_gimbal_controllers/test/gimbal_config_template.yaml b/rm_gimbal_controllers/test/gimbal_config_template.yaml deleted file mode 100644 index 37e9b3ab..00000000 --- a/rm_gimbal_controllers/test/gimbal_config_template.yaml +++ /dev/null @@ -1,32 +0,0 @@ -controllers: - robot_state_controller: - type: robot_state_controller/RobotStateController - publish_rate: 50 - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - gimbal_controller: - type: rm_gimbal_controllers/Controller - yaw: - joint: "yaw_joint" - pid: { p: 7.5, i: 0, d: 0.3, i_clamp_max: 0.3, i_clamp_min: -0.3, antiwindup: true, publish_state: true } - k_chassis_vel: 0.01 - pitch: - joint: "pitch_joint" - pid: { p: 22.0, i: 0, d: 0.75, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - imu_name: "gimbal_imu" - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 5.0 - g: 9.81 - delay: 0.15 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 diff --git a/rm_gimbal_controllers/test/load_controllers.launch b/rm_gimbal_controllers/test/load_controllers.launch deleted file mode 100644 index e2eb54f5..00000000 --- a/rm_gimbal_controllers/test/load_controllers.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - From 5af56c67166765aee5f2c83f722836238d90ac9a Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 13 Oct 2026 14:32:50 +0800 Subject: [PATCH 22/31] Remove redundant code,add the cfg. --- rm_gimbal_controllers/README.md | 179 ----------------------- rm_gimbal_controllers/cfg/GimbalBase.cfg | 1 + 2 files changed, 1 insertion(+), 179 deletions(-) delete mode 100644 rm_gimbal_controllers/README.md diff --git a/rm_gimbal_controllers/README.md b/rm_gimbal_controllers/README.md deleted file mode 100644 index 71378730..00000000 --- a/rm_gimbal_controllers/README.md +++ /dev/null @@ -1,179 +0,0 @@ -# rm_gimbal_controllers - -## Overview - -The rm_gimbal_controllers has three states: RATE, TRACK, and DIRECT. It performs PID control on the yaw joint and pitch joint according to commands. It can also perform moving average filtering based on detection data and calculate, predict and track targets based on the ballistic model. - -**Keywords:** ROS, Robomaster, gimbal, bullet solver, moving average filter - -### License - -The source code is released under a [ BSD 3-Clause license](https://github.com/rm-controls/rm_controllers/blob/master/LICENSE). - -**Author: DynamicX
-Affiliation: DynamicX
-Maintainer: DynamicX** - -The package has been tested under [ROS](https://www.ros.org/) Indigo, Melodic and Noetic on respectively Ubuntu 18.04 and 20.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. - -### Hardware interface type -+ `JointStateInterface` Used to get the speed and position of gimbal joint. -+ `EffortJointInterface` Used to send effort command to gimbal joint . -+ `RoboStateInterface` Used to get the current and historical transform between gimbal and the world coordinate system and the transform between visual target and the world coordinate system. - -## Installation - -### Installation from Packages - -To install all packages from the this repository as Debian packages use - - sudo apt-get install ros-noetic-rm-gimbal-controllers - -Or better, use `rosdep`: - - sudo rosdep install --from-paths src - -### Dependencies -* roscpp -* rm_common -* effort_controllers -* tf2_eigen -* tf2_geometry_msgs -* visualization_msgs -* dynamic_reconfigure - -## ROS API - -#### Subscribed Topics -* **`command`** (rm_msgs/GimbalCmd) - - Set gimbal mode, pitch and yaw axis rotation speed, tracking target, pointing target and coordinate system. - -* **`/detection`** (rm_msgs/TargetDetectionArray) - - Receive visual recognition data. - -* **`//camera_info`** (CameraInfo) - - Make sure that the detection node receives a new frame of image and sends the prediction data to the detection node. - -#### Published Topics -* **`error`** (rm_msgs/GimbalDesError) - The error calculated by the ballistic model to shoot at the current gimbal angle to the target. - -* **`track`** (rm_msgs/TrackDataArray) - The predicted data used for detection node to decide the ROI. -##### Bullet solver -* **`model_desire`** ( [visualization_msgs/Marker](http://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html) ) - Used to visualize the desired bullet trajectory. - -* **`model_real`** ( [visualization_msgs/Marker](http://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html) ) - Used to visualize the trajectory that caculated by ballistic model in the current gimbal angle. -#### Parameters -* **`detection_topic`** (string, default: "/detection") - - The name of the topic where detection node gets predicted data. - -* **`detection_frame`** (string, default: "detection") - - The name of the frame of detection. - -* **`camera_topic`** (string, default: "/galaxy_camera/camera_info") - - The name of the topic that is determined that the detection node receives a new frame of image and sends the prediction data to the detection node. - -* **`publish_rate`** (double) - - Frequency (in Hz) of publishing gimbal error. - -* **`chassis_angular_data_num`** (double) - - The number of angle data of chassis.Used for chassis angular average filter. - -* **`time_compensation`** (double) - - Time(in s) of image transmission delay(in s).Used to compensate for the effects of images transimission delay - -##### Bullet solver -_Bullet solver is used to get the bullet drop point_ -* **`resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, resistance_coff_qd_30`** ( `double` ) - - The air resistance coefficient used for bullet solver when bullet speed is 10 m/s, 15 m/s, 16 m/s, 18 m/s and 30 m/s. - -* **`g`** (double) - - Acceleration of gravity. - -* **`delay`** (double) - - Shooting delay time(in s) after shooter get the shooting command.Used to compensate for the effects of launch delay. - -* **`timeout`** (double) - - Timeout time((in s)) of bullet solver.Used to judge whether bullet solver can caculate the bullet drop point. - -##### Moving average filter -_Moving average filter is used for filter the target armor center when target is spin._ -* **`is_debug`** ( `bool`, default: true ) - - The debug status.When it is true, debug data will be pulished on the filter topic. - -* **`pos_data_num`** (double, default: 20) - - The number of armor position data. - -* **`vel_data_num`** (double, default: 30) - - The number of armor linear velocity data. - -* **`gyro_data_num`** (double, default: 100) - - The number of target rotation speed data. - -* **`center_data_num`** (double, default: 50) - - The number of target rotation center position data. - -* **`center_offset_z`** (double) - - Offset(in meter) on the z axis.Used to compensate for the error of filter result on z axis. - -## Controller configuration examples - -``` -gimbal_controller: - type: rm_gimbal_controllers/Controller - time_compensation: 0.03 - publish_rate: 100 - chassis_angular_data_num: 20 - camera_topic: "/galaxy_camera/camera_info" - yaw: - joint: "yaw_joint" - pid: { p: 8, i: 0, d: 0.4, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - pitch: - joint: "pitch_joint" - pid: { p: 10, i: 50, d: 0.3, i_clamp_max: 0.4, i_clamp_min: -0.4, antiwindup: true, publish_state: true } - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 0.1 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.1 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - moving_average_filter: - is_debug: true - center_offset_z: 0.05 - pos_data_num: 20 - vel_data_num: 30 - center_data_num: 50 - gyro_data_num: 100 -``` - - -## Bugs & Feature Requests - -Please report bugs and request features using the [Issue Tracker](https://github.com/rm-controls/rm_controllers/issues). diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 5f614c33..6c6a3107 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -13,5 +13,6 @@ gen.add("chassis_comp_a_",double_t, 0,"A param of chassis_compensation", 0.0, 0, gen.add("chassis_comp_b_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_c_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) gen.add("chassis_comp_d_",double_t, 0,"A param of chassis_compensation", 0.0, 0, 999.0) +gen.add("moment_of_inertia_", double_t, 0, "Moment of inertia of gimbal", 0.0, 0, 999.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) From ade5fe4851527e9e3d5227328548b67dd2a012af Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 13 Oct 2026 14:33:56 +0800 Subject: [PATCH 23/31] Add dual yaw controller. --- .../dual_yaw_controller.h | 38 +++++ .../src/dual_yaw_controller.cpp | 154 ++++++++++++++++++ 2 files changed, 192 insertions(+) create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h create mode 100644 rm_gimbal_controllers/src/dual_yaw_controller.cpp diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h new file mode 100644 index 00000000..85dccb45 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/dual_yaw_controller.h @@ -0,0 +1,38 @@ +/******************************************************************************* + * BSD 3-Clause License + ******************************************************************************/ + +#pragma once + +#include + +namespace rm_gimbal_controllers +{ +class DualYawController : public Controller +{ +public: + DualYawController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + +protected: + bool shouldInitializeController(const std::string& name, const urdf::JointConstSharedPtr& joint_urdf, + int axis) const override; + bool shouldApplyJointLimit(int axis) const override; + std::string getBaseFrameID(const std::unordered_map& joint_urdfs) override; + void updateYawJoint(const ros::Time& time, const ros::Duration& period, const geometry_msgs::Vector3& angular_vel, + const double pos_real[3], const double pos_des[3], const double pos_des_temp[3], + const double vel_des[3], const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]) override; + +private: + bool initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh); + bool getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point); + void publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error); + + urdf::JointConstSharedPtr base_yaw_joint_; + std::unique_ptr base_yaw_ctrl_; + std::unique_ptr base_yaw_pid_pos_; + std::unique_ptr> base_yaw_pos_state_pub_; +}; +} // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/dual_yaw_controller.cpp b/rm_gimbal_controllers/src/dual_yaw_controller.cpp new file mode 100644 index 00000000..69ffa1b4 --- /dev/null +++ b/rm_gimbal_controllers/src/dual_yaw_controller.cpp @@ -0,0 +1,154 @@ +#include "rm_gimbal_controllers/dual_yaw_controller.h" + +#include +#include +#include +#include +#include +#include +#include + +namespace rm_gimbal_controllers +{ +bool DualYawController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, + ros::NodeHandle& controller_nh) +{ + if (!initBaseYaw(robot_hw, controller_nh)) + return false; + return Controller::init(robot_hw, root_nh, controller_nh); +} + +bool DualYawController::initBaseYaw(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& controller_nh) +{ + ros::NodeHandle base_yaw_nh(controller_nh, "controllers/base_yaw"); + ros::NodeHandle base_yaw_pid_nh(controller_nh, "controllers/base_yaw/pid_pos"); + + urdf::Model urdf; + if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) + { + ROS_ERROR("Failed to parse urdf file"); + return false; + } + base_yaw_joint_ = urdf.getJoint(getParam(base_yaw_nh, "joint", std::string("base_yaw_joint"))); + if (!base_yaw_joint_) + { + ROS_ERROR("Could not find base_yaw joint in urdf"); + return false; + } + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); + base_yaw_ctrl_ = std::make_unique(); + base_yaw_pid_pos_ = std::make_unique(); + base_yaw_pos_state_pub_ = + std::make_unique>(base_yaw_nh, "pos_state", 1); + return base_yaw_ctrl_->init(effort_joint_interface, base_yaw_nh) && base_yaw_pid_pos_->init(base_yaw_pid_nh); +} + +bool DualYawController::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& joint_urdf, int axis) const +{ + return !(axis == 2 && base_yaw_joint_ && joint_urdf->name == base_yaw_joint_->name); +} + +bool DualYawController::shouldApplyJointLimit(int axis) const +{ + return axis != 2; +} + +std::string DualYawController::getBaseFrameID(const std::unordered_map& joint_urdfs) +{ + if (base_yaw_joint_) + return base_yaw_joint_->parent_link_name.c_str(); + return Controller::getBaseFrameID(joint_urdfs); +} + +void DualYawController::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double /*pos_des_temp*/[3], + const double vel_des[3], const double angle_error[3], + const double traject_pos_des[3], const double traject_angle_error[3]) +{ + if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) + { + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); + } + + if (!base_yaw_ctrl_ || !base_yaw_pid_pos_) + return; + + double base_yaw_pos_real = 0.; + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + double roll{}, pitch{}; + quatToRPY(odom2base_yaw.transform.rotation, roll, pitch, base_yaw_pos_real); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + + double armor_set_point{}; + double base_yaw_set_point = getTrackArmorSetPoint(time, armor_set_point) ? armor_set_point : pos_des[2]; + double base_yaw_error = angles::shortest_angular_distance(base_yaw_pos_real, base_yaw_set_point); + base_yaw_pid_pos_->computeCommand(base_yaw_error, period); + base_yaw_ctrl_->setCommand(base_yaw_pid_pos_->getCurrentCmd() - chassis_vel_->angular_->z()); + base_yaw_ctrl_->update(time, period); + publishBaseYawState(time, base_yaw_pos_real, base_yaw_set_point, vel_des, base_yaw_error); +} + +bool DualYawController::getTrackArmorSetPoint(const ros::Time& time, double& armor_set_point) +{ + if (state_ != TRACK || !base_yaw_joint_ || data_track_.header.stamp.isZero()) + return false; + + geometry_msgs::Point target_pos = data_track_.position; + if (!std::isfinite(target_pos.x) || !std::isfinite(target_pos.y) || !std::isfinite(target_pos.z)) + return false; + + try + { + geometry_msgs::TransformStamped odom2base_yaw = + robot_state_handle_.lookupTransform("odom", base_yaw_joint_->child_link_name, time); + geometry_msgs::Point base_yaw_pos_odom; + base_yaw_pos_odom.x = odom2base_yaw.transform.translation.x; + base_yaw_pos_odom.y = odom2base_yaw.transform.translation.y; + base_yaw_pos_odom.z = odom2base_yaw.transform.translation.z; + + armor_set_point = std::atan2(target_pos.y - base_yaw_pos_odom.y, target_pos.x - base_yaw_pos_odom.x); + if (!std::isfinite(armor_set_point)) + return false; + + return true; + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } +} + +void DualYawController::publishBaseYawState(const ros::Time& time, double base_yaw_pos_real, double base_yaw_set_point, + const double vel_des[3], double base_yaw_error) +{ + if (base_yaw_pos_state_pub_ && loop_count_ % 10 == 0 && base_yaw_pos_state_pub_->trylock()) + { + base_yaw_pos_state_pub_->msg_.header.stamp = time; + base_yaw_pos_state_pub_->msg_.set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.traject_set_point = base_yaw_set_point; + base_yaw_pos_state_pub_->msg_.set_point_dot = vel_des[2]; + base_yaw_pos_state_pub_->msg_.process_value = base_yaw_pos_real; + base_yaw_pos_state_pub_->msg_.error = base_yaw_error; + base_yaw_pos_state_pub_->msg_.command = base_yaw_pid_pos_->getCurrentCmd(); + base_yaw_pos_state_pub_->msg_.shoot_number = bullet_solver_->getShootnum(); + base_yaw_pos_state_pub_->unlockAndPublish(); + } +} + +} // namespace rm_gimbal_controllers + +PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::DualYawController, controller_interface::ControllerBase) From d157ed95fde631e948eb6dba166bde3b113089c5 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Wed, 14 Oct 2026 19:38:07 +0800 Subject: [PATCH 24/31] Recover the README.md file. --- rm_gimbal_controllers/README.md | 179 ++++++++++++++++++++++++++++++++ 1 file changed, 179 insertions(+) create mode 100644 rm_gimbal_controllers/README.md diff --git a/rm_gimbal_controllers/README.md b/rm_gimbal_controllers/README.md new file mode 100644 index 00000000..71378730 --- /dev/null +++ b/rm_gimbal_controllers/README.md @@ -0,0 +1,179 @@ +# rm_gimbal_controllers + +## Overview + +The rm_gimbal_controllers has three states: RATE, TRACK, and DIRECT. It performs PID control on the yaw joint and pitch joint according to commands. It can also perform moving average filtering based on detection data and calculate, predict and track targets based on the ballistic model. + +**Keywords:** ROS, Robomaster, gimbal, bullet solver, moving average filter + +### License + +The source code is released under a [ BSD 3-Clause license](https://github.com/rm-controls/rm_controllers/blob/master/LICENSE). + +**Author: DynamicX
+Affiliation: DynamicX
+Maintainer: DynamicX** + +The package has been tested under [ROS](https://www.ros.org/) Indigo, Melodic and Noetic on respectively Ubuntu 18.04 and 20.04. This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. + +### Hardware interface type ++ `JointStateInterface` Used to get the speed and position of gimbal joint. ++ `EffortJointInterface` Used to send effort command to gimbal joint . ++ `RoboStateInterface` Used to get the current and historical transform between gimbal and the world coordinate system and the transform between visual target and the world coordinate system. + +## Installation + +### Installation from Packages + +To install all packages from the this repository as Debian packages use + + sudo apt-get install ros-noetic-rm-gimbal-controllers + +Or better, use `rosdep`: + + sudo rosdep install --from-paths src + +### Dependencies +* roscpp +* rm_common +* effort_controllers +* tf2_eigen +* tf2_geometry_msgs +* visualization_msgs +* dynamic_reconfigure + +## ROS API + +#### Subscribed Topics +* **`command`** (rm_msgs/GimbalCmd) + + Set gimbal mode, pitch and yaw axis rotation speed, tracking target, pointing target and coordinate system. + +* **`/detection`** (rm_msgs/TargetDetectionArray) + + Receive visual recognition data. + +* **`//camera_info`** (CameraInfo) + + Make sure that the detection node receives a new frame of image and sends the prediction data to the detection node. + +#### Published Topics +* **`error`** (rm_msgs/GimbalDesError) + The error calculated by the ballistic model to shoot at the current gimbal angle to the target. + +* **`track`** (rm_msgs/TrackDataArray) + The predicted data used for detection node to decide the ROI. +##### Bullet solver +* **`model_desire`** ( [visualization_msgs/Marker](http://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html) ) + Used to visualize the desired bullet trajectory. + +* **`model_real`** ( [visualization_msgs/Marker](http://docs.ros.org/en/api/visualization_msgs/html/msg/Marker.html) ) + Used to visualize the trajectory that caculated by ballistic model in the current gimbal angle. +#### Parameters +* **`detection_topic`** (string, default: "/detection") + + The name of the topic where detection node gets predicted data. + +* **`detection_frame`** (string, default: "detection") + + The name of the frame of detection. + +* **`camera_topic`** (string, default: "/galaxy_camera/camera_info") + + The name of the topic that is determined that the detection node receives a new frame of image and sends the prediction data to the detection node. + +* **`publish_rate`** (double) + + Frequency (in Hz) of publishing gimbal error. + +* **`chassis_angular_data_num`** (double) + + The number of angle data of chassis.Used for chassis angular average filter. + +* **`time_compensation`** (double) + + Time(in s) of image transmission delay(in s).Used to compensate for the effects of images transimission delay + +##### Bullet solver +_Bullet solver is used to get the bullet drop point_ +* **`resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, resistance_coff_qd_30`** ( `double` ) + + The air resistance coefficient used for bullet solver when bullet speed is 10 m/s, 15 m/s, 16 m/s, 18 m/s and 30 m/s. + +* **`g`** (double) + + Acceleration of gravity. + +* **`delay`** (double) + + Shooting delay time(in s) after shooter get the shooting command.Used to compensate for the effects of launch delay. + +* **`timeout`** (double) + + Timeout time((in s)) of bullet solver.Used to judge whether bullet solver can caculate the bullet drop point. + +##### Moving average filter +_Moving average filter is used for filter the target armor center when target is spin._ +* **`is_debug`** ( `bool`, default: true ) + + The debug status.When it is true, debug data will be pulished on the filter topic. + +* **`pos_data_num`** (double, default: 20) + + The number of armor position data. + +* **`vel_data_num`** (double, default: 30) + + The number of armor linear velocity data. + +* **`gyro_data_num`** (double, default: 100) + + The number of target rotation speed data. + +* **`center_data_num`** (double, default: 50) + + The number of target rotation center position data. + +* **`center_offset_z`** (double) + + Offset(in meter) on the z axis.Used to compensate for the error of filter result on z axis. + +## Controller configuration examples + +``` +gimbal_controller: + type: rm_gimbal_controllers/Controller + time_compensation: 0.03 + publish_rate: 100 + chassis_angular_data_num: 20 + camera_topic: "/galaxy_camera/camera_info" + yaw: + joint: "yaw_joint" + pid: { p: 8, i: 0, d: 0.4, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } + pitch: + joint: "pitch_joint" + pid: { p: 10, i: 50, d: 0.3, i_clamp_max: 0.4, i_clamp_min: -0.4, antiwindup: true, publish_state: true } + bullet_solver: + resistance_coff_qd_10: 0.45 + resistance_coff_qd_15: 0.1 + resistance_coff_qd_16: 0.7 + resistance_coff_qd_18: 0.55 + resistance_coff_qd_30: 3.0 + g: 9.81 + delay: 0.1 + dt: 0.001 + timeout: 0.001 + publish_rate: 50 + moving_average_filter: + is_debug: true + center_offset_z: 0.05 + pos_data_num: 20 + vel_data_num: 30 + center_data_num: 50 + gyro_data_num: 100 +``` + + +## Bugs & Feature Requests + +Please report bugs and request features using the [Issue Tracker](https://github.com/rm-controls/rm_controllers/issues). From 17e34835542a6d30da75bae87c493b271d5de190 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Wed, 14 Oct 2026 21:39:45 +0800 Subject: [PATCH 25/31] Recover file. --- rm_gimbal_controllers/CMakeLists.txt | 6 +- rm_gimbal_controllers/cfg/BallisticSolver.cfg | 22 - rm_gimbal_controllers/cfg/BulletSolver.cfg | 14 +- .../rm_gimbal_controllers/ballistic_solver.h | 52 -- .../rm_gimbal_controllers/bullet_solver.h | 111 +-- .../rm_gimbal_controllers/gimbal_base.h | 82 ++- .../src/ballistic_solver.cpp | 174 ----- rm_gimbal_controllers/src/bullet_solver.cpp | 690 ++++++++++-------- rm_gimbal_controllers/src/gimbal_base.cpp | 493 +++++++++---- 9 files changed, 852 insertions(+), 792 deletions(-) delete mode 100644 rm_gimbal_controllers/cfg/BallisticSolver.cfg delete mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h delete mode 100644 rm_gimbal_controllers/src/ballistic_solver.cpp diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98e8736b..50971382 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -14,6 +14,7 @@ add_definitions(-Wall -Werror) find_package(catkin REQUIRED COMPONENTS roscpp + std_msgs rm_common controller_interface effort_controllers @@ -25,7 +26,7 @@ find_package(catkin REQUIRED ) generate_dynamic_reconfigure_options( - cfg/BallisticSolver.cfg + # cfg/BallisticSolver.cfg cfg/BulletSolver.cfg cfg/GimbalBase.cfg ) @@ -45,6 +46,7 @@ catkin_package( LIBRARIES CATKIN_DEPENDS roscpp + std_msgs rm_common effort_controllers tf2_eigen @@ -65,7 +67,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_gimbal_controllers/cfg/BallisticSolver.cfg b/rm_gimbal_controllers/cfg/BallisticSolver.cfg deleted file mode 100644 index 4b5f07ae..00000000 --- a/rm_gimbal_controllers/cfg/BallisticSolver.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "rm_gimbal_controllers" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -gen.add("mass", double_t, 0, "Bullet mass (kg)", 0.0445, 0.01, 0.1) -gen.add("radius", double_t, 0, "Bullet radius (m)", 0.02125, 0.01, 0.05) -gen.add("Cd_value", double_t, 0, "Fitting air drag coefficient", 0.4, 0.0, 1.0) -gen.add("Cd_distance", double_t, 0, "Distance of fitting air drag coefficient (m)", 12.0, 0.0, 30.0) -gen.add("Cd_slope", double_t, 0, "Fitting air drag coefficient change rate", 0.0, -10.0, 10.0) -gen.add("air_density", double_t, 0, "Air density (kg/m³)", 1.2, 0.8, 1.5) -gen.add("g", double_t, 0, "Gravity (m/s²)", 9.81, 9.0, 10.0) -gen.add("initial_vel", double_t, 0, "Initial velocity (m/s)", 14.0, 10.0, 20.0) -gen.add("rk4_simulate_step", double_t, 0, "Simulate step (s)", 0.01, 0.001, 0.05) -gen.add("newton_convergence_tol", double_t, 0, "Newton convergence tolerance", 2e-5, 1e-6, 1e-4) -gen.add("newton_pitch_epsilon", double_t, 0, "Finite difference epsilon", 1e-4, 1e-6, 1e-3) -gen.add("max_newton_step", double_t, 0, "Max Newton step", 0.04, 0.01, 0.1) -gen.add("max_newton_iterations", int_t, 0, "Max Newton iterations", 0, 1, 5) - -exit(gen.generate(PACKAGE, "ballistic_solver", "BallisticSolver")) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 3cf38627..0abda770 100644 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -10,22 +10,26 @@ gen.add("resistance_coff_qd_15", double_t, 0, "Air resistance divided by mass of gen.add("resistance_coff_qd_16", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("resistance_coff_qd_30", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) +gen.add("resistance_coff_qd_800", double_t, 0, "Air resistance divided by mass of 10 m/s", 0.1, 0, 5.0) gen.add("g", double_t, 0, "Air resistance divided by mass", 9.8, 9.6, 10.0) gen.add("delay", double_t, 0, "Delay of bullet firing", 0.0, 0, 0.5) +gen.add("outpost_delay", double_t, 0, "Delay of bullet firing of outpost", 0.0, 0, 0.5) gen.add("center_delay", double_t, 0, "Delay of shooting this target when in center mode", 0.105, -0.5, 0.5) 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("max_selected_armor_", int_t, 0, "Max selected armor number", 0, -4, 4) +gen.add("outpost_switch_angle_offset", double_t, 0,"Switch angle offset", 0.0, -20.0, 20) +gen.add("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 1, 99) +gen.add("traject_start_fit_", int_t, 0, "The time of traject start ahead of switch", 0, 1, 99) gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 3.0, 0.0, 20) gen.add("track_rotate_target_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("track_move_target_delay", double_t, 0, "Used to estimate current armor's position when moving", 0.0, -1.0, 1.0) +gen.add("yaw_max_acc", double_t, 0, "the max acc of yaw joint" , 0.0, -400.0, 400.0) +gen.add("track_rotate_outpost_delay", double_t, 0, "Used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("traject_ahead_",double_t, 0, "Used to switch in advance ", 0.0, -5.0, 5.0) gen.add("clean_shoot_num_", int_t, 0, "Used to clean shoot_num", 0, 0, 1) gen.add("end_pos_offset",double_t, 0, "The offset of the end pos", 0.0 , -1.0, 1.0) +gen.add("traject_k_effort",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) +gen.add("traject_k_vel",double_t, 0, "The offset of the traject_switch_time", 0.0 , -10.0, 10.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h deleted file mode 100644 index 32718ff4..00000000 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h +++ /dev/null @@ -1,52 +0,0 @@ -// -// Created by ljyi on 25-9-20. -// - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rm_gimbal_controllers -{ -struct BallisticConfig -{ - double mass, radius, gun_offset_x, gun_offset_z, Cd_value, Cd_distance, Cd_slope, air_density, g; - double initial_vel, rk4_simulate_step; - double newton_convergence_tol, newton_pitch_epsilon, max_newton_step; - int max_newton_iterations; -}; - -class BallisticSolver -{ -public: - explicit BallisticSolver(ros::NodeHandle& nh); - ~BallisticSolver() = default; - double simulate(double pitch_angle_, double initial_vel_, double target_dis, double target_hgt); - void solver(const geometry_msgs::TransformStamped& base2gimbal, double& yaw, double& pitch); - void reconfigCB(rm_gimbal_controllers::BallisticSolverConfig& config, uint32_t); - -private: - void base2targetDataCB(const geometry_msgs::PointConstPtr& msg); - ros::Subscriber base2target_data_sub_; - dynamic_reconfigure::Server* d_srv_{}; - BallisticConfig config_{}; - realtime_tools::RealtimeBuffer config_rt_buffer_; - rm_common::LinearInterp output_pitch_match_lut_; - bool dynamic_reconfig_initialized_{}; - geometry_msgs::Point base2target_; - std::mutex data_mutex_; - std::atomic base2target_data_valid_{ false }; -}; -} // namespace rm_gimbal_controllers 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 163f8475..d0b3cb32 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -57,15 +57,15 @@ 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, center_delay, max_switch_angle, switch_angle_offset, switch_duration_scale, - switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, track_rotate_target_delay, - track_move_target_delay; - int min_fit_switch_count; - int max_selected_armor_; + double resistance_coff_qd_1, resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, + resistance_coff_qd_18, resistance_coff_qd_30, resistance_coff_qd_800, g, delay, outpost_delay, center_delay, + max_switch_angle, switch_angle_offset, outpost_switch_angle_offset, min_shoot_beforehand_vel, + track_rotate_target_delay, track_move_target_delay, yaw_max_acc, track_rotate_outpost_delay; + int min_fit_switch_count, traject_start_fit_; double traject_ahead_; int clean_shoot_num_; double end_pos_offset; + double traject_k_effort, traject_k_vel_; }; struct TrajectoryFunctionCoefficients { @@ -82,22 +82,27 @@ class BulletSolver explicit BulletSolver(ros::NodeHandle& controller_nh); bool solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double start_vel); + double r1, double r2, double dz, double armors_num, double start_vel, double track_id); double getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, - double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed); - double getResistanceCoefficient(double bullet_speed) const; + double r2, double dz, double armors_num, double yaw_real, double pitch_real, + double bullet_speed); + double getResistanceCoefficient(double target_distance) const; double getYaw() const { - return output_yaw_; + return output_yaw_[0]; } double getPitch() const { - return -output_pitch_; + return -output_pitch_[0]; } double getTrajectYaw() const { return traject_output_yaw_; } + double getTrajectVel() const + { + return traject_vel_; + } bool getUsingtraject() const { return using_traject_; @@ -110,26 +115,32 @@ class BulletSolver { return traject_effort_ff_; } - bool getTrackTarget() + bool getTrackTarget() const { return track_target_; } - double getGimbalSwitchDuration(double v_yaw); + void CleanTrackCount() + { + track_count_ = 0; + } + double getFlyTime() + { + return fly_time_[0]; + } 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); - void judgeShootBeforehand(const ros::Time& time, double v_yaw); + double r1, double r2, double dz, double armors_num); + uint8_t judgeShootBeforehand(double v_yaw, int id); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); - double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_); + double planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw); void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg); ~BulletSolver() = default; private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> shoot_beforehand_cmd_pub_; std::shared_ptr> fly_time_pub_; std::shared_ptr> bullet_solver_pub; ros::Subscriber identified_target_change_sub_; @@ -138,46 +149,56 @@ class BulletSolver realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; - double max_track_target_vel_; - double output_yaw_{}, output_pitch_{}, traject_output_yaw_{}; + double yaw_[150], pos_x[150], pos_y[150]; + double max_track_target_vel_{}; + double output_yaw_[150], output_pitch_[150], traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; - double fly_time_; - double switch_hysteresis_; + double fly_time_[150]; + double switch_hysteresis_{}; double last_yaw_{}, filtered_yaw_{}; double gimbal_switch_duration_{}; - double yaw_subtract_; - double switch_armor_angle; + double yaw_subtract_{}; + double switch_armor_angle{}; double filtered_v_yaw_{}; - double switchtime; - double traject_effort_ff_; - double traject_switch_time_; - double switche_time_yaw_; - double traject_max_acc_; - int shoot_num_ = 0; - int shoot_beforehand_cmd_{}; - int count_; - int traject_count_; - int ban_shoot_count_ = 0; - int selected_armor_ = 0; + double switchtime{}; + double traject_effort_ff_{}; + double traject_switch_time_{}; + double switche_time_yaw_{}; + double traject_max_acc_{}; + double last_output_yaw_{}; + double traject_vel_{}; + double r_traject_{}; + double intital_yaw_{}; - bool track_target_ = true; + int shoot_num_{}; + int shoot_beforehand_cmd_{}; + int count_[150]{}; + int next_count_[150]{}; + int ban_shoot_count_{}; + int selected_armor_[150] = {}; + int last_selected_armor_ = {}; + bool track_target_ = false; + int track_count_{}; bool identified_target_change_ = true; - bool is_in_delay_before_switch_{}; bool dynamic_reconfig_initialized_{}; - bool change_armor = false; - bool using_traject_; - bool last_shoot_state_; + bool using_traject_{}; + bool last_shoot_state_{}; + bool is_aheading_two_[150]{}; + bool start_traject_{}; + double filtered_vel_des{}; geometry_msgs::Point after_traject_output_yaw_{}; - geometry_msgs::Point target_pos_{}; + geometry_msgs::Point target_pos_[150]{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; - ros::Time start_using_traject_time; - ros::Time ban_shoot_time_; + ros::Time start_using_traject_time{}; + ros::Time ban_shoot_time_{}; + ros::Time last_output_time_{}; + ros::Time ban_shoot_start_time_{}; - TrajectoryFunctionCoefficients trajectory_function_coefficients; - TrajectoryLimitParams stauts_limit_; + TrajectoryFunctionCoefficients trajectory_function_coefficients{}; + TrajectoryLimitParams stauts_limit_{}; - mutable std::mutex heat_mutex_; + mutable std::mutex heat_mutex_{}; }; } // namespace rm_gimbal_controllers 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 50c3eff2..42b5ea17 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -44,12 +44,12 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -57,22 +57,24 @@ #include #include #include -#include -#include -#include +#include +#include +#include namespace rm_gimbal_controllers { struct GimbalConfig { double yaw_k_v_{}, pitch_k_v_{}, accel_pitch_{}, accel_yaw_{}; - double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, chassis_comp_d_{}; + double chassis_comp_a_{}, chassis_comp_b_{}, chassis_comp_c_{}, + chassis_comp_d_{}; // sine wave compensation, a * sin(b * chassis_angular_z + c) + d + double moment_of_inertia_{}; }; class ChassisVel { public: - explicit ChassisVel(const ros::NodeHandle& nh) + ChassisVel(const ros::NodeHandle& nh) { double num_data; nh.param("num_data", num_data, 20.0); @@ -141,26 +143,46 @@ class Controller : public controller_interface::MultiInterfaceController& joint_urdfs); + virtual std::string getBaseFrameID(const std::unordered_map& joint_urdfs); + virtual void updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]); + virtual void updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double pos_real[3], + const double pos_des[3], const double pos_des_temp[3], const double vel_des[3], + const double angle_error[3], const double traject_pos_des[3], + const double traject_angle_error[3]); + void updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, control_toolbox::Pid& pid_pos, + double k_v, const double vel_des[3], const double angle_error[3], + const double traject_angle_error[3]); void rate(const ros::Time& time, const ros::Duration& period); - void track(const ros::Time& time); + void track(const ros::Time& time, const ros::Duration& period); + void externalAimTrack(const ros::Time& time); void direct(const ros::Time& time); void traj(const ros::Time& time); + bool externalAimIsFresh(const ros::Time& time) const; bool setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, double current_angle); void moveJoint(const ros::Time& time, const ros::Duration& period); - void updateChassisVel(); - void updateBallisticSolution(const ros::Time& time); double gravityFeedForward(const ros::Time& time); + void updateChassisVel(); double updateCompensation(double chassis_vel_angular_z); + void publishShootBeforehand(const ros::Time& time, uint8_t cmd); void commandCB(const rm_msgs::GimbalCmdConstPtr& msg); void trackCB(const rm_msgs::TrackDataConstPtr& msg); - void ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg); + void mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg); void reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t); - std::string getGimbalFrameID(std::unordered_map joint_urdfs); - std::string getBaseFrameID(std::unordered_map joint_urdfs); + bool TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res); + void publishTargetIsArmor(); rm_control::RobotStateHandle robot_state_handle_; hardware_interface::ImuSensorHandle imu_sensor_handle_; @@ -169,34 +191,43 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs_; std::unordered_map pos_des_in_limit_; bool has_imu_ = true; - double ballistic_yaw_{}, ballistic_pitch_{}; std::shared_ptr bullet_solver_; - std::shared_ptr ballistic_solver_; + double gimbal_real_z_vel_{}; + enum class TrackSolverType + { + BULLET_SOLVER, + EXTERNAL_MPC_AIM + }; + TrackSolverType track_solver_type_{ TrackSolverType::BULLET_SOLVER }; + double external_aim_timeout_{ 0.1 }; + bool external_aim_active_{ false }; // ROS Interface ros::Time last_publish_time_{}; - ros::Time last_ballistic_publish_time_{}; + ros::Time last_track_time_{}; std::unordered_map>> pos_state_pub_; std::shared_ptr> error_pub_; - std::shared_ptr> ballistic_solution_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; - ros::Subscriber ballistic_solver_request_sub_; + ros::Subscriber mpc_gimbal_aim_sub_; + ros::ServiceServer TrackSolver_type_srv_; + ros::Publisher target_is_armor_pub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; - realtime_tools::RealtimeBuffer ballistic_track_rt_buffer_; + realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; + rm_msgs::MPCGimbalAimCmd gimbal_aim_; std::string gimbal_des_frame_id_{}, imu_name_{}, gimbal_traject_des_frame_id_; double publish_rate_{}; bool state_changed_{}; int loop_count_{}; // Transform - geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, base2gimbal_, - odom2gimbal_traject_des_; + geometry_msgs::TransformStamped odom2gimbal_des_, odom2gimbal_, odom2base_, last_odom2base_, odom2gimbal_traject_des_; // Gravity Compensation geometry_msgs::Vector3 mass_origin_; @@ -221,9 +252,12 @@ class Controller : public controller_interface::MultiInterfaceController(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - base2target_data_sub_ = - controller_nh.subscribe("/base2target", 1, &BallisticSolver::base2targetDataCB, this); -} - -void BallisticSolver::solver(const geometry_msgs::TransformStamped& base2gimbal, double& yaw, double& pitch) -{ - BallisticConfig config = *config_rt_buffer_.readFromRT(); - std::lock_guard lock(data_mutex_); - if (!base2target_data_valid_) - { - pitch = 0.0, yaw = 0.0; - return; - } - geometry_msgs::Vector3 launch2target; - launch2target.x = base2target_.x - base2gimbal.transform.translation.x - config.gun_offset_x; - launch2target.y = base2target_.y - base2gimbal.transform.translation.y; - launch2target.z = base2target_.z - base2gimbal.transform.translation.z - config.gun_offset_z; - double target_dis = std::sqrt(launch2target.x * launch2target.x + launch2target.y * launch2target.y); - double target_hgt = launch2target.z; - // std::cout << "target_dis" << target_dis << std::endl << "target_hgt" << target_hgt << std::endl; - yaw = std::atan2(launch2target.y, launch2target.x); - double initial_pitch = -output_pitch_match_lut_.output(target_dis); - // std::cout << "initial_pitch: " << initial_pitch << std::endl; - auto error_function = [this, &config, &target_dis, &target_hgt](double pitch_angle) -> double { - return simulate(pitch_angle, config.initial_vel, target_dis, target_hgt); - }; - double current_pitch = initial_pitch; - double error = error_function(current_pitch); - for (int iter = 0; iter < config.max_newton_iterations; ++iter) - { - double error_after_step = error_function(current_pitch + config.newton_pitch_epsilon); - double error_derivative = (error_after_step - error) / config.newton_pitch_epsilon; - double pitch_adjustment = error / error_derivative; - pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? -config.max_newton_step : - (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : - pitch_adjustment; - double update_pitch = current_pitch - pitch_adjustment; - double update_error = error_function(update_pitch); - if (std::abs(update_error) < config.newton_convergence_tol) - { - pitch = -update_pitch; - return; - } - current_pitch = update_pitch; - error = update_error; - } - pitch = -initial_pitch; -} - -double BallisticSolver::simulate(double pitch_angle, double initial_vel, double target_dis, double target_hgt) -{ - BallisticConfig config = *config_rt_buffer_.readFromRT(); - double dt = config.rk4_simulate_step; - double x_previous = 0.0, z_previous = 0.0, x_current = 0.0, z_current = 0.0; - std::array state = { { 0.0, 0.0, initial_vel * std::cos(pitch_angle), - initial_vel * std::sin(pitch_angle) } }; - std::array k1{}, k2{}, k3{}, k4{}, temp{}; - auto systemEquation = [&config, &target_dis](const std::array& state, - std::array& stateDerivative) { - double vx = state[2], vz = state[3]; - double v = std::sqrt(vx * vx + vz * vz); - stateDerivative[0] = vx; - stateDerivative[1] = vz; - double fitting_Cd = config.Cd_value + config.Cd_slope * (target_dis - config.Cd_distance); - double F_drag = 0.5 * config.air_density * fitting_Cd * M_PI * config.radius * config.radius * v * v; - double drag_accel = F_drag / config.mass; - stateDerivative[2] = -drag_accel * vx / v; - stateDerivative[3] = -config.g - drag_accel * vz / v; - }; - while (state[0] < target_dis) - { - x_previous = x_current; - z_previous = z_current; - x_current = state[0]; - z_current = state[1]; - systemEquation(state, k1); - for (int i = 0; i < 4; ++i) - temp[i] = state[i] + 0.5 * dt * k1[i]; - systemEquation(temp, k2); - for (int i = 0; i < 4; ++i) - temp[i] = state[i] + 0.5 * dt * k2[i]; - systemEquation(temp, k3); - for (int i = 0; i < 4; ++i) - temp[i] = state[i] + dt * k3[i]; - systemEquation(temp, k4); - for (int i = 0; i < 4; ++i) - state[i] += dt / 6.0 * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]); - } - double z_at_target = z_previous + (z_current - z_previous) * (target_dis - x_previous) / (x_current - x_previous); - return z_at_target - target_hgt; -} - -void BallisticSolver::base2targetDataCB(const geometry_msgs::Point::ConstPtr& msg) -{ - std::lock_guard lock(data_mutex_); - base2target_ = *msg; - if (sqrt(msg->x * msg->x + msg->y * msg->y) <= 30) - base2target_data_valid_ = true; - else - base2target_data_valid_ = false; -} - -void BallisticSolver::reconfigCB(rm_gimbal_controllers::BallisticSolverConfig& config, uint32_t /*unused*/) -{ - ROS_INFO("[Ballistic Solver] Dynamic params changed"); - if (!dynamic_reconfig_initialized_) - { - BallisticConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init from YAML - config.mass = init_config.mass; - config.radius = init_config.radius; - config.Cd_value = init_config.Cd_value; - config.Cd_distance = init_config.Cd_distance; - config.Cd_slope = init_config.Cd_slope; - config.air_density = init_config.air_density; - config.g = init_config.g; - config.initial_vel = init_config.initial_vel; - config.rk4_simulate_step = init_config.rk4_simulate_step; - config.newton_convergence_tol = init_config.newton_convergence_tol; - config.newton_pitch_epsilon = init_config.newton_pitch_epsilon; - config.max_newton_step = init_config.max_newton_step; - config.max_newton_iterations = init_config.max_newton_iterations; - dynamic_reconfig_initialized_ = true; - } - BallisticConfig config_non_rt{ - .mass = config.mass, - .radius = config.radius, - .Cd_value = config.Cd_value, - .Cd_distance = config.Cd_distance, - .Cd_slope = config.Cd_slope, - .air_density = config.air_density, - .g = config.g, - .initial_vel = config.initial_vel, - .rk4_simulate_step = config.rk4_simulate_step, - .newton_convergence_tol = config.newton_convergence_tol, - .newton_pitch_epsilon = config.newton_pitch_epsilon, - .max_newton_step = config.max_newton_step, - .max_newton_iterations = config.max_newton_iterations, - }; - config_rt_buffer_.writeFromNonRT(config_non_rt); -} -} // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 06f330f3..a604342d 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -40,6 +40,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -51,22 +52,25 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .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.), + .resistance_coff_qd_800 = getParam(controller_nh, "resistance_coff_qd_800", 0.001), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), + .outpost_delay = getParam(controller_nh, "outpost_delay", 0.), .center_delay = getParam(controller_nh, "center_delay", 0.0), - .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), .switch_angle_offset = getParam(controller_nh, "switch_angle_offset", 0.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.1), - .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 3.0), + .outpost_switch_angle_offset = getParam(controller_nh, "outpost_switch_angle_offset", 0.0), + .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 2.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.), + .yaw_max_acc = getParam(controller_nh, "yaw_max_acc", 60.), + .track_rotate_outpost_delay = getParam(controller_nh, "track_rotate_outpost_delay", 0.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), - .max_selected_armor_ = getParam(controller_nh, "max_selecteed_armor", 3), + .traject_start_fit_ = getParam(controller_nh, "traject_start_fit_", 45), .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1), .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.), + .traject_k_effort = getParam(controller_nh, "traject_k_effort", 0.0), + .traject_k_vel_ = getParam(controller_nh, "traject_k_vel", 0.65), }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); @@ -96,8 +100,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - 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)); bullet_solver_pub.reset( new realtime_tools::RealtimePublisher(controller_nh, "bullet_solver_data", 10)); @@ -107,352 +109,385 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) &BulletSolver::heatCB, this); } -double BulletSolver::getResistanceCoefficient(double bullet_speed) const +double BulletSolver::getResistanceCoefficient(double target_distance) const { - // bullet_speed have 5 value:10,15,16,18,30 double resistance_coff; - if (bullet_speed < 12.5) - resistance_coff = config_.resistance_coff_qd_10; - else if (bullet_speed < 15.5) - resistance_coff = config_.resistance_coff_qd_15; - else if (bullet_speed < 17) - resistance_coff = config_.resistance_coff_qd_16; - else if (bullet_speed < 24) - resistance_coff = config_.resistance_coff_qd_18; - else - resistance_coff = config_.resistance_coff_qd_30; + // hero + resistance_coff = config_.resistance_coff_qd_800; return resistance_coff; } bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double bullet_speed, double yaw, - double v_yaw, double r1, double r2, double dz, int armors_num, double start_vel) + double v_yaw, double r1, double r2, double dz, double armors_num, double start_vel, + double track_id) { + while (yaw > M_PI / 2) + yaw -= M_PI; + while (yaw < -M_PI / 2) + yaw += M_PI; + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); 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; filtered_v_yaw_ = filtered_v_yaw_ + (v_yaw - filtered_v_yaw_) * 0.05; - double temp_z = pos.z; - double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); - double output_yaw_central = std::atan2(pos.y, pos.x); - output_pitch_ = std::atan2(temp_z, std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2))); - double rough_fly_time = - (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; - double r = r1; - double z = pos.z; - if (track_target_) - yaw += filtered_v_yaw_ * config_.track_rotate_target_delay; - pos.x += vel.x * config_.track_move_target_delay; - pos.y += vel.y * config_.track_move_target_delay; - if (track_target_) + if (track_count_ <= 300) { - if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) - track_target_ = false; + track_target_ = false; + track_count_++; } - else + if (track_count_ > 300) { - if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) + if (std::abs(filtered_v_yaw_) >= max_track_target_vel_ + switch_hysteresis_) + track_target_ = false; + else if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) track_target_ = true; - } - switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; - traject_switch_time_ = 1.321 * exp(-0.289 * abs(filtered_v_yaw_ + 1.0)) * config_.traject_ahead_; - // traject_switch_time_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_; - // yaw_subtract_ = filtered_yaw_ - output_yaw_central; - yaw_subtract_ = filtered_yaw_ - output_yaw_central; - while (yaw_subtract_ > M_PI) - yaw_subtract_ -= 2 * M_PI; - while (yaw_subtract_ < -M_PI) - yaw_subtract_ += 2 * M_PI; - double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; - if (((after_fly_yaw_subtract_ < switch_armor_angle - 0.3 && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ > switch_armor_angle + 0.3 && filtered_v_yaw_ < -1.)) && - track_target_) - { - selected_armor_ = 0; - } - else if (!track_target_) - { - selected_armor_ = 0; + if (sqrt(pow(pos.x, 2) + pow(pos.y, 2)) < 1.2) + track_target_ = false; } - if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 1.) || - (after_fly_yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -1.)) + if (identified_target_change_) { - count_++; - if (change_armor) + for (int i = 0; i < 150; i++) { - count_ = 0; - change_armor = false; - switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI / armors_num; + count_[i] = 0; + next_count_[i] = 0; } - if (count_ >= config_.min_fit_switch_count) + identified_target_change_ = false; + } + for (int i = 0; i < 150; i++) + { + double temp_z = pos.z; + if (track_target_) { - selected_armor_ = v_yaw > 0. ? -1 : 1; - r = armors_num == 4 ? r2 : r1; - z = pos.z + dz; - if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) - { - selected_armor_ = -2; - } - else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < switch_armor_angle) && v_yaw < 0.) + if (track_id == 6) { - selected_armor_ = 2; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_outpost_delay + i * 0.001); } - if (count_ == config_.min_fit_switch_count) + else { - switch_armor_time_ = ros::Time::now(); - traject_count_ = 0; + yaw_[i] = yaw + filtered_v_yaw_ * (config_.track_rotate_target_delay + i * 0.001); } } - } - if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 2. && - count_ > 2) - { - traject_count_++; - if (traject_count_ == 2) + else + yaw_[i] = yaw; + pos_x[i] = pos.x + vel.x * (config_.track_move_target_delay + i * 0.001); + pos_y[i] = pos.y + vel.y * (config_.track_move_target_delay + i * 0.001); + double target_rho = std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2)); + resistance_coff_ = getResistanceCoefficient(target_rho) != 0 ? getResistanceCoefficient(target_rho) : 0.001; + double output_yaw_central = std::atan2(pos_y[i], pos_x[i]); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(pos_x[i], 2) + std::pow(pos_y[i], 2))); + double rough_fly_time = + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + if (bullet_speed == 0.0) { - using_traject_ = true; - start_using_traject_time = ros::Time::now(); + ROS_ERROR("solve bullet speed is zero"); + return false; } - } - double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 1.57; - double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_central; - 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 + filtered_v_yaw_ * config_.delay) > switch_armor_angle) && filtered_v_yaw_ > 1.) || - (((aim_yaw_subtract + filtered_v_yaw_ * config_.delay) < -switch_armor_angle) && filtered_v_yaw_ < -1.)) && - track_target_; - int count{}; - double error = 999; - if (track_target_) - { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 1.57); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.57); - } - 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)); - if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) > 0.5) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.5)) + double r = r1; + double z = pos.z; + + switch_armor_angle = acos(r1 / target_rho) - config_.switch_angle_offset; + if (track_id == 6) { - selected_armor_ = filtered_v_yaw_ > 0. ? -1 : 1; + switch_armor_angle = acos(r1 / target_rho) - config_.outpost_switch_angle_offset; } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num) < -0.5))) + yaw_subtract_ = yaw_[i] - output_yaw_central; + // while (yaw_subtract_ > M_PI) + // yaw_subtract_ -= 2 * M_PI; + // while (yaw_subtract_ < -M_PI) + // yaw_subtract_ += 2 * M_PI; + double after_fly_yaw_subtract_ = yaw_subtract_ + v_yaw * rough_fly_time; + if (abs(v_yaw) < 2.) + selected_armor_[i] = 0; + else if ((after_fly_yaw_subtract_ < switch_armor_angle - 0.4 && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ > -switch_armor_angle + 0.4 && filtered_v_yaw_ < -2.)) + selected_armor_[i] = 0; + + if ((after_fly_yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 2.) || + (after_fly_yaw_subtract_ < -switch_armor_angle && filtered_v_yaw_ < -2.)) { - selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; + count_[i]++; + if (count_[i] >= config_.min_fit_switch_count) + { + selected_armor_[i] = v_yaw > 0. ? -1 : 1; + r = armors_num == 4 ? r2 : r1; + z = pos.z + dz; + if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) + { + selected_armor_[i] = -2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + else if ((after_fly_yaw_subtract_ + 2 * M_PI / armors_num < -switch_armor_angle) && v_yaw < 0.) + { + selected_armor_[i] = 2; + r = armors_num == 4 ? r1 : r2; + z = pos.z; + next_count_[i]++; + if (next_count_[i] == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_[i] = true; + } + } + if (count_[i] == config_.min_fit_switch_count) + { + if (is_aheading_two_[i]) + { + is_aheading_two_[i] = false; + } + else + { + switch_armor_time_ = ros::Time::now(); + } + } + } } - if (((filtered_v_yaw_ > 1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num) > 0.5) || - (filtered_v_yaw_ < -1.0 && - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num) < -0.5))) + int count{}; + double error = 999; + if (track_target_) { - selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; + target_pos_[i].x = pos_x[i] - r * cos(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] - r * sin(yaw_[i] + selected_armor_[i] * 2 * M_PI / armors_num); } - if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || - (selected_armor_ < 0 && selected_armor_ < -config_.max_selected_armor_)) + else { - selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; + target_pos_[i].x = pos_x[i] - r * cos(atan2(pos_y[i], pos_x[i])); + target_pos_[i].y = pos_y[i] - r * sin(atan2(pos_y[i], pos_x[i])); + selected_armor_[i] = 0; + if ((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay)) < -0.7)) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -1 : 1; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 1 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 1 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -2 : 2; + } + if (((filtered_v_yaw_ > 2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) - + 2 * 2 * M_PI / armors_num) > 0.7) || + (filtered_v_yaw_ < -2.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_[i] + config_.center_delay) + + 2 * 2 * M_PI / armors_num) < -0.7))) + { + selected_armor_[i] = filtered_v_yaw_ > 0. ? -3 : 3; + } + + if (selected_armor_[i] % 2 == 0) + { + r = r1; + z = pos.z; + } + else + { + r = r2; + z = pos.z + dz; + } } - if (selected_armor_ % 2 == 0) + target_pos_[i].z = z; + while (error >= 0.001) { - r = r1; - z = pos.z; + output_yaw_[i] = std::atan2(target_pos_[i].y, target_pos_[i].x); + output_pitch_[i] = std::atan2(temp_z, std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2))); + target_rho = std::sqrt(std::pow(target_pos_[i].x, 2) + std::pow(target_pos_[i].y, 2)); + fly_time_[i] = (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[i])))) / + resistance_coff_; + double real_z = (bullet_speed_ * std::sin(output_pitch_[i]) + (config_.g / resistance_coff_)) * + (1 - std::exp(-resistance_coff_ * fly_time_[i])) / resistance_coff_ - + config_.g * fly_time_[i] / resistance_coff_; + if (track_target_) + { + target_pos_[i].x = pos_x[i] + vel.x * fly_time_[i] - + r * cos(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + target_pos_[i].y = pos_y[i] + vel.y * fly_time_[i] - + r * sin(yaw_[i] + v_yaw * fly_time_[i] + selected_armor_[i] * 2 * M_PI / armors_num); + } + else + { + double target_pos_after_fly_time[2]; + target_pos_after_fly_time[0] = pos_x[i] + vel.x * fly_time_[i]; + target_pos_after_fly_time[1] = pos_y[i] + vel.y * fly_time_[i]; + target_pos_[i].x = + target_pos_after_fly_time[0] - r * cos(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + target_pos_[i].y = + target_pos_after_fly_time[1] - r * sin(atan2(target_pos_after_fly_time[1], target_pos_after_fly_time[0])); + } + target_pos_[i].z = z + vel.z * fly_time_[i]; + + double target_yaw = std::atan2(target_pos_[i].y, target_pos_[i].x); + double error_theta = target_yaw - output_yaw_[i]; + double error_z = target_pos_[i].z - real_z; + temp_z += error_z; + error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); + count++; + if (count >= 20 || std::isnan(error)) + return false; } } - target_pos_.z = z; - double r_trajcet_; - if (selected_armor_ == 1 || selected_armor_ == -1) + if (fly_time_pub_->trylock()) { - r_trajcet_ = r1; + fly_time_pub_->msg_.data = fly_time_[0]; + fly_time_pub_->unlockAndPublish(); + } + if (selected_armor_[0] == 1 || selected_armor_[0] == -1 || selected_armor_[0] == 3 || selected_armor_[0] == -3) + { + r_traject_ = r1; } else { - r_trajcet_ = r2; + r_traject_ = r2; } - - if (!using_traject_) + for (int i = 1; i <= config_.traject_start_fit_; i++) { - switchtime = 0.08; - - if (filtered_v_yaw_ > 0) - { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ - 1) * 2 * M_PI / armors_num); - } - else + if (selected_armor_[i] != selected_armor_[0] && using_traject_ == false && abs(v_yaw) > 4.5) { - after_traject_output_yaw_.x = - pos.x + vel.x * (fly_time_ + switchtime) - - r_trajcet_ * cos(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); - after_traject_output_yaw_.y = - pos.y + vel.y * (fly_time_ + switchtime) - - r_trajcet_ * sin(yaw + v_yaw * (fly_time_ + switchtime) + (selected_armor_ + 1) * 2 * M_PI / armors_num); + start_traject_ = true; + using_traject_ = true; + start_using_traject_time = ros::Time::now(); + break; } - stauts_limit_.start_pos = output_yaw_; - stauts_limit_.start_vel = start_vel; - stauts_limit_.end_pos = - std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; - stauts_limit_.end_vel = stauts_limit_.start_vel; + } - if (switchtime > 0.2) + if (start_traject_ && using_traject_) + { + traject_max_acc_ = 400.; + switchtime = 0.05; + while (abs(traject_max_acc_) > config_.yaw_max_acc) { - using_traject_ = false; - } - trajectory_function_coefficients.a0 = stauts_limit_.start_pos; - trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - - Eigen::Matrix2d A; - A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); - Eigen::Vector2d B; - B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), - stauts_limit_.end_vel - stauts_limit_.start_vel; + switchtime += 0.005; + if (filtered_v_yaw_ > 0) + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] - 1) * 2 * M_PI / armors_num); + } + else + { + after_traject_output_yaw_.x = pos_x[0] + vel.x * (fly_time_[0] + switchtime) - + r_traject_ * cos(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + after_traject_output_yaw_.y = pos_y[0] + vel.y * (fly_time_[0] + switchtime) - + r_traject_ * sin(yaw_[0] + v_yaw * (fly_time_[0] + switchtime) + + (selected_armor_[0] + 1) * 2 * M_PI / armors_num); + } + stauts_limit_.start_pos = output_yaw_[0]; + stauts_limit_.start_vel = start_vel; + stauts_limit_.end_pos = + std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x) + config_.end_pos_offset; + stauts_limit_.end_pos = + stauts_limit_.start_pos + angles::shortest_angular_distance(stauts_limit_.start_pos, stauts_limit_.end_pos); + stauts_limit_.end_vel = stauts_limit_.start_vel; - Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); + if (switchtime > 0.15) + { + break; + } + trajectory_function_coefficients.a0 = stauts_limit_.start_pos; + trajectory_function_coefficients.a1 = stauts_limit_.start_vel; - trajectory_function_coefficients.a2 = X(0); - trajectory_function_coefficients.a3 = X(1); - traject_max_acc_ = 2 * trajectory_function_coefficients.a2; - } + Eigen::Matrix2d A; + A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); + Eigen::Vector2d B; + B << stauts_limit_.end_pos - (stauts_limit_.start_pos + stauts_limit_.start_vel * switchtime), + stauts_limit_.end_vel - stauts_limit_.start_vel; - 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)); - 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_; + Eigen::Vector2d X = A.colPivHouseholderQr().solve(B); - 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); + trajectory_function_coefficients.a2 = X(0); + trajectory_function_coefficients.a3 = X(1); + traject_max_acc_ = abs(2 * trajectory_function_coefficients.a2); } - 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_; - double error_z = target_pos_.z - real_z; - temp_z += error_z; - error = std::sqrt(std::pow(error_theta * target_rho, 2) + std::pow(error_z, 2)); - count++; - - if (count >= 20 || std::isnan(error)) - return false; - } - if (fly_time_pub_->trylock()) - { - fly_time_pub_->msg_.data = fly_time_; - fly_time_pub_->unlockAndPublish(); + start_traject_ = false; } if (using_traject_) { ros::Time temp = ros::Time::now(); - traject_output_yaw_ = planningPoint(temp, start_using_traject_time); + traject_output_yaw_ = planningPoint(temp, start_using_traject_time, v_yaw); + while (traject_output_yaw_ > M_PI) + traject_output_yaw_ -= 2 * M_PI; + while (traject_output_yaw_ < -M_PI) + traject_output_yaw_ += 2 * M_PI; if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) { using_traject_ = false; } } - else + if (!using_traject_ || !track_target_) { - traject_output_yaw_ = output_yaw_; + traject_output_yaw_ = output_yaw_[0]; } + double vel_des = (traject_output_yaw_ - last_output_yaw_) / (ros::Time::now().toSec() - last_output_time_.toSec()); + filtered_vel_des = filtered_vel_des + (vel_des - filtered_vel_des) * (0.001 / (0.01 + 0.001)); + last_output_time_ = ros::Time::now(); + last_output_yaw_ = traject_output_yaw_; if (bullet_solver_pub->trylock()) { - bullet_solver_pub->msg_.selected_armor_ = selected_armor_; - bullet_solver_pub->msg_.yaw_subtract_ = yaw_subtract_; - bullet_solver_pub->msg_.output_yaw_central = output_yaw_central; - bullet_solver_pub->msg_.after_fly_yaw_subtract_ = after_fly_yaw_subtract_; - bullet_solver_pub->msg_.switch_armor_angle = switch_armor_angle; - bullet_solver_pub->msg_.test_number = traject_switch_time_; - bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; - bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; - bullet_solver_pub->msg_.test_number = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - if (v_yaw > 1.0) + for (int i = 0; i < 150; ++i) { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) - 3 * 2 * M_PI / armors_num); - } - else if (v_yaw < -1.0) - { - bullet_solver_pub->msg_.center_this_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 0 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 1 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_diagonal_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 2 * 2 * M_PI / armors_num); - bullet_solver_pub->msg_.center_triple_next_armor_ = - (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + 3 * 2 * M_PI / armors_num); + bullet_solver_pub->msg_.selected_armor_[i] = static_cast(selected_armor_[i]); + bullet_solver_pub->msg_.count_number_[i] = static_cast(count_[i]); } + bullet_solver_pub->msg_.start_vel = stauts_limit_.start_vel; + bullet_solver_pub->msg_.switchtime = switchtime; + bullet_solver_pub->msg_.traject_output_yaw_ = traject_output_yaw_; + bullet_solver_pub->msg_.traject_acc_ = traject_effort_ff_; + bullet_solver_pub->msg_.traject_vel_ = filtered_vel_des; + bullet_solver_pub->msg_.using_traject_ = using_traject_; + bullet_solver_pub->msg_.test_number[0] = yaw_[0]; bullet_solver_pub->unlockAndPublish(); } return true; } + void BulletSolver::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) + double v_yaw, double r1, double r2, double dz, double armors_num) { + if (armors_num == 3) + { + armors_num = 3.6; + } double r = r1, z = pos.z; - if (armors_num == 4 && selected_armor_ != 0 && armors_num == 3) + if (armors_num == 4 && selected_armor_[0] % 2 != 0) { 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_); + pos.x += vel.x * (config_.track_move_target_delay + fly_time_[0]); + pos.y += vel.y * (config_.track_move_target_delay + fly_time_[0]); if (track_target_) { - 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.x = pos.x - r * cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); + armor_pos.y = pos.y - r * sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 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) + - selected_armor_ * 2 * M_PI / armors_num); + sin(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 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); + cos(yaw + v_yaw * (fly_time_[0] + config_.track_rotate_target_delay) + + selected_armor_[0] * 2 * M_PI / armors_num); armor_vel.z = vel.z; } else @@ -470,18 +505,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi double roll{}, pitch{}, yaw{}; quatToRPY(odom2pitch.transform.rotation, roll, pitch, yaw); geometry_msgs::Point point_desire{}, point_real{}; - double target_rho = std::sqrt(std::pow(target_pos_.x, 2) + std::pow(target_pos_.y, 2)); + double target_rho = std::sqrt(std::pow(target_pos_[0].x, 2) + std::pow(target_pos_[0].y, 2)); int point_num = int(target_rho * 20); for (int i = 0; i <= point_num; i++) { double rt_bullet_rho = target_rho * i / point_num; - double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / + double fly_time = (-std::log(1 - rt_bullet_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_[0])))) / resistance_coff_; - double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * + double rt_bullet_z = (bullet_speed_ * std::sin(output_pitch_[0]) + (config_.g / resistance_coff_)) * (1 - std::exp(-resistance_coff_ * fly_time)) / resistance_coff_ - config_.g * fly_time / resistance_coff_; - point_desire.x = rt_bullet_rho * std::cos(output_yaw_) + odom2pitch.transform.translation.x; - point_desire.y = rt_bullet_rho * std::sin(output_yaw_) + odom2pitch.transform.translation.y; + point_desire.x = rt_bullet_rho * std::cos(output_yaw_[0]) + odom2pitch.transform.translation.x; + point_desire.y = rt_bullet_rho * std::sin(output_yaw_[0]) + odom2pitch.transform.translation.y; point_desire.z = rt_bullet_z + odom2pitch.transform.translation.z; marker_desire_.points.push_back(point_desire); } @@ -513,14 +548,18 @@ void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pi } double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, - double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, - double bullet_speed) + double r1, double r2, double dz, double armors_num, double yaw_real, + double pitch_real, double bullet_speed) { + if (armors_num == 3) + { + armors_num = 3.6; + } config_ = *config_rt_buffer_.readFromRT(); double delay; delay = track_target_ ? 0. : config_.center_delay; double r, z; - if (selected_armor_ % 2 == 0) + if (selected_armor_[0] % 2 == 0) { r = r1; z = pos.z; @@ -534,28 +573,28 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec if (track_target_) { double bullet_rho = - bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_; + bullet_speed * std::cos(pitch_real) * (1 - std::exp(-resistance_coff_ * fly_time_[0])) / 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)); + (1 - std::exp(-resistance_coff_ * fly_time_[0])) / resistance_coff_ - + config_.g * fly_time_[0] / resistance_coff_; + error = std::sqrt(std::pow(target_pos_[0].x - bullet_x, 2) + std::pow(target_pos_[0].y - bullet_y, 2) + + std::pow(target_pos_[0].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); + pos.x + vel.x * (fly_time_[0] + delay) - + r * cos(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 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)); + pos.y + vel.y * (fly_time_[0] + delay) - + r * sin(yaw + v_yaw * (fly_time_[0] + delay) + selected_armor_[0] * 2 * M_PI / armors_num); + target_pos_after_fly_time_and_delay.z = z + vel.z * (fly_time_[0] + delay); + error = std::sqrt(std::pow(target_pos_[0].x - target_pos_after_fly_time_and_delay.x, 2) + + std::pow(target_pos_[0].y - target_pos_after_fly_time_and_delay.y, 2) + + std::pow(target_pos_[0].z - target_pos_after_fly_time_and_delay.z, 2)); } return error; } @@ -566,47 +605,53 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) identified_target_change_ = true; else if (!msg->data && identified_target_change_) { - change_armor = true; identified_target_change_ = false; } } -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) +uint8_t BulletSolver::judgeShootBeforehand(double v_yaw, int id) { if (!track_target_) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel) { - if ((ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay)) && - (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime)) + int ban_shoot_delay_number = static_cast(55 + config_.delay * 1000); + if (id == 6) + { + ban_shoot_delay_number = static_cast(55 + config_.outpost_delay * 1000); + } + ban_shoot_delay_number = ban_shoot_delay_number > 149 ? 149 : ban_shoot_delay_number; + if (selected_armor_[ban_shoot_delay_number] != selected_armor_[0]) + { + ban_shoot_count_++; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + if (ban_shoot_count_ == 1) + { + ban_shoot_start_time_ = ros::Time::now(); + } + } + else if (using_traject_ && ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (ros::Time::now().toSec() > - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime && - ros::Time::now().toSec() < - (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) + switchtime + config_.delay) + } + else if (ros::Time::now().toSec() > ban_shoot_start_time_.toSec() + switchtime && + ros::Time::now().toSec() < ban_shoot_start_time_.toSec() + switchtime + config_.delay) + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + ban_shoot_count_ = 0; + } else + { shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + ban_shoot_count_ = 0; + } } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; - if (shoot_beforehand_cmd_pub_->trylock()) - { - shoot_beforehand_cmd_pub_->msg_.stamp = time; - shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_; - shoot_beforehand_cmd_pub_->unlockAndPublish(); - } + return static_cast(shoot_beforehand_cmd_); } -double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_) +double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_time_, double v_yaw) { double a0 = trajectory_function_coefficients.a0; double a1 = trajectory_function_coefficients.a1; @@ -614,8 +659,8 @@ double BulletSolver::planningPoint(ros::Time& time, ros::Time& start_trajectory_ double a3 = trajectory_function_coefficients.a3; double n_time_ = (time - start_trajectory_time_).toSec(); double plansetpoint = a0 + a1 * n_time_ + a2 * pow(n_time_, 2) + a3 * pow(n_time_, 3); - // traject_effort_ff_ = 1.4 - 1.4 * n_time_/switchtime; - traject_effort_ff_ = 0.0; + traject_effort_ff_ = (2 * a2 + 6 * a3 * n_time_) / (2 * a2) * 1.0 * config_.traject_k_effort; + traject_vel_ = (a1 + 2 * a2 * n_time_ + 3 * a3 * pow(n_time_, 2)) * config_.traject_k_vel_; return plansetpoint; } @@ -646,20 +691,22 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.resistance_coff_qd_30 = init_config.resistance_coff_qd_30; config.g = init_config.g; config.delay = init_config.delay; + config.outpost_delay = init_config.outpost_delay; config.center_delay = init_config.center_delay; - config.max_switch_angle = init_config.max_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.outpost_switch_angle_offset = init_config.outpost_switch_angle_offset; config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; + config.yaw_max_acc = init_config.yaw_max_acc; + config.track_rotate_outpost_delay = init_config.track_rotate_outpost_delay; config.min_fit_switch_count = init_config.min_fit_switch_count; - config.max_selected_armor_ = init_config.max_selected_armor_; + config.traject_start_fit_ = init_config.traject_start_fit_; config.traject_ahead_ = init_config.traject_ahead_; config.clean_shoot_num_ = init_config.clean_shoot_num_; config.end_pos_offset = init_config.end_pos_offset; + config.traject_k_effort = init_config.traject_k_effort; + config.traject_k_vel = init_config.traject_k_vel_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -667,22 +714,25 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_16 = config.resistance_coff_qd_16, .resistance_coff_qd_18 = config.resistance_coff_qd_18, .resistance_coff_qd_30 = config.resistance_coff_qd_30, + .resistance_coff_qd_800 = config.resistance_coff_qd_800, .g = config.g, .delay = config.delay, + .outpost_delay = config.outpost_delay, .center_delay = config.center_delay, - .max_switch_angle = config.max_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, + .outpost_switch_angle_offset = config.outpost_switch_angle_offset, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, + .yaw_max_acc = config.yaw_max_acc, + .track_rotate_outpost_delay = config.track_rotate_outpost_delay, .min_fit_switch_count = config.min_fit_switch_count, - .max_selected_armor_ = config.max_selected_armor_, + .traject_start_fit_ = config.traject_start_fit_, .traject_ahead_ = config.traject_ahead_, .clean_shoot_num_ = config.clean_shoot_num_, - .end_pos_offset = config.end_pos_offset }; + .end_pos_offset = config.end_pos_offset, + .traject_k_effort = config.traject_k_effort, + .traject_k_vel_ = config.traject_k_vel }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index e7c67730..0c5d5b75 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -36,6 +36,7 @@ // #include "rm_gimbal_controllers/gimbal_base.h" +#include #include #include #include @@ -48,27 +49,6 @@ namespace rm_gimbal_controllers { bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { - ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); - chassis_vel_ = std::make_shared(chassis_vel_nh); - ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); - bullet_solver_ = std::make_shared(nh_bullet_solver); - ros::NodeHandle nh_ballistic_solver = ros::NodeHandle(controller_nh, "ballistic_solver"); - ballistic_solver_ = std::make_shared(nh_ballistic_solver); - - config_ = { .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), - .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), - .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), - .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), - .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), - .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), - .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.) }; - config_rt_buffer_.initRT(config_); - d_srv_ = new dynamic_reconfigure::Server(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - XmlRpc::XmlRpcValue xml_rpc_value; const bool enable_feedforward = controller_nh.getParam("feedforward", xml_rpc_value); if (enable_feedforward) @@ -82,7 +62,57 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - auto* effort_joint_interface = robot_hw->get(); + ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); + chassis_vel_ = std::make_shared(chassis_vel_nh); + ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); + bullet_solver_ = std::make_shared(nh_bullet_solver); + std::string track_solver_type; + controller_nh.param("track_solver_type", track_solver_type, "bullet_solver"); + if (track_solver_type == "external_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_WARN("[Gimbal] track_solver_type=external_aim is deprecated, using external_mpc_aim instead"); + } + else if (track_solver_type == "external_mpc_aim") + { + track_solver_type_ = TrackSolverType::EXTERNAL_MPC_AIM; + ROS_INFO("[Gimbal] TRACK mode uses external MPC aim solver"); + } + else if (track_solver_type != "bullet_solver") + { + track_solver_type_ = TrackSolverType::BULLET_SOLVER; + ROS_WARN("[Gimbal] Unknown track_solver_type '%s', falling back to bullet_solver", track_solver_type.c_str()); + } + controller_nh.param("external_aim_timeout", external_aim_timeout_, 0.1); + std::string external_mpc_aim_topic; + controller_nh.param("external_mpc_aim_topic", external_mpc_aim_topic, "/sp_vision/mpc_gimbal_aim"); + + config_ = { + .yaw_k_v_ = getParam(controller_nh, "controllers/yaw/k_v", 0.), + .pitch_k_v_ = getParam(controller_nh, "controllers/pitch/k_v", 0.), + .accel_pitch_ = getParam(controller_nh, "controllers/pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "controllers/yaw/accel", 99.), + .chassis_comp_a_ = getParam(controller_nh, "controllers/yaw/chassis_comp_a", 0.), + .chassis_comp_b_ = getParam(controller_nh, "controllers/yaw/chassis_comp_b", 0.), + .chassis_comp_c_ = getParam(controller_nh, "controllers/yaw/chassis_comp_c", 0.), + .chassis_comp_d_ = getParam(controller_nh, "controllers/yaw/chassis_comp_d", 0.), + .moment_of_inertia_ = getParam(controller_nh, "moment_of_inertia", 0.), + }; + + config_rt_buffer_.initRT(config_); + d_srv_ = new dynamic_reconfigure::Server(controller_nh); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; + d_srv_->setCallback(cb); + std::string target_is_armor_topic; + controller_nh.param("target_is_armor_topic", target_is_armor_topic, "/vision/target_is_armor"); + controller_nh.param("initial_target_is_armor", target_is_armor_, true); + target_is_armor_pub_ = root_nh.advertise(target_is_armor_topic, 1, true); + publishTargetIsArmor(); + TrackSolver_type_srv_ = root_nh.advertiseService("/Processor/status_change", &Controller::TrackSolver_typeCB, this); + + hardware_interface::EffortJointInterface* effort_joint_interface = + robot_hw->get(); if (controller_nh.getParam("controllers", xml_rpc_value)) { // Get URDF info about joint @@ -103,12 +133,15 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro return false; } const int axis = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - joint_urdfs_.emplace(axis, std::move(joint_urdf)); - ctrls_.emplace(axis, std::make_unique()); - pid_pos_.emplace(axis, std::make_unique()); - pos_des_in_limit_.emplace(axis, true); - pos_state_pub_.emplace( - axis, std::make_unique>(nh, "pos_state", 1)); + if (!shouldInitializeController(it.first, joint_urdf, axis)) + continue; + joint_urdfs_.insert(std::make_pair(axis, joint_urdf)); + ctrls_.insert(std::make_pair(axis, std::make_unique())); + pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_des_in_limit_.insert(std::make_pair(axis, true)); + pos_state_pub_.insert(std::make_pair( + axis, std::make_unique>(nh, "pos_state", 1))); + if (!ctrls_.at(axis)->init(effort_joint_interface, nh) || !pid_pos_.at(axis)->init(nh_pid_pos)) return false; } @@ -119,8 +152,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro if (has_imu_) { imu_name_ = getParam(controller_nh, "imu_name", static_cast("gimbal_imu")); - auto* imu_sensor_interface = robot_hw->get(); + hardware_interface::ImuSensorInterface* imu_sensor_interface = + robot_hw->get(); imu_sensor_handle_ = imu_sensor_interface->getHandle(imu_name_); + ROS_WARN_ONCE("has imu"); } else { @@ -137,18 +172,26 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2base_.header.frame_id = "odom"; odom2base_.child_frame_id = getBaseFrameID(joint_urdfs_); odom2base_.transform.rotation.w = 1.; + odom2gimbal_traject_des_.header.frame_id = "odom"; odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_; odom2gimbal_traject_des_.transform.rotation.w = 1.; + rm_msgs::GimbalCmd default_cmd; + rm_msgs::TrackData default_track; + rm_msgs::MPCGimbalAimCmd default_aim; + cmd_rt_buffer_.initRT(default_cmd); + track_rt_buffer_.initRT(default_track); + gimbal_aim_rt_buffer_.initRT(default_aim); + cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); - ballistic_solver_request_sub_ = controller_nh.subscribe("/ballistic_solver_request", 1, - &Controller::ballisticSolverRequestCB, this); + data_track_sub_ = controller_nh.subscribe("/sp_vision/track", 1, &Controller::trackCB, this); + mpc_gimbal_aim_sub_ = + controller_nh.subscribe(external_mpc_aim_topic, 1, &Controller::mpcGimbalAimCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - ballistic_solution_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "ballistic_solution", 100)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); return true; } @@ -164,19 +207,18 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); + gimbal_aim_ = *gimbal_aim_rt_buffer_.readFromRT(); config_ = *config_rt_buffer_.readFromRT(); try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); odom2base_ = robot_state_handle_.lookupTransform("odom", odom2base_.child_frame_id, time); - base2gimbal_ = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, odom2gimbal_.child_frame_id, time); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(5, "%s\n", ex.what()); return; } - updateBallisticSolution(time); updateChassisVel(); if (state_ != cmd_gimbal_.mode) { @@ -189,7 +231,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) rate(time, period); break; case TRACK: - track(time); + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) + externalAimTrack(time); + else + track(time, period); break; case DIRECT: direct(time); @@ -201,24 +246,40 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } +bool Controller::shouldInitializeController(const std::string& /*name*/, + const urdf::JointConstSharedPtr& /*joint_urdf*/, int /*axis*/) const +{ + return true; +} + +bool Controller::shouldApplyJointLimit(int /*axis*/) const +{ + return true; +} + void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, double traject_yaw_des, bool update_yaw, bool update_pitch) { - tf2::Quaternion odom2base, odom2gimbal_des, base2gimbal_des, base2gimbal_traject_des; + tf2::Quaternion odom2base, odom2gimbal_des_q, base2gimbal_des, base2gimbal_traject_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); - tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des); + tf2::fromMsg(odom2gimbal_des_.transform.rotation, odom2gimbal_des_q); double current_rpy[3], des_rpy[3], traject_rpy[3]; - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), current_rpy[0], current_rpy[1], current_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), des_rpy[0], des_rpy[1], des_rpy[2]); - odom2gimbal_des.setRPY(0, pitch_des, traject_yaw_des); - quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), current_rpy[0], current_rpy[1], current_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), des_rpy[0], des_rpy[1], des_rpy[2]); + odom2gimbal_des_q.setRPY(0, pitch_des, traject_yaw_des); + quatToRPY(toMsg(odom2base.inverse() * odom2gimbal_des_q), traject_rpy[0], traject_rpy[1], traject_rpy[2]); + for (const auto& it : joint_urdfs_) { - bool update = (it.first != 1 && it.first != 2) || (it.first == 1 && update_pitch) || (it.first == 2 && update_yaw); - pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + bool update = (it.first == 1) ? update_pitch : ((it.first == 2) ? update_yaw : true); + if (shouldApplyJointLimit(it.first)) + pos_des_in_limit_[it.first] = setDesIntoLimit(des_rpy[it.first], it.second, update, current_rpy[it.first]); + else + pos_des_in_limit_[it.first] = true; } + base2gimbal_des.setRPY(des_rpy[0], des_rpy[1], des_rpy[2]); base2gimbal_traject_des.setRPY(traject_rpy[0], traject_rpy[1], traject_rpy[2]); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); @@ -229,6 +290,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des, void Controller::rate(const ros::Time& time, const ros::Duration& period) { + bullet_solver_->CleanTrackCount(); if (state_changed_) { // on enter state_changed_ = false; @@ -254,8 +316,16 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) } } -void Controller::track(const ros::Time& time) +void Controller::track(const ros::Time& time, const ros::Duration& period) { + external_aim_active_ = false; + if (data_track_.header.stamp.toSec() - time.toSec() > 0.5) + { + state_ = RATE; + state_changed_ = true; + rate(time, period); + return; + } if (state_changed_) { // on enter state_changed_ = false; @@ -273,7 +343,7 @@ void Controller::track(const ros::Time& time) { if (!data_track_.header.frame_id.empty()) { - auto transform = + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform("odom", data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); @@ -296,8 +366,8 @@ void Controller::track(const ros::Time& time) target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, - data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity()); - bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); + data_track_.armors_num, gimbal_real_z_vel_, data_track_.id); + publishShootBeforehand(time, bullet_solver_->judgeShootBeforehand(data_track_.v_yaw, data_track_.id)); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { @@ -324,6 +394,58 @@ void Controller::track(const ros::Time& time) } } +bool Controller::externalAimIsFresh(const ros::Time& time) const +{ + if (!gimbal_aim_.valid || gimbal_aim_.header.stamp.isZero()) + return false; + const double age = (time - gimbal_aim_.header.stamp).toSec(); + return age >= -external_aim_timeout_ && age <= external_aim_timeout_; +} + +void Controller::externalAimTrack(const ros::Time& time) +{ + if (state_changed_) + { + state_changed_ = false; + ROS_INFO("[Gimbal] Enter TRACK with external aim"); + } + + external_aim_active_ = externalAimIsFresh(time); + if (external_aim_active_) + { + setDes(time, gimbal_aim_.target_yaw, gimbal_aim_.pitch, gimbal_aim_.yaw); + publishShootBeforehand(time, gimbal_aim_.shoot_cmd == 1 ? rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT : + rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + else + { + odom2gimbal_des_.header.stamp = time; + robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); + publishShootBeforehand(time, rm_msgs::ShootBeforehandCmd::BAN_SHOOT); + } + + if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + { + if (error_pub_->trylock()) + { + error_pub_->msg_.stamp = time; + error_pub_->msg_.error = external_aim_active_ && std::isfinite(gimbal_aim_.error) ? gimbal_aim_.error : 1.0; + error_pub_->unlockAndPublish(); + } + last_publish_time_ = time; + } +} + +void Controller::publishShootBeforehand(const ros::Time& time, uint8_t cmd) +{ + if (shoot_beforehand_cmd_pub_ && shoot_beforehand_cmd_pub_->trylock()) + { + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = cmd; + shoot_beforehand_cmd_pub_->unlockAndPublish(); + } +} + void Controller::direct(const ros::Time& time) { if (state_changed_) @@ -335,11 +457,9 @@ void Controller::direct(const ros::Time& time) try { if (!cmd_gimbal_.target_pos.header.frame_id.empty()) - { - auto transform = robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, - cmd_gimbal_.target_pos.header.stamp); - tf2::doTransform(aim_point_odom, aim_point_odom, transform); - } + tf2::doTransform(aim_point_odom, aim_point_odom, + robot_state_handle_.lookupTransform("odom", cmd_gimbal_.target_pos.header.frame_id, + cmd_gimbal_.target_pos.header.stamp)); } catch (tf2::TransformException& ex) { @@ -419,9 +539,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) gyro.z = imu_sensor_handle_.getAngularVelocity()[2]; try { - auto transform = - robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), time); - tf2::doTransform(gyro, angular_vel, transform); + tf2::doTransform(gyro, angular_vel, + robot_state_handle_.lookupTransform(odom2gimbal_.child_frame_id, imu_sensor_handle_.getFrameId(), + time)); } catch (tf2::TransformException& ex) { @@ -438,6 +558,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) if (ctrls_.find(2) != ctrls_.end()) angular_vel.z = ctrls_.at(2)->joint_.getVelocity(); } + gimbal_real_z_vel_ = angular_vel.z; quatToRPY(odom2gimbal_des_.transform.rotation, pos_des[0], pos_des[1], pos_des[2]); quatToRPY(odom2gimbal_.transform.rotation, pos_real[0], pos_real[1], pos_real[2]); quatToRPY(odom2gimbal_traject_des_.transform.rotation, traject_pos_des[0], traject_pos_des[1], traject_pos_des[2]); @@ -453,79 +574,83 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } else if (state_ == TRACK) { - geometry_msgs::Point target_pos; - geometry_msgs::Vector3 target_vel; - if (data_track_.id != 12) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) { - geometry_msgs::Point pos = data_track_.position; - double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); - pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); - pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); - pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); - bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, - data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, - data_track_.dz, data_track_.armors_num); + if (external_aim_active_ && externalAimIsFresh(time)) + { + vel_des[2] = gimbal_aim_.yaw_rate; + vel_des[1] = gimbal_aim_.pitch_rate; + } + else + { + vel_des[2] = 0.; + vel_des[1] = 0.; + } } else { - target_pos = data_track_.position; - target_vel = data_track_.velocity; - } - target_vel.x -= chassis_vel_->linear_->x(); - target_vel.y -= chassis_vel_->linear_->y(); - target_vel.z -= chassis_vel_->linear_->z(); - tf2::Vector3 target_pos_tf, target_vel_tf; - try - { - if (joint_urdfs_.find(2) != joint_urdfs_.end()) + geometry_msgs::Point target_pos; + geometry_msgs::Vector3 target_vel; + if (data_track_.id != 12) { - auto transform = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, data_track_.header.frame_id, - data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - 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); + geometry_msgs::Point pos = data_track_.position; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + pos.x += data_track_.velocity.x * (time - data_track_.header.stamp).toSec(); + pos.y += data_track_.velocity.y * (time - data_track_.header.stamp).toSec(); + pos.z += data_track_.velocity.z * (time - data_track_.header.stamp).toSec(); + bullet_solver_->getSelectedArmorPosAndVel(target_pos, target_vel, pos, data_track_.velocity, yaw, + data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, + data_track_.dz, data_track_.armors_num); } - if (joint_urdfs_.find(1) != joint_urdfs_.end()) + else { - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->parent_link_name, - data_track_.header.frame_id, data_track_.header.stamp); - tf2::doTransform(target_pos, target_pos, transform); - tf2::doTransform(target_vel, target_vel, transform); - tf2::fromMsg(target_pos, target_pos_tf); - tf2::fromMsg(target_vel, target_vel_tf); - vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + target_pos = data_track_.position; + target_vel = data_track_.velocity; + } + target_vel.x -= chassis_vel_->linear_->x(); + target_vel.y -= chassis_vel_->linear_->y(); + target_vel.z -= chassis_vel_->linear_->z(); + tf2::Vector3 target_pos_tf, target_vel_tf; + try + { + if (joint_urdfs_.find(2) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + odom2base_.child_frame_id, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + 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); + } + if (bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) + { + vel_des[2] = bullet_solver_->getTrajectVel(); + } + if (joint_urdfs_.find(1) != joint_urdfs_.end()) + { + geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( + joint_urdfs_.at(1)->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + tf2::doTransform(target_pos, target_pos, transform); + tf2::doTransform(target_vel, target_vel, transform); + tf2::fromMsg(target_pos, target_pos_tf); + tf2::fromMsg(target_vel, target_vel_tf); + vel_des[1] = target_pos_tf.cross(target_vel_tf).y() / std::pow((target_pos_tf.length()), 2); + } + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); } - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); } } + for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; - - if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) - { - pid_pos_.at(1)->computeCommand(angle_error[1], period); - ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + - ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); - ctrls_.at(1)->update(time, period); - ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); - } - if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) - { - pid_pos_.at(2)->computeCommand(bullet_solver_->getUsingtraject() ? traject_angle_error[2] : angle_error[2], period); - double cmd = pid_pos_.at(2)->getCurrentCmd() - - updateCompensation(chassis_vel_->angular_->z()) * chassis_vel_->angular_->z() + - config_.yaw_k_v_ * vel_des[2] + ctrls_.at(2)->joint_.getVelocity() - angular_vel.z; - if (state_ == TRACK && bullet_solver_->getUsingtraject() && bullet_solver_->getTrackTarget()) - cmd += bullet_solver_->getTrajectEffortff(); - ctrls_.at(2)->setCommand(cmd); - ctrls_.at(2)->update(time, period); - } + updatePitchJoint(time, period, angular_vel, vel_des, angle_error); + updateYawJoint(time, period, angular_vel, pos_real, pos_des, pos_des_temp, vel_des, angle_error, traject_pos_des, + traject_angle_error); // publish state if (loop_count_ % 10 == 0) @@ -561,9 +686,9 @@ double Controller::gravityFeedForward(const ros::Time& time) if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); - auto transform = robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, - joint_urdfs_.at(1)->parent_link_name, time); - tf2::doTransform(gravity_compensation, gravity_compensation, transform); + tf2::doTransform(gravity_compensation, gravity_compensation, + robot_state_handle_.lookupTransform(joint_urdfs_.at(1)->child_link_name, + joint_urdfs_.at(1)->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -591,42 +716,97 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } -void Controller::updateBallisticSolution(const ros::Time& time) +void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double vel_des[3], + const double angle_error[3]) +{ + if (pid_pos_.find(1) != pid_pos_.end() && ctrls_.find(1) != ctrls_.end()) + { + pid_pos_.at(1)->computeCommand(angle_error[1], period); + ctrls_.at(1)->setCommand(pid_pos_.at(1)->getCurrentCmd() + config_.pitch_k_v_ * vel_des[1] + + ctrls_.at(1)->joint_.getVelocity() - angular_vel.y); + ctrls_.at(1)->update(time, period); + ctrls_.at(1)->joint_.setCommand(ctrls_.at(1)->joint_.getCommand() + gravityFeedForward(time)); + } +} + +void Controller::updateYawJoint(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, const double /*pos_real*/[3], + const double /*pos_des*/[3], const double /*pos_des_temp*/[3], const double vel_des[3], + const double angle_error[3], const double /*traject_pos_des*/[3], + const double traject_angle_error[3]) { - if (publish_rate_ > 0.0 && last_ballistic_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + if (pid_pos_.find(2) == pid_pos_.end() || ctrls_.find(2) == ctrls_.end()) + return; + + updateGimbalYawController(time, period, angular_vel, *ctrls_.at(2), *pid_pos_.at(2), config_.yaw_k_v_, vel_des, + angle_error, traject_angle_error); +} + +void Controller::updateGimbalYawController(const ros::Time& time, const ros::Duration& period, + const geometry_msgs::Vector3& angular_vel, + effort_controllers::JointVelocityController& ctrl, + control_toolbox::Pid& pid_pos, double k_v, const double vel_des[3], + const double angle_error[3], const double traject_angle_error[3]) +{ + if (state_ == TRACK) + { + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); + } + else + { + pid_pos_.at(2)->computeCommand(angle_error[2], period); + } + if (state_ == TRACK) { - if (ballistic_solution_pub_->trylock()) + if (track_solver_type_ == TrackSolverType::EXTERNAL_MPC_AIM && target_is_armor_) { - std_msgs::Float32MultiArray data; - data.data.emplace_back(ballistic_yaw_); - data.data.emplace_back(ballistic_pitch_); - ballistic_solution_pub_->msg_.data = data.data; - ballistic_solution_pub_->unlockAndPublish(); + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); + auto cmd = ctrls_.at(2)->joint_.getCommand(); + cmd = config_.moment_of_inertia_ * gimbal_aim_.yaw_acc + cmd; + ROS_WARN("cmd is %f, yaw_acc is %f", cmd, gimbal_aim_.yaw_acc); + ctrls_.at(2)->joint_.setCommand(cmd); + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); } - last_ballistic_publish_time_ = time; + } + else + { + ctrls_.at(2)->setCommand(pid_pos_.at(2)->getCurrentCmd() - + 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); } } -std::string Controller::getGimbalFrameID(std::unordered_map joint_urdfs) +std::string Controller::getGimbalFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->child_link_name; + return joint_urdfs.at(1)->child_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->child_link_name; + return joint_urdfs.at(0)->child_link_name.c_str(); if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->child_link_name; - return {}; + return joint_urdfs.at(2)->child_link_name.c_str(); + return std::string(); } -std::string Controller::getBaseFrameID(std::unordered_map joint_urdfs) +std::string Controller::getBaseFrameID(const std::unordered_map& joint_urdfs) { if (joint_urdfs.find(2) != joint_urdfs.end()) - return joint_urdfs.at(2)->parent_link_name; + return joint_urdfs.at(2)->parent_link_name.c_str(); if (joint_urdfs.find(0) != joint_urdfs.end()) - return joint_urdfs.at(0)->parent_link_name; + return joint_urdfs.at(0)->parent_link_name.c_str(); if (joint_urdfs.find(1) != joint_urdfs.end()) - return joint_urdfs.at(1)->parent_link_name; - return {}; + return joint_urdfs.at(1)->parent_link_name.c_str(); + return std::string(); } double Controller::updateCompensation(double chassis_vel_angular_z) @@ -649,12 +829,27 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } -void Controller::ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg) +void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) +{ + gimbal_aim_rt_buffer_.writeFromNonRT(*msg); +} + +bool Controller::TrackSolver_typeCB(rm_msgs::StatusChangeRequest& req, rm_msgs::StatusChangeResponse& res) +{ + const bool previous = target_is_armor_; + target_is_armor_ = req.target == rm_msgs::StatusChangeRequest::ARMOR; + publishTargetIsArmor(); + if (previous != target_is_armor_) + ROS_INFO("[Gimbal] armor vision pipeline %s", target_is_armor_ ? "enabled" : "disabled"); + res.switch_is_success = true; + return true; +} + +void Controller::publishTargetIsArmor() { - ballistic_track_rt_buffer_.writeFromNonRT(*msg); - bool ballistic_solver_request = msg->data; - if (ballistic_solver_request) - ballistic_solver_->solver(base2gimbal_, ballistic_yaw_, ballistic_pitch_); + std_msgs::Bool msg; + msg.data = target_is_armor_; + target_is_armor_pub_.publish(msg); } void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) @@ -665,12 +860,13 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; - config.accel_pitch_ = init_config.accel_pitch_; - config.accel_yaw_ = init_config.accel_yaw_; config.chassis_comp_a_ = init_config.chassis_comp_a_; config.chassis_comp_b_ = init_config.chassis_comp_b_; config.chassis_comp_c_ = init_config.chassis_comp_c_; config.chassis_comp_d_ = init_config.chassis_comp_d_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; + config.moment_of_inertia_ = init_config.moment_of_inertia_; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, @@ -680,7 +876,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin .chassis_comp_a_ = config.chassis_comp_a_, .chassis_comp_b_ = config.chassis_comp_b_, .chassis_comp_c_ = config.chassis_comp_c_, - .chassis_comp_d_ = config.chassis_comp_d_ }; + .chassis_comp_d_ = config.chassis_comp_d_, + .moment_of_inertia_ = config.moment_of_inertia_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From 416e1d571f4ddd2320eb8c95ccb3acc3df8caf6d Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Wed, 14 Oct 2026 21:42:35 +0800 Subject: [PATCH 26/31] Fix format. --- rm_chassis_controllers/src/chassis_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index f1c13625..42c3e5ca 100755 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -527,4 +527,4 @@ void ChassisBase::capacityCallback(const rm_msgs::PowerManagementSampleAnd } } -} // namespace rm_chassis_controllers \ No newline at end of file +} // namespace rm_chassis_controllers From 411d3603f2c71ec352601361903e7f50284dc0b4 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Wed, 14 Oct 2026 23:47:05 +0800 Subject: [PATCH 27/31] Recover file for ballistic solver. --- rm_gimbal_controllers/CMakeLists.txt | 4 +- rm_gimbal_controllers/cfg/BallisticSolver.cfg | 22 +++ .../rm_gimbal_controllers/ballistic_solver.h | 52 ++++++ .../src/ballistic_solver.cpp | 174 ++++++++++++++++++ 4 files changed, 250 insertions(+), 2 deletions(-) create mode 100644 rm_gimbal_controllers/cfg/BallisticSolver.cfg create mode 100644 rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h create mode 100644 rm_gimbal_controllers/src/ballistic_solver.cpp diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 50971382..9a3a0e1c 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -26,7 +26,7 @@ find_package(catkin REQUIRED ) generate_dynamic_reconfigure_options( - # cfg/BallisticSolver.cfg + cfg/BallisticSolver.cfg cfg/BulletSolver.cfg cfg/GimbalBase.cfg ) @@ -67,7 +67,7 @@ include_directories( ) ## Declare a cpp library -add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp) +add_library(${PROJECT_NAME} src/gimbal_base.cpp src/dual_yaw_controller.cpp src/bullet_solver.cpp src/ballistic_solver.cpp) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_gimbal_controllers/cfg/BallisticSolver.cfg b/rm_gimbal_controllers/cfg/BallisticSolver.cfg new file mode 100644 index 00000000..4b5f07ae --- /dev/null +++ b/rm_gimbal_controllers/cfg/BallisticSolver.cfg @@ -0,0 +1,22 @@ +#!/usr/bin/env python +PACKAGE = "rm_gimbal_controllers" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("mass", double_t, 0, "Bullet mass (kg)", 0.0445, 0.01, 0.1) +gen.add("radius", double_t, 0, "Bullet radius (m)", 0.02125, 0.01, 0.05) +gen.add("Cd_value", double_t, 0, "Fitting air drag coefficient", 0.4, 0.0, 1.0) +gen.add("Cd_distance", double_t, 0, "Distance of fitting air drag coefficient (m)", 12.0, 0.0, 30.0) +gen.add("Cd_slope", double_t, 0, "Fitting air drag coefficient change rate", 0.0, -10.0, 10.0) +gen.add("air_density", double_t, 0, "Air density (kg/m³)", 1.2, 0.8, 1.5) +gen.add("g", double_t, 0, "Gravity (m/s²)", 9.81, 9.0, 10.0) +gen.add("initial_vel", double_t, 0, "Initial velocity (m/s)", 14.0, 10.0, 20.0) +gen.add("rk4_simulate_step", double_t, 0, "Simulate step (s)", 0.01, 0.001, 0.05) +gen.add("newton_convergence_tol", double_t, 0, "Newton convergence tolerance", 2e-5, 1e-6, 1e-4) +gen.add("newton_pitch_epsilon", double_t, 0, "Finite difference epsilon", 1e-4, 1e-6, 1e-3) +gen.add("max_newton_step", double_t, 0, "Max Newton step", 0.04, 0.01, 0.1) +gen.add("max_newton_iterations", int_t, 0, "Max Newton iterations", 0, 1, 5) + +exit(gen.generate(PACKAGE, "ballistic_solver", "BallisticSolver")) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h new file mode 100644 index 00000000..32718ff4 --- /dev/null +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/ballistic_solver.h @@ -0,0 +1,52 @@ +// +// Created by ljyi on 25-9-20. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rm_gimbal_controllers +{ +struct BallisticConfig +{ + double mass, radius, gun_offset_x, gun_offset_z, Cd_value, Cd_distance, Cd_slope, air_density, g; + double initial_vel, rk4_simulate_step; + double newton_convergence_tol, newton_pitch_epsilon, max_newton_step; + int max_newton_iterations; +}; + +class BallisticSolver +{ +public: + explicit BallisticSolver(ros::NodeHandle& nh); + ~BallisticSolver() = default; + double simulate(double pitch_angle_, double initial_vel_, double target_dis, double target_hgt); + void solver(const geometry_msgs::TransformStamped& base2gimbal, double& yaw, double& pitch); + void reconfigCB(rm_gimbal_controllers::BallisticSolverConfig& config, uint32_t); + +private: + void base2targetDataCB(const geometry_msgs::PointConstPtr& msg); + ros::Subscriber base2target_data_sub_; + dynamic_reconfigure::Server* d_srv_{}; + BallisticConfig config_{}; + realtime_tools::RealtimeBuffer config_rt_buffer_; + rm_common::LinearInterp output_pitch_match_lut_; + bool dynamic_reconfig_initialized_{}; + geometry_msgs::Point base2target_; + std::mutex data_mutex_; + std::atomic base2target_data_valid_{ false }; +}; +} // namespace rm_gimbal_controllers diff --git a/rm_gimbal_controllers/src/ballistic_solver.cpp b/rm_gimbal_controllers/src/ballistic_solver.cpp new file mode 100644 index 00000000..f9c8284d --- /dev/null +++ b/rm_gimbal_controllers/src/ballistic_solver.cpp @@ -0,0 +1,174 @@ +// +// Created by ljyi on 25-9-20. +// + +#include "rm_gimbal_controllers/ballistic_solver.h" + +namespace rm_gimbal_controllers +{ +BallisticSolver::BallisticSolver(ros::NodeHandle& controller_nh) +{ + config_ = { .mass = getParam(controller_nh, "mass", 0.0445), + .radius = getParam(controller_nh, "radius", 0.02125), + .gun_offset_x = getParam(controller_nh, "gun_offset_x", 0.2), + .gun_offset_z = getParam(controller_nh, "gun_offset_z", 0.0), + .Cd_value = getParam(controller_nh, "Cd_value", 0.4), + .Cd_distance = getParam(controller_nh, "Cd_distance", 12.0), + .Cd_slope = getParam(controller_nh, "Cd_slope", 0.0), + .air_density = getParam(controller_nh, "air_density", 1.1), + .g = getParam(controller_nh, "g", 9.81), + .initial_vel = getParam(controller_nh, "initial_vel", 16.5), + .rk4_simulate_step = getParam(controller_nh, "rk4_simulate_step", 0.01), + .newton_convergence_tol = getParam(controller_nh, "newton_convergence_tol", 2e-5), + .newton_pitch_epsilon = getParam(controller_nh, "newton_pitch_epsilon", 1e-4), + .max_newton_step = getParam(controller_nh, "max_newton_step", 0.04), + .max_newton_iterations = getParam(controller_nh, "max_newton_iterations", 5) }; + config_rt_buffer_.initRT(config_); + XmlRpc::XmlRpcValue lut_config; + if (controller_nh.getParam("output_pitch_match", lut_config)) + output_pitch_match_lut_.init(lut_config); + d_srv_ = new dynamic_reconfigure::Server(controller_nh); + dynamic_reconfigure::Server::CallbackType cb = + [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; + d_srv_->setCallback(cb); + base2target_data_sub_ = + controller_nh.subscribe("/base2target", 1, &BallisticSolver::base2targetDataCB, this); +} + +void BallisticSolver::solver(const geometry_msgs::TransformStamped& base2gimbal, double& yaw, double& pitch) +{ + BallisticConfig config = *config_rt_buffer_.readFromRT(); + std::lock_guard lock(data_mutex_); + if (!base2target_data_valid_) + { + pitch = 0.0, yaw = 0.0; + return; + } + geometry_msgs::Vector3 launch2target; + launch2target.x = base2target_.x - base2gimbal.transform.translation.x - config.gun_offset_x; + launch2target.y = base2target_.y - base2gimbal.transform.translation.y; + launch2target.z = base2target_.z - base2gimbal.transform.translation.z - config.gun_offset_z; + double target_dis = std::sqrt(launch2target.x * launch2target.x + launch2target.y * launch2target.y); + double target_hgt = launch2target.z; + // std::cout << "target_dis" << target_dis << std::endl << "target_hgt" << target_hgt << std::endl; + yaw = std::atan2(launch2target.y, launch2target.x); + double initial_pitch = -output_pitch_match_lut_.output(target_dis); + // std::cout << "initial_pitch: " << initial_pitch << std::endl; + auto error_function = [this, &config, &target_dis, &target_hgt](double pitch_angle) -> double { + return simulate(pitch_angle, config.initial_vel, target_dis, target_hgt); + }; + double current_pitch = initial_pitch; + double error = error_function(current_pitch); + for (int iter = 0; iter < config.max_newton_iterations; ++iter) + { + double error_after_step = error_function(current_pitch + config.newton_pitch_epsilon); + double error_derivative = (error_after_step - error) / config.newton_pitch_epsilon; + double pitch_adjustment = error / error_derivative; + pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? + -config.max_newton_step : + (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : pitch_adjustment; + double update_pitch = current_pitch - pitch_adjustment; + double update_error = error_function(update_pitch); + if (std::abs(update_error) < config.newton_convergence_tol) + { + pitch = -update_pitch; + return; + } + current_pitch = update_pitch; + error = update_error; + } + pitch = -initial_pitch; +} + +double BallisticSolver::simulate(double pitch_angle, double initial_vel, double target_dis, double target_hgt) +{ + BallisticConfig config = *config_rt_buffer_.readFromRT(); + double dt = config.rk4_simulate_step; + double x_previous = 0.0, z_previous = 0.0, x_current = 0.0, z_current = 0.0; + std::array state = { { 0.0, 0.0, initial_vel * std::cos(pitch_angle), + initial_vel * std::sin(pitch_angle) } }; + std::array k1{}, k2{}, k3{}, k4{}, temp{}; + auto systemEquation = [&config, &target_dis](const std::array& state, + std::array& stateDerivative) { + double vx = state[2], vz = state[3]; + double v = std::sqrt(vx * vx + vz * vz); + stateDerivative[0] = vx; + stateDerivative[1] = vz; + double fitting_Cd = config.Cd_value + config.Cd_slope * (target_dis - config.Cd_distance); + double F_drag = 0.5 * config.air_density * fitting_Cd * M_PI * config.radius * config.radius * v * v; + double drag_accel = F_drag / config.mass; + stateDerivative[2] = -drag_accel * vx / v; + stateDerivative[3] = -config.g - drag_accel * vz / v; + }; + while (state[0] < target_dis) + { + x_previous = x_current; + z_previous = z_current; + x_current = state[0]; + z_current = state[1]; + systemEquation(state, k1); + for (int i = 0; i < 4; ++i) + temp[i] = state[i] + 0.5 * dt * k1[i]; + systemEquation(temp, k2); + for (int i = 0; i < 4; ++i) + temp[i] = state[i] + 0.5 * dt * k2[i]; + systemEquation(temp, k3); + for (int i = 0; i < 4; ++i) + temp[i] = state[i] + dt * k3[i]; + systemEquation(temp, k4); + for (int i = 0; i < 4; ++i) + state[i] += dt / 6.0 * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]); + } + double z_at_target = z_previous + (z_current - z_previous) * (target_dis - x_previous) / (x_current - x_previous); + return z_at_target - target_hgt; +} + +void BallisticSolver::base2targetDataCB(const geometry_msgs::Point::ConstPtr& msg) +{ + std::lock_guard lock(data_mutex_); + base2target_ = *msg; + if (sqrt(msg->x * msg->x + msg->y * msg->y) <= 30) + base2target_data_valid_ = true; + else + base2target_data_valid_ = false; +} + +void BallisticSolver::reconfigCB(rm_gimbal_controllers::BallisticSolverConfig& config, uint32_t /*unused*/) +{ + ROS_INFO("[Ballistic Solver] Dynamic params changed"); + if (!dynamic_reconfig_initialized_) + { + BallisticConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init from YAML + config.mass = init_config.mass; + config.radius = init_config.radius; + config.Cd_value = init_config.Cd_value; + config.Cd_distance = init_config.Cd_distance; + config.Cd_slope = init_config.Cd_slope; + config.air_density = init_config.air_density; + config.g = init_config.g; + config.initial_vel = init_config.initial_vel; + config.rk4_simulate_step = init_config.rk4_simulate_step; + config.newton_convergence_tol = init_config.newton_convergence_tol; + config.newton_pitch_epsilon = init_config.newton_pitch_epsilon; + config.max_newton_step = init_config.max_newton_step; + config.max_newton_iterations = init_config.max_newton_iterations; + dynamic_reconfig_initialized_ = true; + } + BallisticConfig config_non_rt{ + .mass = config.mass, + .radius = config.radius, + .Cd_value = config.Cd_value, + .Cd_distance = config.Cd_distance, + .Cd_slope = config.Cd_slope, + .air_density = config.air_density, + .g = config.g, + .initial_vel = config.initial_vel, + .rk4_simulate_step = config.rk4_simulate_step, + .newton_convergence_tol = config.newton_convergence_tol, + .newton_pitch_epsilon = config.newton_pitch_epsilon, + .max_newton_step = config.max_newton_step, + .max_newton_iterations = config.max_newton_iterations, + }; + config_rt_buffer_.writeFromNonRT(config_non_rt); +} +} // namespace rm_gimbal_controllers From 0108361fdc2eabac0e3356763bd45fef5c5c2fe4 Mon Sep 17 00:00:00 2001 From: chenqicheng <205455603@qq.com> Date: Wed, 14 Oct 2026 23:59:14 +0800 Subject: [PATCH 28/31] Fix format. --- rm_gimbal_controllers/src/ballistic_solver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/ballistic_solver.cpp b/rm_gimbal_controllers/src/ballistic_solver.cpp index f9c8284d..9e85b624 100644 --- a/rm_gimbal_controllers/src/ballistic_solver.cpp +++ b/rm_gimbal_controllers/src/ballistic_solver.cpp @@ -64,9 +64,9 @@ void BallisticSolver::solver(const geometry_msgs::TransformStamped& base2gimbal, double error_after_step = error_function(current_pitch + config.newton_pitch_epsilon); double error_derivative = (error_after_step - error) / config.newton_pitch_epsilon; double pitch_adjustment = error / error_derivative; - pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? - -config.max_newton_step : - (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : pitch_adjustment; + pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? -config.max_newton_step : + (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : + pitch_adjustment; double update_pitch = current_pitch - pitch_adjustment; double update_error = error_function(update_pitch); if (std::abs(update_error) < config.newton_convergence_tol) From d34193d763680d189f41d07080456c964f1820b9 Mon Sep 17 00:00:00 2001 From: chenqicheng <205455603@qq.com> Date: Thu, 15 Oct 2026 00:00:55 +0800 Subject: [PATCH 29/31] Fix format. --- rm_gimbal_controllers/src/ballistic_solver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/ballistic_solver.cpp b/rm_gimbal_controllers/src/ballistic_solver.cpp index 9e85b624..97131f3b 100644 --- a/rm_gimbal_controllers/src/ballistic_solver.cpp +++ b/rm_gimbal_controllers/src/ballistic_solver.cpp @@ -65,7 +65,7 @@ void BallisticSolver::solver(const geometry_msgs::TransformStamped& base2gimbal, double error_derivative = (error_after_step - error) / config.newton_pitch_epsilon; double pitch_adjustment = error / error_derivative; pitch_adjustment = (pitch_adjustment < -config.max_newton_step) ? -config.max_newton_step : - (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : + (pitch_adjustment > config.max_newton_step) ? config.max_newton_step : pitch_adjustment; double update_pitch = current_pitch - pitch_adjustment; double update_error = error_function(update_pitch); From 12fa933e25920cfc43b97171aab955bf171d8274 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Thu, 15 Oct 2026 17:32:45 +0800 Subject: [PATCH 30/31] Recover ballistic solver. --- .../rm_gimbal_controllers/gimbal_base.h | 15 +++++++- rm_gimbal_controllers/src/gimbal_base.cpp | 34 ++++++++++++++++++- 2 files changed, 47 insertions(+), 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 42b5ea17..dd78d003 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +61,9 @@ #include #include #include +#include +#include +#include namespace rm_gimbal_controllers { @@ -175,6 +179,8 @@ class Controller : public controller_interface::MultiInterfaceController joint_urdfs_; std::unordered_map pos_des_in_limit_; bool has_imu_ = true; + double ballistic_yaw_{}, ballistic_pitch_{}; std::shared_ptr bullet_solver_; + std::shared_ptr ballistic_solver_; double gimbal_real_z_vel_{}; enum class TrackSolverType { @@ -205,17 +213,21 @@ class Controller : public controller_interface::MultiInterfaceController>> pos_state_pub_; std::shared_ptr> error_pub_; + std::shared_ptr> ballistic_solution_pub_; std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; + ros::Subscriber ballistic_solver_request_sub_; ros::Subscriber mpc_gimbal_aim_sub_; ros::ServiceServer TrackSolver_type_srv_; ros::Publisher target_is_armor_pub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; + realtime_tools::RealtimeBuffer ballistic_track_rt_buffer_; realtime_tools::RealtimeBuffer gimbal_aim_rt_buffer_; rm_msgs::GimbalCmd cmd_gimbal_; @@ -227,7 +239,8 @@ class Controller : public controller_interface::MultiInterfaceController(chassis_vel_nh); ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); bullet_solver_ = std::make_shared(nh_bullet_solver); + ros::NodeHandle nh_ballistic_solver = ros::NodeHandle(controller_nh, "ballistic_solver"); + ballistic_solver_ = std::make_shared(nh_ballistic_solver); + std::string track_solver_type; controller_nh.param("track_solver_type", track_solver_type, "bullet_solver"); if (track_solver_type == "external_aim") @@ -172,7 +175,6 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro odom2base_.header.frame_id = "odom"; odom2base_.child_frame_id = getBaseFrameID(joint_urdfs_); odom2base_.transform.rotation.w = 1.; - odom2gimbal_traject_des_.header.frame_id = "odom"; odom2gimbal_traject_des_.child_frame_id = gimbal_traject_des_frame_id_; odom2gimbal_traject_des_.transform.rotation.w = 1.; @@ -186,12 +188,16 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); data_track_sub_ = controller_nh.subscribe("/sp_vision/track", 1, &Controller::trackCB, this); + ballistic_solver_request_sub_ = controller_nh.subscribe("/ballistic_solver_request", 1, + &Controller::ballisticSolverRequestCB, this); mpc_gimbal_aim_sub_ = controller_nh.subscribe(external_mpc_aim_topic, 1, &Controller::mpcGimbalAimCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); shoot_beforehand_cmd_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); + ballistic_solution_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "ballistic_solution", 100)); return true; } @@ -213,12 +219,14 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); odom2base_ = robot_state_handle_.lookupTransform("odom", odom2base_.child_frame_id, time); + base2gimbal_ = robot_state_handle_.lookupTransform(odom2base_.child_frame_id, odom2gimbal_.child_frame_id, time); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(5, "%s\n", ex.what()); return; } + updateBallisticSolution(time); updateChassisVel(); if (state_ != cmd_gimbal_.mode) { @@ -716,6 +724,21 @@ void Controller::updateChassisVel() last_odom2base_ = odom2base_; } +void Controller::updateBallisticSolution(const ros::Time& time) +{ + if (publish_rate_ > 0.0 && last_ballistic_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) + { + if (ballistic_solution_pub_->trylock()) + { + std_msgs::Float32MultiArray data; + data.data.emplace_back(ballistic_yaw_); + data.data.emplace_back(ballistic_pitch_); + ballistic_solution_pub_->msg_.data = data.data; + ballistic_solution_pub_->unlockAndPublish(); + } + last_ballistic_publish_time_ = time; + } +} void Controller::updatePitchJoint(const ros::Time& time, const ros::Duration& period, const geometry_msgs::Vector3& angular_vel, const double vel_des[3], const double angle_error[3]) @@ -829,6 +852,13 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } +void Controller::ballisticSolverRequestCB(const std_msgs::BoolConstPtr& msg) +{ + ballistic_track_rt_buffer_.writeFromNonRT(*msg); + bool ballistic_solver_request = msg->data; + if (ballistic_solver_request) + ballistic_solver_->solver(base2gimbal_, ballistic_yaw_, ballistic_pitch_); +} void Controller::mpcGimbalAimCB(const rm_msgs::MPCGimbalAimCmdConstPtr& msg) { gimbal_aim_rt_buffer_.writeFromNonRT(*msg); @@ -860,6 +890,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; config.chassis_comp_a_ = init_config.chassis_comp_a_; config.chassis_comp_b_ = init_config.chassis_comp_b_; config.chassis_comp_c_ = init_config.chassis_comp_c_; From 4b5cc1cfeae56b167c21d32ab8809a800609d3b6 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Thu, 15 Oct 2026 17:50:43 +0800 Subject: [PATCH 31/31] Fix gimbal base. --- .../include/rm_gimbal_controllers/gimbal_base.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 dd78d003..48a9fcfc 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -180,11 +180,11 @@ class Controller : public controller_interface::MultiInterfaceController