From b2486a512d6a6fd2a5edda4f26a27f6712973c44 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Thu, 13 Nov 2025 18:39:26 +0800 Subject: [PATCH 01/14] Finish the fuction of the trajcetplanner,add count of shoot num. --- rm_gimbal_controllers/CMakeLists.txt | 3 +- rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 + .../rm_gimbal_controllers/bullet_solver.h | 60 ++++- .../rm_gimbal_controllers/gimbal_base.h | 12 +- rm_gimbal_controllers/src/bullet_solver.cpp | 216 ++++++++++++++---- rm_gimbal_controllers/src/gimbal_base.cpp | 66 +++++- rm_shooter_controllers/src/standard.cpp | 13 +- 7 files changed, 293 insertions(+), 79 deletions(-) diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 995f5af7..fb4183bd 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -68,7 +68,8 @@ 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/bullet_solver.cpp + ) ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index c8de53c0..bf852d58 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -27,5 +27,7 @@ gen.add("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforeha gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0) 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("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) 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 e81ba1dc..b28be184 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -50,6 +50,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -60,15 +61,27 @@ struct Config min_switch_angle, switch_angle_offset, switch_duration_scale, switch_duration_rate, switch_duration_offset, min_shoot_beforehand_vel, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay; int min_fit_switch_count; + double traject_ahead_; + int clean_shoot_num_; +}; +struct TrajectoryFunctionCoefficients +{ + double a0,a1,a2,a3; +}; +struct TrajectoryLimitParams +{ + double start_pos,end_pos,start_vel,end_vel; }; class BulletSolver { public: + 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 chassis_angular_vel_z); + double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z, + ros::Time time,ros::Duration period,double start_pos,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; @@ -80,6 +93,22 @@ class BulletSolver { return -output_pitch_; } + double getTrajectYaw() const + { + return traject_output_yaw_; + } + bool getUsingtraject() const + { + return using_traject_; + } + int getShootnum() const + { + return shoot_num_; + } + double getTrajectEffortff() const + { + return traject_effort_ff_; + } 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, @@ -88,6 +117,8 @@ 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_); + void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg); ~BulletSolver() = default; private: @@ -96,27 +127,48 @@ class BulletSolver std::shared_ptr> shoot_beforehand_cmd_pub_; std::shared_ptr> fly_time_pub_; ros::Subscriber identified_target_change_sub_; + ros::Subscriber shoot_state_sub_; ros::Time switch_armor_time_{}; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; double max_track_target_vel_; - double output_yaw_{}, output_pitch_{}; + double output_yaw_{}, output_pitch_{},traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; double fly_time_; double switch_hysteresis_; - double last_yaw_{}, filtered_yaw_{}; + double last_yaw_{}, filtered_yaw_{} ; double gimbal_switch_duration_{}; + double yaw_subtract_; + double switch_armor_angle; + double filtered_v_yaw_{}; + double switchtime; + double traject_effort_ff_; + double traject_switch_time_; + + int shoot_num_ = 0; int shoot_beforehand_cmd_{}; - int selected_armor_; int count_; + int traject_count_; + int selected_armor_ = 0; + 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_ ; + geometry_msgs::Point after_traject_output_yaw_{}; geometry_msgs::Point target_pos_{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; + ros::Time start_using_traject_time; + + TrajectoryFunctionCoefficients trajectory_function_coefficients; + TrajectoryLimitParams stauts_limit_; + + 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 71b5b7c5..c6d3b118 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -137,7 +137,7 @@ class Controller : public controller_interface::MultiInterfaceController #include #include +#include + namespace rm_gimbal_controllers { @@ -61,12 +63,14 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .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.08), + .switch_duration_offset = getParam(controller_nh, "switch_duration_offset", 0.1), .min_shoot_beforehand_vel = getParam(controller_nh, "min_shoot_beforehand_vel", 4.5), .max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5), .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), + .traject_ahead_ = getParam(controller_nh, "traject_ahead_", 1.), + .clean_shoot_num_ = getParam(controller_nh, "clean_shoot_num_", 1), }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); @@ -101,6 +105,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); identified_target_change_sub_ = controller_nh.subscribe("/change", 10, &BulletSolver::identifiedTargetChangeCB, this); + shoot_state_sub_ = + controller_nh.subscribe("/local_heat_state/shooter_state", 50, &BulletSolver::heatCB, this); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -121,12 +127,12 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) 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 chassis_angular_vel_z) + double v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z, + ros::Time time,ros::Duration period,double start_pos,double start_vel) { 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) @@ -134,82 +140,96 @@ 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; double temp_z = pos.z; double target_rho = std::sqrt(std::pow(pos.x, 2) + std::pow(pos.y, 2)); - output_yaw_ = std::atan2(pos.y, pos.x); + 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_; - selected_armor_ = 0; double r = r1; double z = pos.z; - double min_switch_angle = config_.min_switch_angle / 180 * M_PI; + 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 (std::abs(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(v_yaw) <= max_track_target_vel_ - switch_hysteresis_) + if (std::abs(filtered_v_yaw_) <= max_track_target_vel_ - switch_hysteresis_) track_target_ = true; } - double switch_armor_angle = - track_target_ ? M_PI / armors_num - (2 * rough_fly_time + getGimbalSwitchDuration(abs(v_yaw))) / 2 * abs(v_yaw) + - config_.switch_angle_offset : - min_switch_angle; - double yaw_subtract = filtered_yaw_ - output_yaw_; - while (yaw_subtract > M_PI) - yaw_subtract -= 2 * M_PI; - while (yaw_subtract < -M_PI) - yaw_subtract += 2 * M_PI; - if (((yaw_subtract > switch_armor_angle) && v_yaw > 1.) || ((yaw_subtract < -switch_armor_angle) && v_yaw < -1.)) + double after_fly_time_yaw_ = yaw + rough_fly_time * v_yaw; + switch_armor_angle = acos(r1/target_rho) - 0.7; + traject_switch_time_ = 1.321 * exp(-0.289 * abs(filtered_v_yaw_)) * config_.traject_ahead_; + yaw_subtract_ = after_fly_time_yaw_ - output_yaw_central; + while (yaw_subtract_ > M_PI) + yaw_subtract_ -= 2 * M_PI; + while (yaw_subtract_ < -M_PI) + yaw_subtract_ += 2 * M_PI; + + if (( yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 4.)|| ( yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -4.)) { count_++; - if (identified_target_change_) + if (change_armor) { count_ = 0; - identified_target_change_ = false; + change_armor = false; } if (count_ >= config_.min_fit_switch_count) { - if (count_ == config_.min_fit_switch_count) - switch_armor_time_ = ros::Time::now(); selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; + if (count_ == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + traject_count_ = 0; + } } } - double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 2 * M_PI / armors_num; - double aim_yaw_subtract = filtered_aim_armor_yaw - output_yaw_; + else if (( yaw_subtract_ < switch_armor_angle - 0.3 && filtered_v_yaw_ > 4.)|| ( yaw_subtract_ > switch_armor_angle + 0.3 && filtered_v_yaw_ < -4.)) + { + selected_armor_ = 0; + } + if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 4) + { + traject_count_ ++; + if ( traject_count_ == 2) + { + using_traject_ = true; + start_using_traject_time = ros::Time::now(); + } + } + double filtered_aim_armor_yaw = filtered_yaw_ + selected_armor_ * 1.9; + 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 + v_yaw * config_.delay) > switch_armor_angle) && v_yaw > 1.) || - (((aim_yaw_subtract + v_yaw * config_.delay) < -switch_armor_angle) && v_yaw < -1.)) && + 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 (track_target_) - yaw += 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; int count{}; double error = 999; if (track_target_) { - target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 2 * M_PI / armors_num); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 2 * M_PI / armors_num); + target_pos_.x = pos.x - r * cos(yaw + selected_armor_ * 1.9); + target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.9); } 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 ((v_yaw > 1.0 && (yaw_subtract + v_yaw * (fly_time_ + config_.wait_next_armor_delay) + + if ((filtered_v_yaw_ > 1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.wait_next_armor_delay) + selected_armor_ * 2 * M_PI / armors_num) > 0.) || - (v_yaw < -1.0 && (yaw_subtract + v_yaw * (fly_time_ + config_.wait_next_armor_delay) + + (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.wait_next_armor_delay) + selected_armor_ * 2 * M_PI / armors_num) < 0.)) - selected_armor_ = v_yaw > 0. ? -2 : 2; + selected_armor_ = filtered_v_yaw_ > 0. ? -2 : 2; if (selected_armor_ % 2 == 0) { r = r1; @@ -217,23 +237,73 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } } target_pos_.z = z; + + double r_trajcet_; + if (selected_armor_ == 1 || selected_armor_ == -1) + { + r_trajcet_ = r1; + } + else + { + r_trajcet_ = r2; + } + + if (!using_traject_) + { + switchtime = 0.09; + + 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 = start_vel; + stauts_limit_.end_pos = std::atan2(after_traject_output_yaw_.y, after_traject_output_yaw_.x); + 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; + + Eigen::Matrix2d A; + A<< std::pow(switchtime,2),std::pow(switchtime,3), + 2*switchtime,3*std::pow(switchtime,2); + Eigen::Vector2d B; + B<= 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_; + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_ ; double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; + (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - + config_.g * fly_time_ / resistance_coff_; 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); + 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); + pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); } else { @@ -241,9 +311,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d 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_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_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_; @@ -262,9 +332,21 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d fly_time_pub_->msg_.data = fly_time_; fly_time_pub_->unlockAndPublish(); } + if (using_traject_) + { + ros::Time temp = ros::Time::now(); + traject_output_yaw_ = planningPoint(temp,start_using_traject_time); + if ( ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) + { + using_traject_ = false; + } + } + else + { + traject_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) @@ -403,6 +485,11 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) { if (msg->data) identified_target_change_ = true; + else if (!msg->data && identified_target_change_) + { + change_armor = true; + identified_target_change_ = false; + } } double BulletSolver::getGimbalSwitchDuration(double v_yaw) @@ -419,12 +506,11 @@ 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() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw)) - config_.delay) + if ((ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ - config_.delay) && abs(filtered_v_yaw_) > 4) && + (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ + switchtime - config_.delay))) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - if (((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw)))) + if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ + switchtime - config_.delay) && abs(filtered_v_yaw_) > 4) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; - if (is_in_delay_before_switch_) - shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; @@ -436,6 +522,34 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) } } +double BulletSolver::planningPoint(ros::Time& time,ros::Time& start_trajectory_time_) +{ + double a0=trajectory_function_coefficients.a0; + double a1=trajectory_function_coefficients.a1; + double a2=trajectory_function_coefficients.a2; + 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; + return plansetpoint; +} + +void BulletSolver::heatCB(const rm_msgs::LocalHeatStateConstPtr& msg) +{ + std::lock_guard lock(heat_mutex_); + if (msg->has_shoot && last_shoot_state_ != msg->has_shoot) + { + shoot_num_ += 1; + } + last_shoot_state_ = msg->has_shoot; + config_ = *config_rt_buffer_.readFromRT(); + ROS_WARN("clean_shoot_num_ is %d" , config_.clean_shoot_num_); + if (config_.clean_shoot_num_ == 0) + { + shoot_num_ = 0; + ROS_WARN("[Bullet Solver] clean_shoot_num_ is 0"); + } +} void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t /*unused*/) { ROS_INFO("[Bullet Solver] Dynamic params change"); @@ -464,6 +578,8 @@ 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.min_fit_switch_count = init_config.min_fit_switch_count; + config.traject_ahead_ = init_config.traject_ahead_; + config.clean_shoot_num_ = init_config.clean_shoot_num_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -487,7 +603,9 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .max_chassis_angular_vel = config.max_chassis_angular_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, - .min_fit_switch_count = config.min_fit_switch_count }; + .min_fit_switch_count = config.min_fit_switch_count, + .traject_ahead_ = config.traject_ahead_, + .clean_shoot_num_ = config.clean_shoot_num_}; 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 50ff79c4..b946cb82 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -147,6 +147,11 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro 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.; + + cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); @@ -167,6 +172,8 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) cmd_gimbal_ = *cmd_rt_buffer_.readFromRT(); data_track_ = *track_rt_buffer_.readFromNonRT(); config_ = *config_rt_buffer_.readFromRT(); + period_ = period; + time_ = time; try { odom2gimbal_ = robot_state_handle_.lookupTransform("odom", odom2gimbal_.child_frame_id, time); @@ -201,15 +208,18 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } -void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) +void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des ,double traject_yaw_des) { - tf2::Quaternion odom2base, odom2gimbal_des; + tf2::Quaternion odom2base, odom2gimbal_des ,odom2gimbal_traject_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); odom2gimbal_des.setRPY(0, pitch_des, yaw_des); + odom2gimbal_traject_des.setRPY(0, pitch_des, traject_yaw_des); tf2::Quaternion base2gimbal_des = odom2base.inverse() * odom2gimbal_des; + tf2::Quaternion base2gimbal_traject_des = odom2base.inverse() * odom2gimbal_traject_des; for (const auto& it : joint_urdfs_) pos_des_in_limit_[it.first] = setDesIntoLimit(base2gimbal_des, it.second, base2gimbal_des); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); + odom2gimbal_traject_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_traject_des); odom2gimbal_des_.header.stamp = time; robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers"); } @@ -232,7 +242,7 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) { double roll{}, pitch{}, yaw{}; quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); - setDes(time, yaw + period.toSec() * cmd_gimbal_.rate_yaw, pitch + period.toSec() * cmd_gimbal_.rate_pitch); + setDes(time, yaw + period.toSec() * cmd_gimbal_.rate_yaw, pitch + period.toSec() * cmd_gimbal_.rate_pitch,yaw + period.toSec() * cmd_gimbal_.rate_yaw); } } @@ -278,7 +288,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, chassis_vel_->angular_->z()); + data_track_.armors_num, chassis_vel_->angular_->z(),time_,period_, + pos_des[2],ctrls_.at(2)->joint_.getVelocity()); bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) @@ -298,7 +309,7 @@ void Controller::track(const ros::Time& time) } if (solve_success) - setDes(time, bullet_solver_->getYaw(), bullet_solver_->getPitch()); + setDes(time, bullet_solver_->getYaw(), bullet_solver_->getPitch(),bullet_solver_->getTrajectYaw()); else { odom2gimbal_des_.header.stamp = time; @@ -330,7 +341,7 @@ void Controller::direct(const ros::Time& time) double pitch = -std::atan2(aim_point_odom.z - odom2gimbal_.transform.translation.z, std::sqrt(std::pow(aim_point_odom.x - odom2gimbal_.transform.translation.x, 2) + std::pow(aim_point_odom.y - odom2gimbal_.transform.translation.y, 2))); - setDes(time, yaw, pitch); + setDes(time, yaw, pitch,yaw); } void Controller::traj(const ros::Time& time) @@ -361,7 +372,7 @@ void Controller::traj(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - setDes(time, traj[2], traj[1]); + setDes(time, traj[2], traj[1],traj[2]); } bool Controller::setDesIntoLimit(const tf2::Quaternion& base2gimbal_des, const urdf::JointConstSharedPtr& joint_urdf, @@ -420,11 +431,13 @@ 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(); } - double pos_real[3]{ 0. }, pos_des[3]{ 0. }, vel_des[3]{ 0. }, angle_error[3]{ 0. }; 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]); 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; @@ -483,6 +496,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ROS_WARN("%s", ex.what()); } } + for (const auto& in_limit : pos_des_in_limit_) if (!in_limit.second) vel_des[in_limit.first] = 0.; @@ -497,10 +511,36 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } if (pid_pos_.find(2) != pid_pos_.end() && ctrls_.find(2) != ctrls_.end()) { - pid_pos_.at(2)->computeCommand(angle_error[2], 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); + if (bullet_solver_->getUsingtraject()) + { + 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()) + { + 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()); + } + 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); } @@ -513,10 +553,12 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { pub.second->msg_.header.stamp = time; pub.second->msg_.set_point = pos_des[pub.first]; + pub.second->msg_.traject_set_point = traject_pos_des[pub.first]; pub.second->msg_.set_point_dot = vel_des[pub.first]; pub.second->msg_.process_value = pos_real[pub.first]; pub.second->msg_.error = angle_error[pub.first]; pub.second->msg_.command = pid_pos_[pub.first]->getCurrentCmd(); + pub.second->msg_.shoot_number = bullet_solver_->getShootnum(); pub.second->unlockAndPublish(); } } diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 07089712..a68f830c 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -297,15 +297,8 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { - // Used to distinguish the front and rear friction wheels. - if (j == 0) - ctrls_friction_[i][j]->setCommand( - wheel_speed_directions_[i][j] * - (cmd_.wheel_speed + config_.extra_wheel_speed + cmd_.wheels_speed_offset_back)); - if (j == 1) - ctrls_friction_[i][j]->setCommand( - wheel_speed_directions_[i][j] * - (cmd_.wheel_speed + config_.extra_wheel_speed + cmd_.wheels_speed_offset_front)); + ctrls_friction_[i][j]->setCommand(wheel_speed_directions_[i][j] * + (cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offsets_[i][j])); } } } @@ -341,7 +334,7 @@ void Controller::normalize() enter_ready_ = false; } else - ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() - 0.01) / push_angle)); + ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() - 0.08) / push_angle)); } void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period) From 79d2cfd12724cd221a14ddf51f70b97fb889c9b6 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 9 Dec 2025 17:04:08 +0800 Subject: [PATCH 02/14] Add the active_suspension controller . --- rm_chassis_controllers/CMakeLists.txt | 1 + .../active_suspension.h | 37 ++++++++++++ .../include/rm_chassis_controllers/omni.h | 5 +- .../rm_chassis_controllers_plugins.xml | 7 +++ .../src/active_suspension.cpp | 57 +++++++++++++++++++ 5 files changed, 106 insertions(+), 1 deletion(-) create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h create mode 100644 rm_chassis_controllers/src/active_suspension.cpp diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index ec9391d0..fe16a118 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -69,6 +69,7 @@ add_library(${PROJECT_NAME} src/sentry.cpp src/swerve.cpp src/balance.cpp + src/active_suspension.cpp ) ## Specify libraries to link executable targets against diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h new file mode 100644 index 00000000..20e2685a --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -0,0 +1,37 @@ +// +// Created by xuncheng on 2025/11/29. +// + + +#include "rm_chassis_controllers/chassis_base.h" +#include "rm_chassis_controllers/omni.h" +#include +#include + + +namespace rm_chassis_controllers +{ + class ActiveSuspensionController : public OmniController + { + public: + ActiveSuspensionController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + void ActiveSuspensionCallBack(const std_msgs::BoolConstPtr& msg); + + private: + void moveJoint(const ros::Time& time, const ros::Duration& period) override; + + std::vector> active_suspension_joints_; + std::vector active_suspension_joint_handles_{}; + + ros::Subscriber active_suspension_sub_; + + bool active_suspension_ = true; + + double suspension_pos_; + + }; +} + + + diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index e02c5d7f..852c03de 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -17,11 +17,14 @@ class OmniController : public ChassisBase> joints_; Eigen::MatrixXd chassis2joints_; + +protected: + void moveJoint(const ros::Time& time, const ros::Duration& period) override; + }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml index 6319ebc3..ab7e0fe3 100644 --- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml +++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml @@ -8,6 +8,13 @@ type of hardware interface. + + + The ActiveSuspensionController . + + diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp new file mode 100644 index 00000000..ccf90afb --- /dev/null +++ b/rm_chassis_controllers/src/active_suspension.cpp @@ -0,0 +1,57 @@ +// +// Created by qiayuan on 2022/7/29. +// + +#include +#include + +#include +#include + +#include "rm_chassis_controllers/active_suspension.h" + +namespace rm_chassis_controllers +{ + bool ActiveSuspensionController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) + { + OmniController::init(robot_hw, root_nh, controller_nh); + + active_suspension_sub_=controller_nh.subscribe("is_active_suspension",10,&ActiveSuspensionController::ActiveSuspensionCallBack,this ); + + XmlRpc::XmlRpcValue suspension_legs; + controller_nh.getParam("suspension_leg", suspension_legs); + + for (const auto& suspension_leg : suspension_legs) + { + ros::NodeHandle nh_suspension = ros::NodeHandle(controller_nh, "suspension_leg/" + suspension_leg.first); + active_suspension_joints_.push_back(std::make_shared()); + if (!active_suspension_joints_.back()->init(effort_joint_interface_, nh_suspension)) + { + ROS_ERROR("Failed to init active_suspension_joint"); + return false; + } + } + return true; + } + void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period) + { + OmniController::moveJoint(time, period); + suspension_pos_ = active_suspension_ ? 0.07 : 0.85; + for (auto& joint : active_suspension_joints_) + { + joint->setCommand(suspension_pos_); + joint->update(time, period); + auto cmd = joint->joint_.getCommand(); + cmd += 18.0; + joint->joint_.setCommand(cmd); + + } + + } + void ActiveSuspensionController::ActiveSuspensionCallBack(const std_msgs::BoolConstPtr& msg) + { + active_suspension_ = msg->data; + } + +} // namespace rm_chassis_controllers +PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::ActiveSuspensionController, controller_interface::ControllerBase) From e0036036737ddacddddd9ce2c14391816403f0ad Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 19 Dec 2025 16:49:32 +0800 Subject: [PATCH 03/14] Fix the fuction of the active_suspension controller,add the manual of it . --- .../active_suspension.h | 20 +++++++++---------- .../rm_chassis_controllers/chassis_base.h | 2 ++ .../src/active_suspension.cpp | 8 ++++---- rm_chassis_controllers/src/chassis_base.cpp | 8 ++++---- 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h index 20e2685a..402ad011 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -7,6 +7,7 @@ #include "rm_chassis_controllers/omni.h" #include #include +#include namespace rm_chassis_controllers @@ -14,21 +15,20 @@ namespace rm_chassis_controllers class ActiveSuspensionController : public OmniController { public: - ActiveSuspensionController() = default; - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; - void ActiveSuspensionCallBack(const std_msgs::BoolConstPtr& msg); + ActiveSuspensionController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + void ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg); private: - void moveJoint(const ros::Time& time, const ros::Duration& period) override; + void moveJoint(const ros::Time& time, const ros::Duration& period) override; - std::vector> active_suspension_joints_; - std::vector active_suspension_joint_handles_{}; + std::vector> active_suspension_joints_; + std::vector active_suspension_joint_handles_{}; - ros::Subscriber active_suspension_sub_; + ros::Subscriber active_suspension_sub_; + bool active_suspension_ = true; - bool active_suspension_ = true; - - double suspension_pos_; + double suspension_pos_; }; } diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index e979da0c..355bfbbc 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -47,6 +47,7 @@ #include #include #include +#include #include namespace rm_chassis_controllers @@ -55,6 +56,7 @@ struct Command { geometry_msgs::Twist cmd_vel_; rm_msgs::ChassisCmd cmd_chassis_; + rm_msgs::ChassisActiveSusCmd cmd_active_sus_; ros::Time stamp_; }; template diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp index ccf90afb..6903286a 100644 --- a/rm_chassis_controllers/src/active_suspension.cpp +++ b/rm_chassis_controllers/src/active_suspension.cpp @@ -16,7 +16,7 @@ namespace rm_chassis_controllers { OmniController::init(robot_hw, root_nh, controller_nh); - active_suspension_sub_=controller_nh.subscribe("is_active_suspension",10,&ActiveSuspensionController::ActiveSuspensionCallBack,this ); + active_suspension_sub_=controller_nh.subscribe("/cmd_active_suspension",10,&ActiveSuspensionController::ActiveSuspensionCallBack,this ); XmlRpc::XmlRpcValue suspension_legs; controller_nh.getParam("suspension_leg", suspension_legs); @@ -36,7 +36,7 @@ namespace rm_chassis_controllers void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period) { OmniController::moveJoint(time, period); - suspension_pos_ = active_suspension_ ? 0.07 : 0.85; + suspension_pos_ = active_suspension_ ? 0.0 : 0.85; //0.85 for (auto& joint : active_suspension_joints_) { joint->setCommand(suspension_pos_); @@ -48,9 +48,9 @@ namespace rm_chassis_controllers } } - void ActiveSuspensionController::ActiveSuspensionCallBack(const std_msgs::BoolConstPtr& msg) + void ActiveSuspensionController::ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg) { - active_suspension_ = msg->data; + active_suspension_ = msg.is_active_suspension; } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 627b4831..01c7eeec 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -142,13 +142,13 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio vel_cmd_.y = ramp_y_->output(); vel_cmd_.z = cmd_vel.angular.z; } - +//test if (cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_source_frame.empty()) follow_source_frame_ = "yaw"; else follow_source_frame_ = cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_source_frame; if (cmd_rt_buffer_.readFromRT()->cmd_chassis_.command_source_frame.empty()) - command_source_frame_ = "yaw"; + command_source_frame_ = "bask_link"; else command_source_frame_ = cmd_rt_buffer_.readFromRT()->cmd_chassis_.command_source_frame; @@ -166,10 +166,10 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio raw(); break; case FOLLOW: - follow(time, period); + raw(); break; case TWIST: - twist(time, period); + raw(); break; } From b19e75ed617b2fe4dd9b96960d490e021437bf4f Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Wed, 28 Jan 2026 10:08:02 +0800 Subject: [PATCH 04/14] Delet the dbug msg. --- rm_gimbal_controllers/src/bullet_solver.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 2a1448fe..8936f6f1 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -543,11 +543,9 @@ void BulletSolver::heatCB(const rm_msgs::LocalHeatStateConstPtr& msg) } last_shoot_state_ = msg->has_shoot; config_ = *config_rt_buffer_.readFromRT(); - ROS_WARN("clean_shoot_num_ is %d" , config_.clean_shoot_num_); if (config_.clean_shoot_num_ == 0) { shoot_num_ = 0; - ROS_WARN("[Bullet Solver] clean_shoot_num_ is 0"); } } void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t /*unused*/) From 0d49c5307f49fc556c97f9880ed0718877ca77b0 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 6 Feb 2026 20:10:44 +0800 Subject: [PATCH 05/14] Fix some bug. --- rm_gimbal_controllers/src/bullet_solver.cpp | 25 ++++++++++++++++----- 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 8936f6f1..7dda6628 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -130,6 +130,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z, ros::Time time,ros::Duration period,double start_pos,double start_vel) { + config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; @@ -164,7 +165,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d track_target_ = true; } double after_fly_time_yaw_ = yaw + rough_fly_time * v_yaw; - switch_armor_angle = acos(r1/target_rho) - 0.7; + switch_armor_angle = acos(r1/target_rho) - config_.switch_angle_offset; traject_switch_time_ = 1.321 * exp(-0.289 * abs(filtered_v_yaw_)) * config_.traject_ahead_; yaw_subtract_ = after_fly_time_yaw_ - output_yaw_central; while (yaw_subtract_ > M_PI) @@ -205,7 +206,7 @@ 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.9; + 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; @@ -218,8 +219,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.9); - target_pos_.y = pos.y - r * sin(yaw + selected_armor_ * 1.9); + 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 { @@ -507,11 +508,23 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time, double v_yaw) 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) && abs(filtered_v_yaw_) > 4) && - (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_ + switchtime - config_.delay))) + (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_+ switchtime - config_.delay))) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_ + switchtime - config_.delay) && abs(filtered_v_yaw_) > 4) + else if (ros::Time::now().toSec() > (start_using_traject_time.toSec() + switchtime - config_.delay) && abs(filtered_v_yaw_) > 4 && + (ros::Time::now().toSec() < (start_using_traject_time.toSec() + switchtime))) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + else + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; } + // else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel ) + // { + // if ((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw)) - config_.delay) + // shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + // if (((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw)))) + // shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + // if (is_in_delay_before_switch_) + // shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; + // } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; if (shoot_beforehand_cmd_pub_->trylock()) From f1dde61fad4e8433511d8b68a31f59f9116daa1d Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Tue, 9 Dec 2025 17:04:08 +0800 Subject: [PATCH 06/14] Add the active_suspension controller . --- rm_chassis_controllers/CMakeLists.txt | 1 + .../active_suspension.h | 37 ++++++++++++ .../include/rm_chassis_controllers/omni.h | 5 +- .../rm_chassis_controllers_plugins.xml | 7 +++ .../src/active_suspension.cpp | 57 +++++++++++++++++++ 5 files changed, 106 insertions(+), 1 deletion(-) create mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h create mode 100644 rm_chassis_controllers/src/active_suspension.cpp diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index ec9391d0..fe16a118 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -69,6 +69,7 @@ add_library(${PROJECT_NAME} src/sentry.cpp src/swerve.cpp src/balance.cpp + src/active_suspension.cpp ) ## Specify libraries to link executable targets against diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h new file mode 100644 index 00000000..20e2685a --- /dev/null +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -0,0 +1,37 @@ +// +// Created by xuncheng on 2025/11/29. +// + + +#include "rm_chassis_controllers/chassis_base.h" +#include "rm_chassis_controllers/omni.h" +#include +#include + + +namespace rm_chassis_controllers +{ + class ActiveSuspensionController : public OmniController + { + public: + ActiveSuspensionController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + void ActiveSuspensionCallBack(const std_msgs::BoolConstPtr& msg); + + private: + void moveJoint(const ros::Time& time, const ros::Duration& period) override; + + std::vector> active_suspension_joints_; + std::vector active_suspension_joint_handles_{}; + + ros::Subscriber active_suspension_sub_; + + bool active_suspension_ = true; + + double suspension_pos_; + + }; +} + + + diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index e02c5d7f..852c03de 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -17,11 +17,14 @@ class OmniController : public ChassisBase> joints_; Eigen::MatrixXd chassis2joints_; + +protected: + void moveJoint(const ros::Time& time, const ros::Duration& period) override; + }; } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml index 6319ebc3..ab7e0fe3 100644 --- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml +++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml @@ -8,6 +8,13 @@ type of hardware interface. + + + The ActiveSuspensionController . + + diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp new file mode 100644 index 00000000..ccf90afb --- /dev/null +++ b/rm_chassis_controllers/src/active_suspension.cpp @@ -0,0 +1,57 @@ +// +// Created by qiayuan on 2022/7/29. +// + +#include +#include + +#include +#include + +#include "rm_chassis_controllers/active_suspension.h" + +namespace rm_chassis_controllers +{ + bool ActiveSuspensionController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) + { + OmniController::init(robot_hw, root_nh, controller_nh); + + active_suspension_sub_=controller_nh.subscribe("is_active_suspension",10,&ActiveSuspensionController::ActiveSuspensionCallBack,this ); + + XmlRpc::XmlRpcValue suspension_legs; + controller_nh.getParam("suspension_leg", suspension_legs); + + for (const auto& suspension_leg : suspension_legs) + { + ros::NodeHandle nh_suspension = ros::NodeHandle(controller_nh, "suspension_leg/" + suspension_leg.first); + active_suspension_joints_.push_back(std::make_shared()); + if (!active_suspension_joints_.back()->init(effort_joint_interface_, nh_suspension)) + { + ROS_ERROR("Failed to init active_suspension_joint"); + return false; + } + } + return true; + } + void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period) + { + OmniController::moveJoint(time, period); + suspension_pos_ = active_suspension_ ? 0.07 : 0.85; + for (auto& joint : active_suspension_joints_) + { + joint->setCommand(suspension_pos_); + joint->update(time, period); + auto cmd = joint->joint_.getCommand(); + cmd += 18.0; + joint->joint_.setCommand(cmd); + + } + + } + void ActiveSuspensionController::ActiveSuspensionCallBack(const std_msgs::BoolConstPtr& msg) + { + active_suspension_ = msg->data; + } + +} // namespace rm_chassis_controllers +PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::ActiveSuspensionController, controller_interface::ControllerBase) From 5e606989508f583ceb371002d6060e244427221c Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Fri, 19 Dec 2025 16:49:32 +0800 Subject: [PATCH 07/14] Fix the fuction of the active_suspension controller,add the manual of it . --- .../active_suspension.h | 20 +++++++++---------- .../rm_chassis_controllers/chassis_base.h | 2 ++ .../src/active_suspension.cpp | 8 ++++---- rm_chassis_controllers/src/chassis_base.cpp | 8 ++++---- 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h index 20e2685a..402ad011 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -7,6 +7,7 @@ #include "rm_chassis_controllers/omni.h" #include #include +#include namespace rm_chassis_controllers @@ -14,21 +15,20 @@ namespace rm_chassis_controllers class ActiveSuspensionController : public OmniController { public: - ActiveSuspensionController() = default; - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; - void ActiveSuspensionCallBack(const std_msgs::BoolConstPtr& msg); + ActiveSuspensionController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + void ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg); private: - void moveJoint(const ros::Time& time, const ros::Duration& period) override; + void moveJoint(const ros::Time& time, const ros::Duration& period) override; - std::vector> active_suspension_joints_; - std::vector active_suspension_joint_handles_{}; + std::vector> active_suspension_joints_; + std::vector active_suspension_joint_handles_{}; - ros::Subscriber active_suspension_sub_; + ros::Subscriber active_suspension_sub_; + bool active_suspension_ = true; - bool active_suspension_ = true; - - double suspension_pos_; + double suspension_pos_; }; } diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index e979da0c..355bfbbc 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -47,6 +47,7 @@ #include #include #include +#include #include namespace rm_chassis_controllers @@ -55,6 +56,7 @@ struct Command { geometry_msgs::Twist cmd_vel_; rm_msgs::ChassisCmd cmd_chassis_; + rm_msgs::ChassisActiveSusCmd cmd_active_sus_; ros::Time stamp_; }; template diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp index ccf90afb..6903286a 100644 --- a/rm_chassis_controllers/src/active_suspension.cpp +++ b/rm_chassis_controllers/src/active_suspension.cpp @@ -16,7 +16,7 @@ namespace rm_chassis_controllers { OmniController::init(robot_hw, root_nh, controller_nh); - active_suspension_sub_=controller_nh.subscribe("is_active_suspension",10,&ActiveSuspensionController::ActiveSuspensionCallBack,this ); + active_suspension_sub_=controller_nh.subscribe("/cmd_active_suspension",10,&ActiveSuspensionController::ActiveSuspensionCallBack,this ); XmlRpc::XmlRpcValue suspension_legs; controller_nh.getParam("suspension_leg", suspension_legs); @@ -36,7 +36,7 @@ namespace rm_chassis_controllers void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period) { OmniController::moveJoint(time, period); - suspension_pos_ = active_suspension_ ? 0.07 : 0.85; + suspension_pos_ = active_suspension_ ? 0.0 : 0.85; //0.85 for (auto& joint : active_suspension_joints_) { joint->setCommand(suspension_pos_); @@ -48,9 +48,9 @@ namespace rm_chassis_controllers } } - void ActiveSuspensionController::ActiveSuspensionCallBack(const std_msgs::BoolConstPtr& msg) + void ActiveSuspensionController::ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg) { - active_suspension_ = msg->data; + active_suspension_ = msg.is_active_suspension; } } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 627b4831..01c7eeec 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -142,13 +142,13 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio vel_cmd_.y = ramp_y_->output(); vel_cmd_.z = cmd_vel.angular.z; } - +//test if (cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_source_frame.empty()) follow_source_frame_ = "yaw"; else follow_source_frame_ = cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_source_frame; if (cmd_rt_buffer_.readFromRT()->cmd_chassis_.command_source_frame.empty()) - command_source_frame_ = "yaw"; + command_source_frame_ = "bask_link"; else command_source_frame_ = cmd_rt_buffer_.readFromRT()->cmd_chassis_.command_source_frame; @@ -166,10 +166,10 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio raw(); break; case FOLLOW: - follow(time, period); + raw(); break; case TWIST: - twist(time, period); + raw(); break; } From 7149c8a1162fa4b7ae80464dda37df9a48e0c65d Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sat, 31 Jan 2026 18:04:53 +0800 Subject: [PATCH 08/14] Update the active suspension to hero. --- .../active_suspension.h | 36 +++++----- .../src/active_suspension.cpp | 69 +++++++++++-------- 2 files changed, 60 insertions(+), 45 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h index 402ad011..7923ce7e 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -12,26 +12,30 @@ namespace rm_chassis_controllers { - class ActiveSuspensionController : public OmniController - { - public: - ActiveSuspensionController() = default; - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; - void ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg); + class ActiveSuspensionController : public OmniController + { + public: + ActiveSuspensionController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + void ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg); - private: - void moveJoint(const ros::Time& time, const ros::Duration& period) override; + private: + void moveJoint(const ros::Time& time, const ros::Duration& period) override; - std::vector> active_suspension_joints_; - std::vector active_suspension_joint_handles_{}; + std::vector> active_suspension_joints_; + std::vector active_suspension_joint_handles_{}; - ros::Subscriber active_suspension_sub_; - bool active_suspension_ = true; + ros::Subscriber active_suspension_sub_; - double suspension_pos_; + double suspension_pos_; + + enum + { + DOWN, + MID, + UP + }; + int state_ = DOWN; }; } - - - diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp index 6903286a..d6ba4287 100644 --- a/rm_chassis_controllers/src/active_suspension.cpp +++ b/rm_chassis_controllers/src/active_suspension.cpp @@ -12,45 +12,56 @@ namespace rm_chassis_controllers { - bool ActiveSuspensionController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) - { - OmniController::init(robot_hw, root_nh, controller_nh); + bool ActiveSuspensionController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) + { + OmniController::init(robot_hw, root_nh, controller_nh); - active_suspension_sub_=controller_nh.subscribe("/cmd_active_suspension",10,&ActiveSuspensionController::ActiveSuspensionCallBack,this ); + active_suspension_sub_=controller_nh.subscribe("/cmd_active_suspension",10,&ActiveSuspensionController::ActiveSuspensionCallBack,this ); - XmlRpc::XmlRpcValue suspension_legs; - controller_nh.getParam("suspension_leg", suspension_legs); + XmlRpc::XmlRpcValue suspension_legs; + controller_nh.getParam("suspension_leg", suspension_legs); - for (const auto& suspension_leg : suspension_legs) - { - ros::NodeHandle nh_suspension = ros::NodeHandle(controller_nh, "suspension_leg/" + suspension_leg.first); - active_suspension_joints_.push_back(std::make_shared()); - if (!active_suspension_joints_.back()->init(effort_joint_interface_, nh_suspension)) - { - ROS_ERROR("Failed to init active_suspension_joint"); - return false; - } + for (const auto& suspension_leg : suspension_legs) + { + ros::NodeHandle nh_suspension = ros::NodeHandle(controller_nh, "suspension_leg/" + suspension_leg.first); + active_suspension_joints_.push_back(std::make_shared()); + if (!active_suspension_joints_.back()->init(effort_joint_interface_, nh_suspension)) + { + ROS_ERROR("Failed to init active_suspension_joint"); + return false; + } + } + return true; } - return true; - } void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period) { - OmniController::moveJoint(time, period); - suspension_pos_ = active_suspension_ ? 0.0 : 0.85; //0.85 - for (auto& joint : active_suspension_joints_) - { - joint->setCommand(suspension_pos_); - joint->update(time, period); - auto cmd = joint->joint_.getCommand(); - cmd += 18.0; - joint->joint_.setCommand(cmd); - - } + OmniController::moveJoint(time, period); + switch (state_) + { + case DOWN: + suspension_pos_ = 0.0; + break; + case MID: + suspension_pos_ = 0.4; + break; + case UP: + suspension_pos_ = 0.85; + break; + } + for (auto& joint : active_suspension_joints_) + { + joint->setCommand(suspension_pos_); + joint->update(time, period); + double pos = joint->getPosition(); + auto cmd = joint->joint_.getCommand(); + cmd += 25 - pos*25.88; + joint->joint_.setCommand(cmd); + } } void ActiveSuspensionController::ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg) { - active_suspension_ = msg.is_active_suspension; + state_ = msg.mode; } } // namespace rm_chassis_controllers From a5fcd837d80eea011f297fa319db40b2d7fe4064 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Thu, 5 Feb 2026 00:58:20 +0800 Subject: [PATCH 09/14] Update the active_suspension controllers. --- .../active_suspension.h | 19 +++++- .../src/active_suspension.cpp | 66 +++++++++++++++++-- 2 files changed, 76 insertions(+), 9 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h index 7923ce7e..3904314e 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -9,7 +9,6 @@ #include #include - namespace rm_chassis_controllers { class ActiveSuspensionController : public OmniController @@ -28,6 +27,19 @@ namespace rm_chassis_controllers ros::Subscriber active_suspension_sub_; double suspension_pos_; + double feed_forward_offset; + double feed_forward_effort; + double pos; + double stretch_coff_A_; + double stretch_coff_k_; + double shrink_coff_A_; + double shrink_coff_k_; + double feedforward_effect_time_; + double static_effort ; + int count_; + bool state_change_; + ros::Time state_change_time; + enum { @@ -35,7 +47,10 @@ namespace rm_chassis_controllers MID, UP }; - int state_ = DOWN; + + int state_ = DOWN; + int last_state_ = DOWN; + int check_state_ = DOWN; }; } diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp index d6ba4287..e0919af8 100644 --- a/rm_chassis_controllers/src/active_suspension.cpp +++ b/rm_chassis_controllers/src/active_suspension.cpp @@ -20,6 +20,12 @@ namespace rm_chassis_controllers XmlRpc::XmlRpcValue suspension_legs; controller_nh.getParam("suspension_leg", suspension_legs); + feed_forward_offset = getParam(controller_nh, "feed_forward_offset", 0.); + stretch_coff_A_ = getParam(controller_nh,"stretch_coff_A_",0.); + stretch_coff_k_ = getParam(controller_nh,"stretch_coff_k_",0.); + shrink_coff_A_ = getParam(controller_nh,"shrink_coff_A_",0.); + shrink_coff_k_ = getParam(controller_nh,"shrink_coff_k_",0.); + feedforward_effect_time_ = getParam(controller_nh,"feedforward_effect_time_",0.); for (const auto& suspension_leg : suspension_legs) { @@ -39,25 +45,71 @@ namespace rm_chassis_controllers switch (state_) { case DOWN: - suspension_pos_ = 0.0; + suspension_pos_ = -0.05; + static_effort = -6.0; break; case MID: - suspension_pos_ = 0.4; + suspension_pos_ = 0.3; + static_effort = 8.0; break; case UP: - suspension_pos_ = 0.85; + suspension_pos_ = 0.80; + static_effort = 0.0; break; - } + } + if (state_ != check_state_) + { + count_ =0; + last_state_ = check_state_; + } + if (last_state_ - state_ == -2 || ((last_state_ - state_ == - 1) && (state_ ==2))) + { + count_ ++; + if (count_ == 1) + { + state_change_time = ros::Time::now(); + } + feed_forward_effort = 2 * (stretch_coff_A_ + stretch_coff_k_ * pos); + } + else if ( last_state_ - state_ == -1 && state_ == 1) + { + count_ ++; + if (count_ == 1) + { + state_change_time = ros::Time::now(); + } + feed_forward_effort = stretch_coff_A_ - 45.0 * pos; + } + else if (last_state_ - state_ == 1 || last_state_ - state_ == 2) + { + count_ ++; + if (count_ == 1) + { + state_change_time = ros::Time::now(); + } + feed_forward_effort = shrink_coff_A_ + shrink_coff_k_ * pos; + } + else + { + feed_forward_effort = 0.0; + } + for (auto& joint : active_suspension_joints_) { joint->setCommand(suspension_pos_); joint->update(time, period); - double pos = joint->getPosition(); + pos = joint->getPosition(); auto cmd = joint->joint_.getCommand(); - cmd += 25 - pos*25.88; - joint->joint_.setCommand(cmd); + cmd += feed_forward_effort + feed_forward_offset + static_effort; + joint->joint_.setCommand(cmd); + } + if (ros::Time::now().toSec() - state_change_time.toSec() > feedforward_effect_time_) + { + last_state_ = state_; + count_ = 0; } + check_state_ = state_; } void ActiveSuspensionController::ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg) { From 5b2ef083aaf674e610693bbbcd1ea3a019665633 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Thu, 5 Feb 2026 10:10:18 +0800 Subject: [PATCH 10/14] Add some param. --- .../rm_chassis_controllers/active_suspension.h | 6 ++++++ rm_chassis_controllers/src/active_suspension.cpp | 14 +++++++++++--- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h index 3904314e..09ec2bfd 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -30,6 +30,12 @@ namespace rm_chassis_controllers double feed_forward_offset; double feed_forward_effort; double pos; + double suspension_pos_DOWN_; + double suspension_pos_MID_; + double suspension_pos_UP_; + double static_effort_DOWN_; + double static_effort_MID_; + double static_effort_UP_; double stretch_coff_A_; double stretch_coff_k_; double shrink_coff_A_; diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp index e0919af8..c3760038 100644 --- a/rm_chassis_controllers/src/active_suspension.cpp +++ b/rm_chassis_controllers/src/active_suspension.cpp @@ -25,6 +25,14 @@ namespace rm_chassis_controllers stretch_coff_k_ = getParam(controller_nh,"stretch_coff_k_",0.); shrink_coff_A_ = getParam(controller_nh,"shrink_coff_A_",0.); shrink_coff_k_ = getParam(controller_nh,"shrink_coff_k_",0.); + + + suspension_pos_DOWN_ = getParam(controller_nh,"suspension_pos_DOWN_",0.); + suspension_pos_MID_ = getParam(controller_nh,"suspension_pos_MID_",0.); + suspension_pos_UP_ = getParam(controller_nh,"suspension_pos_UP_",0.); + static_effort_DOWN_ = getParam(controller_nh,"static_effort_DOWN_",0.); + static_effort_MID_ = getParam(controller_nh,"static_effort_MID_",0.); + static_effort_UP_ = getParam(controller_nh,"static_effort_UP_",0.); feedforward_effect_time_ = getParam(controller_nh,"feedforward_effect_time_",0.); for (const auto& suspension_leg : suspension_legs) @@ -45,15 +53,15 @@ namespace rm_chassis_controllers switch (state_) { case DOWN: - suspension_pos_ = -0.05; + suspension_pos_ = suspension_pos_DOWN_; static_effort = -6.0; break; case MID: - suspension_pos_ = 0.3; + suspension_pos_ = suspension_pos_MID_; static_effort = 8.0; break; case UP: - suspension_pos_ = 0.80; + suspension_pos_ = suspension_pos_UP_; static_effort = 0.0; break; } From 9f4e86deceffeb2afb0a9e2d29856f53a2591ef1 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sat, 14 Mar 2026 22:20:44 +0800 Subject: [PATCH 11/14] Optimize bullet solver code, add bullet solver debugging information, fix the abnormal issue with the gimbal yaw command. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 13 +- .../rm_gimbal_controllers/bullet_solver.h | 25 ++- .../rm_gimbal_controllers/gimbal_base.h | 5 +- rm_gimbal_controllers/src/bullet_solver.cpp | 184 ++++++++++++------ rm_gimbal_controllers/src/gimbal_base.cpp | 89 +++++---- 5 files changed, 210 insertions(+), 106 deletions(-) mode change 100755 => 100644 rm_gimbal_controllers/cfg/BulletSolver.cfg diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg old mode 100755 new mode 100644 index bf852d58..cae9b458 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -12,22 +12,21 @@ gen.add("resistance_coff_qd_18", double_t, 0, "Air resistance divided by mass of 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("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("wait_next_armor_delay", double_t, 0, "Delay of shooting next target when in center mode", 0.105, 0.0, 0.5) -gen.add("wait_diagonal_armor_delay", double_t, 0, "Delay of shooting diagonal target when in center mode", 0.105, 0.0, 0.5) -gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) -gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) +gen.add("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("min_shoot_beforehand_vel", double_t, 0, "Min velocity to shoot beforehand", 0.0, 4.5, 20) -gen.add("max_chassis_angular_vel", double_t, 0, "Max chassis angular vel to switch target armor to center", 0.0, 0.0, 99.0) +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) 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) 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 b28be184..d5871fb9 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -57,12 +58,14 @@ namespace rm_gimbal_controllers struct Config { double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, wait_next_armor_delay, wait_diagonal_armor_delay, dt, timeout, max_switch_angle, - min_switch_angle, switch_angle_offset, switch_duration_scale, switch_duration_rate, switch_duration_offset, - min_shoot_beforehand_vel, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay; + 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 traject_ahead_; int clean_shoot_num_; + double end_pos_offset,move_switch_time_coff_; }; struct TrajectoryFunctionCoefficients { @@ -80,8 +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 chassis_angular_vel_z, - ros::Time time,ros::Duration period,double start_pos,double start_vel); + 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; @@ -109,6 +111,10 @@ class BulletSolver { return traject_effort_ff_; } + bool getTrackTarget() + { + 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, @@ -126,6 +132,7 @@ class BulletSolver 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_; ros::Subscriber shoot_state_sub_; ros::Time switch_armor_time_{}; @@ -145,12 +152,16 @@ class BulletSolver 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 next_count_; int traject_count_; + int ban_shoot_count_ = 0; int selected_armor_ = 0; + int last_selected_armor_ = 0; bool track_target_ = true; bool identified_target_change_ = true; @@ -159,12 +170,14 @@ class BulletSolver bool change_armor = false; bool using_traject_; bool last_shoot_state_ ; + bool is_aheading_two_; geometry_msgs::Point after_traject_output_yaw_{}; geometry_msgs::Point target_pos_{}; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; ros::Time start_using_traject_time; + ros::Time ban_shoot_time_; TrajectoryFunctionCoefficients trajectory_function_coefficients; TrajectoryLimitParams stauts_limit_; 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 c6d3b118..f43efb13 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -137,15 +137,14 @@ class Controller : public controller_interface::MultiInterfaceController(controller_nh, "shoot_beforehand_cmd", 10)); fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); + bullet_solver_pub.reset(new realtime_tools::RealtimePublisher(controller_nh,"bullet_solver_data",10)); identified_target_change_sub_ = controller_nh.subscribe("/change", 10, &BulletSolver::identifiedTargetChangeCB, this); shoot_state_sub_ = @@ -127,10 +126,8 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) 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 chassis_angular_vel_z, - ros::Time time,ros::Duration period,double start_pos,double start_vel) + double v_yaw, double r1, double r2, double dz, int armors_num,double start_vel) { - config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; resistance_coff_ = getResistanceCoefficient(bullet_speed_) != 0 ? getResistanceCoefficient(bullet_speed_) : 0.001; @@ -164,40 +161,76 @@ 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; } - double after_fly_time_yaw_ = yaw + rough_fly_time * v_yaw; switch_armor_angle = acos(r1/target_rho) - config_.switch_angle_offset; - traject_switch_time_ = 1.321 * exp(-0.289 * abs(filtered_v_yaw_)) * config_.traject_ahead_; - yaw_subtract_ = after_fly_time_yaw_ - output_yaw_central; + 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_ = (switch_armor_angle - config_.traject_ahead_ - switche_time_yaw_) / filtered_v_yaw_; + 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.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; + } - if (( yaw_subtract_ > switch_armor_angle && filtered_v_yaw_ > 4.)|| ( yaw_subtract_ < switch_armor_angle && filtered_v_yaw_ < -4.)) + 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; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; + if ((after_fly_yaw_subtract_ - 2 * M_PI /armors_num > switch_armor_angle ) && v_yaw > 0.) + { + selected_armor_ = -2; + next_count_ ++; + if (next_count_ == config_.min_fit_switch_count) + { + switch_armor_time_ = ros::Time::now(); + is_aheading_two_ = true; + } + } + else if ((after_fly_yaw_subtract_ + 2 * M_PI /armors_num < switch_armor_angle) && v_yaw < 0.) + { + selected_armor_ = 2; + 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) { - switch_armor_time_ = ros::Time::now(); + if (is_aheading_two_) + { + is_aheading_two_ = false; + } + else + { + switch_armor_time_ = ros::Time::now(); + } traject_count_ = 0; } } } - else if (( yaw_subtract_ < switch_armor_angle - 0.3 && filtered_v_yaw_ > 4.)|| ( yaw_subtract_ > switch_armor_angle + 0.3 && filtered_v_yaw_ < -4.)) - { - selected_armor_ = 0; - } - if (ros::Time::now().toSec() > (switch_armor_time_.toSec() + traject_switch_time_) && abs(filtered_v_yaw_) > 4) + 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) @@ -226,11 +259,29 @@ 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_.wait_next_armor_delay) + - selected_armor_ * 2 * M_PI / armors_num) > 0.) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.wait_next_armor_delay) + - selected_armor_ * 2 * M_PI / armors_num) < 0.)) + 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 ) ) + { + 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) || + (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + + 1 * 2 * M_PI / armors_num) < -0.5)) ) + { 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) || + (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + + 2 * 2 * M_PI / armors_num) < -0.5)) ) + { + selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; + } + if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || (selected_armor_ < 0 && selected_armor_ < - config_.max_selected_armor_)) + { + selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; + } if (selected_armor_ % 2 == 0) { r = r1; @@ -251,7 +302,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (!using_traject_) { - switchtime = 0.09; + switchtime = 0.08; if (filtered_v_yaw_ > 0) { @@ -265,7 +316,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } 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); + 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) @@ -286,6 +337,7 @@ 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) @@ -346,6 +398,41 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d { traject_output_yaw_ = 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 = switch_armor_time_.toSec() ; + bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; + bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; + 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->unlockAndPublish(); + } + last_selected_armor_ = selected_armor_; return true; } void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, @@ -436,11 +523,9 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec double r1, double r2, double dz, int armors_num, double yaw_real, double pitch_real, double bullet_speed) { + config_ = *config_rt_buffer_.readFromRT(); double delay; - if (track_target_) - delay = 0.; - else - delay = selected_armor_ % 2 == 0 ? config_.wait_diagonal_armor_delay : config_.wait_next_armor_delay; + delay = track_target_ ? 0. : config_.center_delay; double r, z; if (selected_armor_ % 2 == 0) { @@ -507,24 +592,15 @@ 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) && abs(filtered_v_yaw_) > 4) && - (ros::Time::now().toSec() < (switch_armor_time_.toSec() + traject_switch_time_+ switchtime - 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)) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (ros::Time::now().toSec() > (start_using_traject_time.toSec() + switchtime - config_.delay) && abs(filtered_v_yaw_) > 4 && - (ros::Time::now().toSec() < (start_using_traject_time.toSec() + switchtime))) + 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) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; } - // else if (std::abs(v_yaw) > config_.min_shoot_beforehand_vel ) - // { - // if ((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw)) - config_.delay) - // shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - // if (((ros::Time::now() - switch_armor_time_).toSec() < getGimbalSwitchDuration(std::abs(v_yaw)))) - // shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; - // if (is_in_delay_before_switch_) - // shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - // } else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; if (shoot_beforehand_cmd_pub_->trylock()) @@ -574,23 +650,21 @@ 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.wait_next_armor_delay = init_config.wait_next_armor_delay; - config.wait_diagonal_armor_delay = init_config.wait_diagonal_armor_delay; - config.dt = init_config.dt; - config.timeout = init_config.timeout; + config.center_delay = init_config.center_delay; config.max_switch_angle = init_config.max_switch_angle; - config.min_switch_angle = init_config.min_switch_angle; config.switch_angle_offset = init_config.switch_angle_offset; config.switch_duration_scale = init_config.switch_duration_scale; config.switch_duration_rate = init_config.switch_duration_rate; config.switch_duration_offset = init_config.switch_duration_offset; config.min_shoot_beforehand_vel = init_config.min_shoot_beforehand_vel; - config.max_chassis_angular_vel = init_config.max_chassis_angular_vel; config.track_rotate_target_delay = init_config.track_rotate_target_delay; config.track_move_target_delay = init_config.track_move_target_delay; 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_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -600,23 +674,21 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .resistance_coff_qd_30 = config.resistance_coff_qd_30, .g = config.g, .delay = config.delay, - .wait_next_armor_delay = config.wait_next_armor_delay, - .wait_diagonal_armor_delay = config.wait_diagonal_armor_delay, - .dt = config.dt, - .timeout = config.timeout, + .center_delay = config.center_delay, .max_switch_angle = config.max_switch_angle, - .min_switch_angle = config.min_switch_angle, .switch_angle_offset = config.switch_angle_offset, .switch_duration_scale = config.switch_duration_scale, .switch_duration_rate = config.switch_duration_rate, .switch_duration_offset = config.switch_duration_offset, .min_shoot_beforehand_vel = config.min_shoot_beforehand_vel, - .max_chassis_angular_vel = config.max_chassis_angular_vel, .track_rotate_target_delay = config.track_rotate_target_delay, .track_move_target_delay = config.track_move_target_delay, .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_}; + .clean_shoot_num_ = config.clean_shoot_num_, + .end_pos_offset = config.end_pos_offset, + .move_switch_time_coff_ = config.move_switch_time_coff_}; 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 b946cb82..30ab00e9 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -208,16 +208,28 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) moveJoint(time, period); } -void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des ,double traject_yaw_des) +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 ,odom2gimbal_traject_des; + tf2::Quaternion odom2base, odom2gimbal_des_q, base2gimbal_des, base2gimbal_traject_des; tf2::fromMsg(odom2base_.transform.rotation, odom2base); - odom2gimbal_des.setRPY(0, pitch_des, yaw_des); - odom2gimbal_traject_des.setRPY(0, pitch_des, traject_yaw_des); - tf2::Quaternion base2gimbal_des = odom2base.inverse() * odom2gimbal_des; - tf2::Quaternion base2gimbal_traject_des = odom2base.inverse() * odom2gimbal_traject_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_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_) - pos_des_in_limit_[it.first] = setDesIntoLimit(base2gimbal_des, it.second, base2gimbal_des); + { + 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]); + } + + 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); odom2gimbal_traject_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_traject_des); odom2gimbal_des_.header.stamp = time; @@ -242,7 +254,14 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period) { double roll{}, pitch{}, yaw{}; quatToRPY(odom2gimbal_des_.transform.rotation, roll, pitch, yaw); - setDes(time, yaw + period.toSec() * cmd_gimbal_.rate_yaw, pitch + period.toSec() * cmd_gimbal_.rate_pitch,yaw + period.toSec() * cmd_gimbal_.rate_yaw); + + 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); } } @@ -288,8 +307,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, chassis_vel_->angular_->z(),time_,period_, - pos_des[2],ctrls_.at(2)->joint_.getVelocity()); + data_track_.armors_num, ctrls_.at(2)->joint_.getVelocity()); bullet_solver_->judgeShootBeforehand(time, data_track_.v_yaw); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) @@ -341,7 +359,7 @@ void Controller::direct(const ros::Time& time) double pitch = -std::atan2(aim_point_odom.z - odom2gimbal_.transform.translation.z, std::sqrt(std::pow(aim_point_odom.x - odom2gimbal_.transform.translation.x, 2) + std::pow(aim_point_odom.y - odom2gimbal_.transform.translation.y, 2))); - setDes(time, yaw, pitch,yaw); + setDes(time, yaw, pitch, yaw); } void Controller::traj(const ros::Time& time) @@ -372,32 +390,34 @@ void Controller::traj(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - setDes(time, traj[2], traj[1],traj[2]); + setDes(time, traj[2], traj[1], traj[2]); } -bool Controller::setDesIntoLimit(const tf2::Quaternion& base2gimbal_des, const urdf::JointConstSharedPtr& joint_urdf, - tf2::Quaternion& base2new_des) +bool Controller::setDesIntoLimit(double& angle, const urdf::JointConstSharedPtr& joint_urdf, bool update, + double current_angle) { - double base2gimbal_current_des[3]; - quatToRPY(toMsg(base2gimbal_des), base2gimbal_current_des[0], base2gimbal_current_des[1], base2gimbal_current_des[2]); + // 如果不需要更新该轴，保持当前值不变 + 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; - int index = (joint_urdf->axis.x == 1) * 0 + (joint_urdf->axis.y == 1) * 1 + (joint_urdf->axis.z == 1) * 2; - if ((base2gimbal_current_des[index] <= upper_limit && base2gimbal_current_des[index] >= lower_limit) || - (angles::two_pi_complement(base2gimbal_current_des[index]) <= upper_limit && - angles::two_pi_complement(base2gimbal_current_des[index]) >= lower_limit)) + + if ((angle <= upper_limit && angle >= lower_limit) || + (angles::two_pi_complement(angle) <= upper_limit && angles::two_pi_complement(angle) >= lower_limit)) { - base2new_des = base2gimbal_des; return true; } else { - base2gimbal_current_des[index] = - std::abs(angles::shortest_angular_distance(base2gimbal_current_des[index], upper_limit)) < - std::abs(angles::shortest_angular_distance(base2gimbal_current_des[index], lower_limit)) ? - upper_limit : - lower_limit; - base2new_des.setRPY(base2gimbal_current_des[0], base2gimbal_current_des[1], base2gimbal_current_des[2]); + // 超限时，钳位到最近的限位值 + angle = std::abs(angles::shortest_angular_distance(angle, upper_limit)) < + std::abs(angles::shortest_angular_distance(angle, lower_limit)) ? + upper_limit : + lower_limit; return false; } } @@ -433,7 +453,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } 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]); + 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++) @@ -513,7 +533,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { if (bullet_solver_->getUsingtraject()) { - pid_pos_.at(2)->computeCommand(traject_angle_error[2] , period); + pid_pos_.at(2)->computeCommand(traject_angle_error[2], period); } else { @@ -521,17 +541,18 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } if (state_ == TRACK) { - if (bullet_solver_ -> getUsingtraject()) + if (bullet_solver_->getUsingtraject()) { 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()); + 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()); } 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); + 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 From 505cca8e9285a450cd4c68316addbd58042125a4 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 15 Mar 2026 10:14:11 +0800 Subject: [PATCH 12/14] Update the active suspension code. --- .../active_suspension.h | 61 +++++------- .../src/active_suspension.cpp | 94 +++++-------------- 2 files changed, 52 insertions(+), 103 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h index 09ec2bfd..bc63707c 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -2,15 +2,25 @@ // Created by xuncheng on 2025/11/29. // - #include "rm_chassis_controllers/chassis_base.h" #include "rm_chassis_controllers/omni.h" -#include #include #include +#include +#include +#include +#include +#include namespace rm_chassis_controllers { + enum class State + { + DOWN = 0, + MID = 1, + UP = 2 + }; + class ActiveSuspensionController : public OmniController { public: @@ -25,38 +35,19 @@ namespace rm_chassis_controllers std::vector active_suspension_joint_handles_{}; ros::Subscriber active_suspension_sub_; - - double suspension_pos_; - double feed_forward_offset; - double feed_forward_effort; - double pos; - double suspension_pos_DOWN_; - double suspension_pos_MID_; - double suspension_pos_UP_; - double static_effort_DOWN_; - double static_effort_MID_; - double static_effort_UP_; - double stretch_coff_A_; - double stretch_coff_k_; - double shrink_coff_A_; - double shrink_coff_k_; - double feedforward_effect_time_; - double static_effort ; - int count_; - bool state_change_; - ros::Time state_change_time; - - - enum - { - DOWN, - MID, - UP - }; - - int state_ = DOWN; - int last_state_ = DOWN; - int check_state_ = DOWN; - + double current_pos_{0.}; + double target_pos_{0.}; + double feedforward_offset{0.}; + double feedforward_effort{0.}; + double stretch_coff_A_{0.}; + double stretch_coff_k_{0.}; + double shrink_coff_A_{0.}; + double shrink_coff_k_{0.}; + double feedforward_effect_time_{0.}; + double static_effort{0.} ; + + ros::Time feedforward_timer; + State current_state_{State::DOWN}; + State last_state_{State::DOWN}; }; } diff --git a/rm_chassis_controllers/src/active_suspension.cpp b/rm_chassis_controllers/src/active_suspension.cpp index c3760038..dd41c95b 100644 --- a/rm_chassis_controllers/src/active_suspension.cpp +++ b/rm_chassis_controllers/src/active_suspension.cpp @@ -2,12 +2,6 @@ // Created by qiayuan on 2022/7/29. // -#include -#include - -#include -#include - #include "rm_chassis_controllers/active_suspension.h" namespace rm_chassis_controllers @@ -20,19 +14,11 @@ namespace rm_chassis_controllers XmlRpc::XmlRpcValue suspension_legs; controller_nh.getParam("suspension_leg", suspension_legs); - feed_forward_offset = getParam(controller_nh, "feed_forward_offset", 0.); stretch_coff_A_ = getParam(controller_nh,"stretch_coff_A_",0.); stretch_coff_k_ = getParam(controller_nh,"stretch_coff_k_",0.); shrink_coff_A_ = getParam(controller_nh,"shrink_coff_A_",0.); shrink_coff_k_ = getParam(controller_nh,"shrink_coff_k_",0.); - - - suspension_pos_DOWN_ = getParam(controller_nh,"suspension_pos_DOWN_",0.); - suspension_pos_MID_ = getParam(controller_nh,"suspension_pos_MID_",0.); - suspension_pos_UP_ = getParam(controller_nh,"suspension_pos_UP_",0.); - static_effort_DOWN_ = getParam(controller_nh,"static_effort_DOWN_",0.); - static_effort_MID_ = getParam(controller_nh,"static_effort_MID_",0.); - static_effort_UP_ = getParam(controller_nh,"static_effort_UP_",0.); + feedforward_offset = getParam(controller_nh, "feed_forward_offset", 0.); feedforward_effect_time_ = getParam(controller_nh,"feedforward_effect_time_",0.); for (const auto& suspension_leg : suspension_legs) @@ -50,79 +36,51 @@ namespace rm_chassis_controllers void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period) { OmniController::moveJoint(time, period); - switch (state_) + switch (current_state_) { - case DOWN: - suspension_pos_ = suspension_pos_DOWN_; + case State::DOWN: + target_pos_ = -0.05; static_effort = -6.0; break; - case MID: - suspension_pos_ = suspension_pos_MID_; - static_effort = 8.0; + case State::MID: + target_pos_ = 0.30; + static_effort = 9.0; break; - case UP: - suspension_pos_ = suspension_pos_UP_; - static_effort = 0.0; + case State::UP: + target_pos_ = 0.80; + static_effort = 8.0; break; } - if (state_ != check_state_) - { - count_ =0; - last_state_ = check_state_; - } - if (last_state_ - state_ == -2 || ((last_state_ - state_ == - 1) && (state_ ==2))) + if (current_state_ != last_state_) { - count_ ++; - if (count_ == 1) - { - state_change_time = ros::Time::now(); - } - feed_forward_effort = 2 * (stretch_coff_A_ + stretch_coff_k_ * pos); + last_state_ = current_state_; + feedforward_timer = ros::Time::now(); } - else if ( last_state_ - state_ == -1 && state_ == 1) + if ((ros::Time::now() - feedforward_timer).toSec() < feedforward_effect_time_) { - count_ ++; - if (count_ == 1) - { - state_change_time = ros::Time::now(); - } - feed_forward_effort = stretch_coff_A_ - 45.0 * pos; - } - else if (last_state_ - state_ == 1 || last_state_ - state_ == 2) - { - count_ ++; - if (count_ == 1) - { - state_change_time = ros::Time::now(); - } - feed_forward_effort = shrink_coff_A_ + shrink_coff_k_ * pos; + if (current_state_ == State::MID) + feedforward_effort = 1.2 * (stretch_coff_A_ - 45.0 * current_pos_); + else if (current_state_ == State::UP) + feedforward_effort = 2 * (stretch_coff_A_ + stretch_coff_k_ * current_pos_); + else if (current_state_ == State::DOWN) + feedforward_effort = shrink_coff_A_ + shrink_coff_k_ * current_pos_; } else - { - feed_forward_effort = 0.0; - } + feedforward_effort = 0.0; for (auto& joint : active_suspension_joints_) { - joint->setCommand(suspension_pos_); + joint->setCommand(target_pos_); joint->update(time, period); - pos = joint->getPosition(); + current_pos_ = joint->getPosition(); auto cmd = joint->joint_.getCommand(); - - cmd += feed_forward_effort + feed_forward_offset + static_effort; + cmd += feedforward_effort + feedforward_offset + static_effort; joint->joint_.setCommand(cmd); } - if (ros::Time::now().toSec() - state_change_time.toSec() > feedforward_effect_time_) - { - last_state_ = state_; - count_ = 0; - } - check_state_ = state_; } void ActiveSuspensionController::ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg) { - state_ = msg.mode; + current_state_ = static_cast(msg.mode); } - } // namespace rm_chassis_controllers -PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::ActiveSuspensionController, controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::ActiveSuspensionController, controller_interface::ControllerBase) \ No newline at end of file From 8cfc2b48cccbf428f87e586c3dc98afc80d02d3b Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 15 Mar 2026 10:17:44 +0800 Subject: [PATCH 13/14] Add default initialization for bullet solver. --- .../rm_gimbal_controllers/bullet_solver.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 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 d5871fb9..bdd53bd3 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -147,13 +147,13 @@ class BulletSolver double last_yaw_{}, filtered_yaw_{} ; double gimbal_switch_duration_{}; double yaw_subtract_; - double switch_armor_angle; + 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_; + 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_; @@ -168,9 +168,9 @@ class BulletSolver 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 using_traject_{}; + bool last_shoot_state_ {}; + bool is_aheading_two_{}; geometry_msgs::Point after_traject_output_yaw_{}; geometry_msgs::Point target_pos_{}; From 0829f42699e1c087f5b8e2398b33b39feb593b66 Mon Sep 17 00:00:00 2001 From: xuncheng <205455603@qq.com> Date: Sun, 15 Mar 2026 23:26:15 +0800 Subject: [PATCH 14/14] Fix format. --- .../active_suspension.h | 64 +++--- .../include/rm_chassis_controllers/omni.h | 1 - .../src/active_suspension.cpp | 146 +++++++------ rm_chassis_controllers/src/chassis_base.cpp | 2 +- .../rm_gimbal_controllers/bullet_solver.h | 25 +-- .../rm_gimbal_controllers/gimbal_base.h | 8 +- rm_gimbal_controllers/src/bullet_solver.cpp | 206 ++++++++++-------- rm_gimbal_controllers/src/gimbal_base.cpp | 3 +- 8 files changed, 237 insertions(+), 218 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h index bc63707c..98447253 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/active_suspension.h @@ -14,40 +14,40 @@ namespace rm_chassis_controllers { - enum class State - { - DOWN = 0, - MID = 1, - UP = 2 - }; +enum class State +{ + DOWN = 0, + MID = 1, + UP = 2 +}; - class ActiveSuspensionController : public OmniController - { - public: - ActiveSuspensionController() = default; - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; - void ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg); +class ActiveSuspensionController : public OmniController +{ +public: + ActiveSuspensionController() = default; + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; + void ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg); - private: - void moveJoint(const ros::Time& time, const ros::Duration& period) override; +private: + void moveJoint(const ros::Time& time, const ros::Duration& period) override; - std::vector> active_suspension_joints_; - std::vector active_suspension_joint_handles_{}; + std::vector> active_suspension_joints_; + std::vector active_suspension_joint_handles_{}; - ros::Subscriber active_suspension_sub_; - double current_pos_{0.}; - double target_pos_{0.}; - double feedforward_offset{0.}; - double feedforward_effort{0.}; - double stretch_coff_A_{0.}; - double stretch_coff_k_{0.}; - double shrink_coff_A_{0.}; - double shrink_coff_k_{0.}; - double feedforward_effect_time_{0.}; - double static_effort{0.} ; + ros::Subscriber active_suspension_sub_; + double current_pos_{ 0. }; + double target_pos_{ 0. }; + double feedforward_offset{ 0. }; + double feedforward_effort{ 0. }; + double stretch_coff_A_{ 0. }; + double stretch_coff_k_{ 0. }; + double shrink_coff_A_{ 0. }; + double shrink_coff_k_{ 0. }; + double feedforward_effect_time_{ 0. }; + double static_effort{ 0. }; - ros::Time feedforward_timer; - State current_state_{State::DOWN}; - State last_state_{State::DOWN}; - }; -} + ros::Time feedforward_timer; + State current_state_{ State::DOWN }; + State last_state_{ State::DOWN }; +}; +} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 852c03de..1bc67336 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -24,7 +24,6 @@ class OmniController : public ChassisBase()); - if (!active_suspension_joints_.back()->init(effort_joint_interface_, nh_suspension)) - { - ROS_ERROR("Failed to init active_suspension_joint"); - return false; - } - } - return true; - } - void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period) + for (const auto& suspension_leg : suspension_legs) + { + ros::NodeHandle nh_suspension = ros::NodeHandle(controller_nh, "suspension_leg/" + suspension_leg.first); + active_suspension_joints_.push_back(std::make_shared()); + if (!active_suspension_joints_.back()->init(effort_joint_interface_, nh_suspension)) { - OmniController::moveJoint(time, period); - switch (current_state_) - { - case State::DOWN: - target_pos_ = -0.05; - static_effort = -6.0; - break; - case State::MID: - target_pos_ = 0.30; - static_effort = 9.0; - break; - case State::UP: - target_pos_ = 0.80; - static_effort = 8.0; - break; - } - if (current_state_ != last_state_) - { - last_state_ = current_state_; - feedforward_timer = ros::Time::now(); - } - if ((ros::Time::now() - feedforward_timer).toSec() < feedforward_effect_time_) - { - if (current_state_ == State::MID) - feedforward_effort = 1.2 * (stretch_coff_A_ - 45.0 * current_pos_); - else if (current_state_ == State::UP) - feedforward_effort = 2 * (stretch_coff_A_ + stretch_coff_k_ * current_pos_); - else if (current_state_ == State::DOWN) - feedforward_effort = shrink_coff_A_ + shrink_coff_k_ * current_pos_; - } - else - feedforward_effort = 0.0; - - for (auto& joint : active_suspension_joints_) - { - joint->setCommand(target_pos_); - joint->update(time, period); - current_pos_ = joint->getPosition(); - auto cmd = joint->joint_.getCommand(); - cmd += feedforward_effort + feedforward_offset + static_effort; - joint->joint_.setCommand(cmd); - } - } - void ActiveSuspensionController::ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg) - { - current_state_ = static_cast(msg.mode); + ROS_ERROR("Failed to init active_suspension_joint"); + return false; } + } + return true; +} +void ActiveSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period) +{ + OmniController::moveJoint(time, period); + switch (current_state_) + { + case State::DOWN: + target_pos_ = -0.05; + static_effort = -6.0; + break; + case State::MID: + target_pos_ = 0.30; + static_effort = 9.0; + break; + case State::UP: + target_pos_ = 0.80; + static_effort = 8.0; + break; + } + if (current_state_ != last_state_) + { + last_state_ = current_state_; + feedforward_timer = ros::Time::now(); + } + if ((ros::Time::now() - feedforward_timer).toSec() < feedforward_effect_time_) + { + if (current_state_ == State::MID) + feedforward_effort = 1.2 * (stretch_coff_A_ - 45.0 * current_pos_); + else if (current_state_ == State::UP) + feedforward_effort = 2 * (stretch_coff_A_ + stretch_coff_k_ * current_pos_); + else if (current_state_ == State::DOWN) + feedforward_effort = shrink_coff_A_ + shrink_coff_k_ * current_pos_; + } + else + feedforward_effort = 0.0; + + for (auto& joint : active_suspension_joints_) + { + joint->setCommand(target_pos_); + joint->update(time, period); + current_pos_ = joint->getPosition(); + auto cmd = joint->joint_.getCommand(); + cmd += feedforward_effort + feedforward_offset + static_effort; + joint->joint_.setCommand(cmd); + } +} +void ActiveSuspensionController::ActiveSuspensionCallBack(const rm_msgs::ChassisActiveSusCmd& msg) +{ + current_state_ = static_cast(msg.mode); +} } // namespace rm_chassis_controllers -PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::ActiveSuspensionController, controller_interface::ControllerBase) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::ActiveSuspensionController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index 01c7eeec..2be04912 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -142,7 +142,7 @@ void ChassisBase::update(const ros::Time& time, const ros::Duration& perio vel_cmd_.y = ramp_y_->output(); vel_cmd_.z = cmd_vel.angular.z; } -//test + // test if (cmd_rt_buffer_.readFromRT()->cmd_chassis_.follow_source_frame.empty()) follow_source_frame_ = "yaw"; else 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 bdd53bd3..6a4ea233 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -58,32 +58,31 @@ 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; + 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 traject_ahead_; int clean_shoot_num_; - double end_pos_offset,move_switch_time_coff_; + double end_pos_offset, move_switch_time_coff_; }; struct TrajectoryFunctionCoefficients { - double a0,a1,a2,a3; + double a0, a1, a2, a3; }; struct TrajectoryLimitParams { - double start_pos,end_pos,start_vel,end_vel; + double start_pos, end_pos, start_vel, end_vel; }; class BulletSolver { public: - 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 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; @@ -113,7 +112,7 @@ class BulletSolver } bool getTrackTarget() { - return track_target_; + return track_target_; } double getGimbalSwitchDuration(double v_yaw); void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, @@ -123,7 +122,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_); void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg); ~BulletSolver() = default; @@ -140,11 +139,11 @@ class BulletSolver dynamic_reconfigure::Server* d_srv_{}; Config config_{}; double max_track_target_vel_; - double output_yaw_{}, output_pitch_{},traject_output_yaw_{}; + double output_yaw_{}, output_pitch_{}, traject_output_yaw_{}; double bullet_speed_{}, resistance_coff_{}; double fly_time_; double switch_hysteresis_; - double last_yaw_{}, filtered_yaw_{} ; + double last_yaw_{}, filtered_yaw_{}; double gimbal_switch_duration_{}; double yaw_subtract_; double switch_armor_angle{}; @@ -169,7 +168,7 @@ class BulletSolver bool dynamic_reconfig_initialized_{}; bool change_armor = false; bool using_traject_{}; - bool last_shoot_state_ {}; + bool last_shoot_state_{}; bool is_aheading_two_{}; geometry_msgs::Point after_traject_output_yaw_{}; 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 f43efb13..90ee90b5 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -137,7 +137,8 @@ class Controller : public controller_interface::MultiInterfaceController #include - namespace rm_gimbal_controllers { BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) @@ -54,7 +53,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .resistance_coff_qd_30 = getParam(controller_nh, "resistance_coff_qd_30", 0.), .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), - .center_delay = getParam(controller_nh,"center_delay",0.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.), @@ -64,11 +63,11 @@ 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.), .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3), - .max_selected_armor_ = getParam(controller_nh,"max_selecteed_armor",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), + .end_pos_offset = getParam(controller_nh, "end_pos_offset", 0.), + .move_switch_time_coff_ = getParam(controller_nh, "move_switch_time_coff_", 0.001), }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); switch_hysteresis_ = getParam(controller_nh, "switch_hysteresis", 1.0); @@ -101,11 +100,12 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) shoot_beforehand_cmd_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); - bullet_solver_pub.reset(new realtime_tools::RealtimePublisher(controller_nh,"bullet_solver_data",10)); + bullet_solver_pub.reset( + new realtime_tools::RealtimePublisher(controller_nh, "bullet_solver_data", 10)); identified_target_change_sub_ = controller_nh.subscribe("/change", 10, &BulletSolver::identifiedTargetChangeCB, this); - shoot_state_sub_ = - controller_nh.subscribe("/local_heat_state/shooter_state", 50, &BulletSolver::heatCB, this); + shoot_state_sub_ = controller_nh.subscribe("/local_heat_state/shooter_state", 50, + &BulletSolver::heatCB, this); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -126,7 +126,7 @@ double BulletSolver::getResistanceCoefficient(double bullet_speed) 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) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; @@ -161,10 +161,12 @@ 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_; + 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.321 * exp(-0.289 * abs(filtered_v_yaw_+1.0)) * config_.traject_ahead_ + move_switch_time_offset; + traject_switch_time_ = + 1.321 * exp(-0.289 * abs(filtered_v_yaw_ + 1.0)) * config_.traject_ahead_ + 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) @@ -172,7 +174,9 @@ 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_) + 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; } @@ -181,7 +185,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d 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.)) + 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) @@ -189,27 +194,27 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d count_ = 0; next_count_ = 0; change_armor = false; - switche_time_yaw_ = after_fly_yaw_subtract_ + selected_armor_ * 2 * M_PI /armors_num; + 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; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; - if ((after_fly_yaw_subtract_ - 2 * M_PI /armors_num > switch_armor_angle ) && v_yaw > 0.) + if ((after_fly_yaw_subtract_ - 2 * M_PI / armors_num > switch_armor_angle) && v_yaw > 0.) { selected_armor_ = -2; - next_count_ ++; + next_count_++; if (next_count_ == config_.min_fit_switch_count) { switch_armor_time_ = ros::Time::now(); 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; - next_count_ ++; + next_count_++; if (next_count_ == config_.min_fit_switch_count) { switch_armor_time_ = ros::Time::now(); @@ -230,24 +235,26 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d } } } - 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; - start_using_traject_time = ros::Time::now(); - } - } + 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; + 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_; + 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_) @@ -259,28 +266,29 @@ 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.5) || + (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay)) < -0.5)) { 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) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + - 1 * 2 * M_PI / armors_num) < -0.5)) ) + 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))) { 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) || - (filtered_v_yaw_ < -1.0 && (yaw_subtract_ + filtered_v_yaw_ * (fly_time_ + config_.center_delay) + - 2 * 2 * M_PI / armors_num) < -0.5)) ) + 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))) { selected_armor_ = filtered_v_yaw_ > 0. ? -3 : 3; } - if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || (selected_armor_ < 0 && selected_armor_ < - config_.max_selected_armor_)) + if ((selected_armor_ > 0 && selected_armor_ > config_.max_selected_armor_) || + (selected_armor_ < 0 && selected_armor_ < -config_.max_selected_armor_)) { - selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; + selected_armor_ = selected_armor_ > 0 ? config_.max_selected_armor_ : -config_.max_selected_armor_; } if (selected_armor_ % 2 == 0) { @@ -306,38 +314,46 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d 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 + 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); + 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 = 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 = + 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; + 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); + A << std::pow(switchtime, 2), std::pow(switchtime, 3), 2 * switchtime, 3 * std::pow(switchtime, 2); Eigen::Vector2d B; - B<= 0.001) @@ -346,17 +362,17 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d 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_ ; + (-std::log(1 - target_rho * resistance_coff_ / (bullet_speed_ * std::cos(output_pitch_)))) / resistance_coff_; double real_z = (bullet_speed_ * std::sin(output_pitch_) + (config_.g / resistance_coff_)) * - (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - - config_.g * fly_time_ / resistance_coff_; + (1 - std::exp(-resistance_coff_ * fly_time_)) / resistance_coff_ - + config_.g * fly_time_ / resistance_coff_; 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); + 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); + pos.y + vel.y * fly_time_ - r * sin(yaw + v_yaw * fly_time_ + selected_armor_ * 2 * M_PI / armors_num); } else { @@ -364,9 +380,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d 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_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_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_; @@ -388,8 +404,8 @@ 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); - if ( ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) + traject_output_yaw_ = planningPoint(temp, start_using_traject_time); + if (ros::Time::now() - start_using_traject_time > ros::Duration(switchtime)) { using_traject_ = false; } @@ -405,30 +421,30 @@ 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 = switch_armor_time_.toSec(); bullet_solver_pub->msg_.switche_time_yaw_ = switche_time_yaw_; bullet_solver_pub->msg_.traject_acc_ = traject_max_acc_; 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_.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_.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->unlockAndPublish(); } @@ -592,11 +608,13 @@ 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)) 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() > + (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) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; @@ -611,15 +629,15 @@ 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 a0=trajectory_function_coefficients.a0; - double a1=trajectory_function_coefficients.a1; - double a2=trajectory_function_coefficients.a2; - double a3=trajectory_function_coefficients.a3; + double a0 = trajectory_function_coefficients.a0; + double a1 = trajectory_function_coefficients.a1; + double a2 = trajectory_function_coefficients.a2; + 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; + 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; return plansetpoint; } @@ -688,7 +706,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .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_ }; 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 30ab00e9..68eecddb 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -151,7 +151,6 @@ 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.; - cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); @@ -327,7 +326,7 @@ void Controller::track(const ros::Time& time) } if (solve_success) - setDes(time, bullet_solver_->getYaw(), bullet_solver_->getPitch(),bullet_solver_->getTrajectYaw()); + setDes(time, bullet_solver_->getYaw(), bullet_solver_->getPitch(), bullet_solver_->getTrajectYaw()); else { odom2gimbal_des_.header.stamp = time;