From 9927d0da262d648e4b621976115cfebf24fd31ba Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 2 Apr 2024 19:12:51 +0800 Subject: [PATCH 01/51] Add chassis vel feedforward for yaw. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 5 +++-- .../include/rm_gimbal_controllers/gimbal_base.h | 5 ++--- rm_gimbal_controllers/src/gimbal_base.cpp | 17 +++++++++-------- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 0dd3e3e2..531897f5 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -5,7 +5,8 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("yaw_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0) -gen.add("pitch_k_v_", double_t, 0, "Pid pitch position max output", 0.0, 0, 1.0) +gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0) +gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) +gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) 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 7001757e..a29a7415 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -60,8 +60,8 @@ namespace rm_gimbal_controllers { struct GimbalConfig { - // Input feedforward - double yaw_k_v_, pitch_k_v_; + // feedforward + double yaw_k_v_, pitch_k_v_, k_chassis_vel_; }; class ChassisVel @@ -190,7 +190,6 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; bool dynamic_reconfig_initialized_{}; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index a5e13561..dfcc35b3 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -67,7 +67,6 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro velocity_saturation_point_ = getParam(resistance_compensation_nh, "velocity_saturation_point", 0.); effort_saturation_point_ = getParam(resistance_compensation_nh, "effort_saturation_point", 0.); - k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.); ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); chassis_vel_ = std::make_shared(chassis_vel_nh); ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); @@ -78,7 +77,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos"); ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); - config_ = { .yaw_k_v_ = getParam(nh_pitch, "k_v", 0.), .pitch_k_v_ = getParam(nh_yaw, "k_v", 0.) }; + config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.), + .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.), + .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -472,8 +473,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } loop_count_++; - ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - - angular_vel_yaw.z); + ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); @@ -539,12 +540,12 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig init_config = *config_rt_buffer_.readFromNonRT(); // config init use yaml config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; + config.k_chassis_vel_ = init_config.k_chassis_vel_; dynamic_reconfig_initialized_ = true; } - GimbalConfig config_non_rt{ - .yaw_k_v_ = config.yaw_k_v_, - .pitch_k_v_ = config.pitch_k_v_, - }; + GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, + .pitch_k_v_ = config.pitch_k_v_, + .k_chassis_vel_ = config.k_chassis_vel_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From adc60be8d3330ce83cdf75204cc69a7b44dc0c9c Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 3 Apr 2024 23:55:13 +0800 Subject: [PATCH 02/51] Use lp filter to estimate chassis angular velocity. --- .../include/rm_gimbal_controllers/gimbal_base.h | 6 ++++++ rm_gimbal_controllers/src/gimbal_base.cpp | 3 ++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index a29a7415..47c90e31 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -55,6 +55,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -74,6 +75,8 @@ class ChassisVel nh.param("debug", is_debug_, true); linear_ = std::make_shared>(num_data); angular_ = std::make_shared>(num_data); + ros::NodeHandle nh_lp = ros::NodeHandle(nh, "lp"); + angular_lp_filter_ = std::make_unique(nh_lp); if (is_debug_) { real_pub_.reset(new realtime_tools::RealtimePublisher(nh, "real", 1)); @@ -82,6 +85,7 @@ class ChassisVel } std::shared_ptr> linear_; std::shared_ptr> angular_; + std::shared_ptr angular_lp_filter_; void update(double linear_vel[3], double angular_vel[3], double period) { if (period < 0) @@ -90,9 +94,11 @@ class ChassisVel { linear_->clear(); angular_->clear(); + angular_lp_filter_->reset(); } linear_->input(linear_vel); angular_->input(angular_vel); + angular_lp_filter_->input(angular_vel[2]); if (is_debug_ && loop_count_ % 10 == 0) { if (real_pub_->trylock()) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index dfcc35b3..f444c41b 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -473,7 +473,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } loop_count_++; - ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + + ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - + config_.k_chassis_vel_ * chassis_vel_->angular_lp_filter_->output() + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); From 1880ed74c5e2396dc778b72622a6caf551c5800a Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 03:26:41 +0800 Subject: [PATCH 03/51] Finish the target position and compensation of yaw. --- rm_gimbal_controllers/src/gimbal_base.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index f444c41b..2a96122f 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -283,14 +283,15 @@ void Controller::track(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - target_pos.x -= odom2pitch_.transform.translation.x; - target_pos.y -= odom2pitch_.transform.translation.y; - target_pos.z -= odom2pitch_.transform.translation.z; + double yaw = data_track_.yaw + data_track_.v_yaw * (time - data_track_.header.stamp).toSec(); + target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; + target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; + target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); bool solve_success = - bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, data_track_.yaw, data_track_.v_yaw, + 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); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) From 87003317c40c12dc0a08f75b3b1e7f57aec89f93 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 17:00:44 +0800 Subject: [PATCH 04/51] Add flag by subscribing vision target changed. --- .../include/rm_gimbal_controllers/bullet_solver.h | 5 +++++ rm_gimbal_controllers/src/bullet_solver.cpp | 14 ++++++++++++++ 2 files changed, 19 insertions(+) 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 5ed6d081..5fbdca0b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,6 +46,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -77,12 +78,14 @@ class BulletSolver geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); + void IsVisionTargetChangedCallback(const std_msgs::Bool data); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; + ros::Subscriber vision_target_changed_sub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; @@ -90,8 +93,10 @@ class BulletSolver bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; + ros::Time switch_angle_time_{}; int selected_armor_; bool track_target_; + bool state_changed_ = true; geometry_msgs::Point target_pos_{}; double fly_time_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 94188c8a..2bc22bd3 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -80,6 +80,9 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); + + vision_target_changed_sub_ = controller_nh.subscribe( + "/armor_processor/change", 10, &BulletSolver::IsVisionTargetChangedCallback, this); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -123,6 +126,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { + if (state_changed_) + { + state_changed_ = false; + switch_angle_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; @@ -301,6 +309,12 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec return error; } +void BulletSolver::IsVisionTargetChangedCallback(const std_msgs::Bool data) +{ + if (data.data) + state_changed_ = true; +} + void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t /*unused*/) { ROS_INFO("[Bullet Solver] Dynamic params change"); From 00ecfab5d9451db47a7eabdcb7529910797cf7fb Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 18:22:07 +0800 Subject: [PATCH 05/51] Control fire near switching target. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 1 + .../rm_gimbal_controllers/bullet_solver.h | 7 ++++- rm_gimbal_controllers/src/bullet_solver.cpp | 27 +++++++++++++++++-- rm_gimbal_controllers/src/gimbal_base.cpp | 1 + 4 files changed, 33 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 8f2b4f4a..0f10dd5c 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -14,5 +14,6 @@ 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("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("time_interrupt_", double_t, 0, "The interrupt of the switching time", 2.0, 0.0, 2.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 5fbdca0b..f1947f70 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -47,13 +47,14 @@ #include #include #include +#include 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, dt, timeout; + resistance_coff_qd_30, g, delay, dt, timeout, time_interrupt_; }; class BulletSolver @@ -77,6 +78,7 @@ class BulletSolver void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num); + void IgnoreErrorToShoot(const ros::Time& time); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void IsVisionTargetChangedCallback(const std_msgs::Bool data); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); @@ -85,6 +87,7 @@ class BulletSolver private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; + std::shared_ptr> control_fire_near_switching_pub_; ros::Subscriber vision_target_changed_sub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; @@ -93,10 +96,12 @@ class BulletSolver bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; + double is_shoot_ignore_error_{}; ros::Time switch_angle_time_{}; int selected_armor_; bool track_target_; bool state_changed_ = true; + bool is_in_delay_before_switch_{}; geometry_msgs::Point target_pos_{}; double fly_time_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 2bc22bd3..11a2bd8f 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -52,7 +52,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .g = getParam(controller_nh, "g", 0.), .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), - .timeout = getParam(controller_nh, "timeout", 0.) }; + .timeout = getParam(controller_nh, "timeout", 0.), + .time_interrupt_ = getParam(controller_nh, "time_interrupt", 2.0) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -80,6 +81,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); + control_fire_near_switching_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); vision_target_changed_sub_ = controller_nh.subscribe( "/armor_processor/change", 10, &BulletSolver::IsVisionTargetChangedCallback, this); @@ -123,6 +126,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d acos(r / target_rho) - M_PI / 12 + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : M_PI / 12; + is_in_delay_before_switch_ = + ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && + track_target_; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { @@ -315,6 +322,20 @@ void BulletSolver::IsVisionTargetChangedCallback(const std_msgs::Bool data) state_changed_ = true; } +void BulletSolver::IgnoreErrorToShoot(const ros::Time& time) +{ + if ((ros::Time::now() - switch_angle_time_ < ros::Duration(config_.time_interrupt_)) || is_in_delay_before_switch_) + is_shoot_ignore_error_ = 0.; + else + is_shoot_ignore_error_ = 1.; + if (control_fire_near_switching_pub_->trylock()) + { + control_fire_near_switching_pub_->msg_.stamp = time; + control_fire_near_switching_pub_->msg_.error = is_shoot_ignore_error_; + control_fire_near_switching_pub_->unlockAndPublish(); + } +} + void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t /*unused*/) { ROS_INFO("[Bullet Solver] Dynamic params change"); @@ -330,6 +351,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.delay = init_config.delay; config.dt = init_config.dt; config.timeout = init_config.timeout; + config.time_interrupt_ = init_config.time_interrupt_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -340,7 +362,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .g = config.g, .delay = config.delay, .dt = config.dt, - .timeout = config.timeout }; + .timeout = config.timeout, + .time_interrupt_ = config.time_interrupt_ }; 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 2a96122f..ab195e1d 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -293,6 +293,7 @@ void Controller::track(const ros::Time& time) bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); + bullet_solver_->IgnoreErrorToShoot(time); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { From d5ee2641623580f6e0bb58a49ec750ae50dc4c90 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 18:42:49 +0800 Subject: [PATCH 06/51] Modify switch limit. --- rm_gimbal_controllers/src/bullet_solver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 11a2bd8f..c08bba22 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -122,10 +122,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double r = r1; double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = track_target_ ? - acos(r / target_rho) - M_PI / 12 + - (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : - M_PI / 12; + double switch_armor_angle = track_target_ ? acos(r / target_rho) - 40 / 180 * M_PI + + (-acos(r / target_rho) + (40 / 180 * M_PI + 4 / 180 * M_PI)) * + std::abs(v_yaw) / max_track_target_vel_ : + 4 / 180 * M_PI; is_in_delay_before_switch_ = ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && From 105f1b9b2e03376735bbde157f1022c2becfe3fa Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 18:43:28 +0800 Subject: [PATCH 07/51] Modify switch angle. --- rm_gimbal_controllers/src/bullet_solver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index c08bba22..898c0f98 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -123,9 +123,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; double switch_armor_angle = track_target_ ? acos(r / target_rho) - 40 / 180 * M_PI + - (-acos(r / target_rho) + (40 / 180 * M_PI + 4 / 180 * M_PI)) * + (-acos(r / target_rho) + (40 / 180 * M_PI + 2 / 180 * M_PI)) * std::abs(v_yaw) / max_track_target_vel_ : - 4 / 180 * M_PI; + 2 / 180 * M_PI; is_in_delay_before_switch_ = ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && From d41e046c4cb476078f3d542a2ab6cb5b50adcf56 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 19:58:59 +0800 Subject: [PATCH 08/51] Modify something wrong. --- rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index ab195e1d..7a263444 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -480,9 +480,9 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) From 53f4a43b18a67e257ea1fcd2c94c58d2423453d0 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 20:34:56 +0800 Subject: [PATCH 09/51] Add mode for suggest fire. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 1 + .../include/rm_gimbal_controllers/bullet_solver.h | 3 ++- rm_gimbal_controllers/src/bullet_solver.cpp | 13 ++++++++++--- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 0f10dd5c..8990e54c 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -15,5 +15,6 @@ gen.add("delay", double_t, 0, "Delay of bullet firing", 0.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("time_interrupt_", double_t, 0, "The interrupt of the switching time", 2.0, 0.0, 2.0) +gen.add("time_over_", double_t, 0, "The cover if oversight", 2.0, 0.0, 2.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 f1947f70..e7cc2fe1 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -48,13 +48,14 @@ #include #include #include +#include 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, dt, timeout, time_interrupt_; + resistance_coff_qd_30, g, delay, dt, timeout, time_interrupt_, time_over_; }; class BulletSolver diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 898c0f98..075dd86c 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -53,7 +53,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.), - .time_interrupt_ = getParam(controller_nh, "time_interrupt", 2.0) }; + .time_interrupt_ = getParam(controller_nh, "time_interrupt", 2.0), + .time_over_ = getParam(controller_nh, "time_over", 2.0) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -324,8 +325,12 @@ void BulletSolver::IsVisionTargetChangedCallback(const std_msgs::Bool data) void BulletSolver::IgnoreErrorToShoot(const ros::Time& time) { - if ((ros::Time::now() - switch_angle_time_ < ros::Duration(config_.time_interrupt_)) || is_in_delay_before_switch_) + if (is_in_delay_before_switch_ && !state_changed_) + is_shoot_ignore_error_ = -1.0; + else if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_interrupt_).toSec()) is_shoot_ignore_error_ = 0.; + else if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_over_).toSec()) + is_shoot_ignore_error_ = 2.; else is_shoot_ignore_error_ = 1.; if (control_fire_near_switching_pub_->trylock()) @@ -352,6 +357,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.dt = init_config.dt; config.timeout = init_config.timeout; config.time_interrupt_ = init_config.time_interrupt_; + config.timeout = init_config.time_over_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -363,7 +369,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .delay = config.delay, .dt = config.dt, .timeout = config.timeout, - .time_interrupt_ = config.time_interrupt_ }; + .time_interrupt_ = config.time_interrupt_, + .time_over_ = config.time_over_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers From 907918f79fcf1c309f8381e64b89a08af95c3c34 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 22:30:05 +0800 Subject: [PATCH 10/51] Add suggest_fire near switching armor. --- rm_gimbal_controllers/src/bullet_solver.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 075dd86c..6629a2fc 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -53,8 +53,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.), - .time_interrupt_ = getParam(controller_nh, "time_interrupt", 2.0), - .time_over_ = getParam(controller_nh, "time_over", 2.0) }; + .time_interrupt_ = getParam(controller_nh, "time_interrupt", 0.0), + .time_over_ = getParam(controller_nh, "time_over", 0.0) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -325,10 +325,10 @@ void BulletSolver::IsVisionTargetChangedCallback(const std_msgs::Bool data) void BulletSolver::IgnoreErrorToShoot(const ros::Time& time) { - if (is_in_delay_before_switch_ && !state_changed_) + if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_interrupt_).toSec()) is_shoot_ignore_error_ = -1.0; - else if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_interrupt_).toSec()) - is_shoot_ignore_error_ = 0.; + else if (is_in_delay_before_switch_ && selected_armor_ == 0) + is_shoot_ignore_error_ = 0.0; else if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_over_).toSec()) is_shoot_ignore_error_ = 2.; else From b1e8201224a0ccb53bb688cd91b04cf2e1055418 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 22:30:29 +0800 Subject: [PATCH 11/51] Add dynamic reconfig. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 8990e54c..448ce566 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -14,7 +14,7 @@ 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("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("time_interrupt_", double_t, 0, "The interrupt of the switching time", 2.0, 0.0, 2.0) -gen.add("time_over_", double_t, 0, "The cover if oversight", 2.0, 0.0, 2.0) +gen.add("time_interrupt_", double_t, 0, "The interrupt of the switching time", 0.0, 0.0, 2.0) +gen.add("time_over_", double_t, 0, "The cover if oversight", 0.0, 0.0, 2.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) From c675052ce6869364a6f1fa48ddd55d3a27c0dc7c Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 23:07:29 +0800 Subject: [PATCH 12/51] Add angles' dynamic reconfig. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 ++ .../include/rm_gimbal_controllers/bullet_solver.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 10 ++++++++-- 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 448ce566..1cd1c665 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -16,5 +16,7 @@ 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("time_interrupt_", double_t, 0, "The interrupt of the switching time", 0.0, 0.0, 2.0) gen.add("time_over_", double_t, 0, "The cover if oversight", 0.0, 0.0, 2.0) +gen.add("angle1", double_t, 0, "Switch angle1", 40.0, 0.0, 90.0) +gen.add("angle2", double_t, 0, "Switch angle2", 2.0, 0.0, 90.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 e7cc2fe1..fcfe65b1 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -55,7 +55,7 @@ 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, dt, timeout, time_interrupt_, time_over_; + resistance_coff_qd_30, g, delay, dt, timeout, time_interrupt_, time_over_, angle1, angle2; }; class BulletSolver diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 6629a2fc..7afa09c5 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -54,7 +54,9 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.), .time_interrupt_ = getParam(controller_nh, "time_interrupt", 0.0), - .time_over_ = getParam(controller_nh, "time_over", 0.0) }; + .time_over_ = getParam(controller_nh, "time_over", 0.0), + .angle1 = getParam(controller_nh, "angle1", 40.0), + .angle2 = getParam(controller_nh, "angle2", 2.0) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -358,6 +360,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.timeout = init_config.timeout; config.time_interrupt_ = init_config.time_interrupt_; config.timeout = init_config.time_over_; + config.angle1 = init_config.angle1; + config.angle2 = init_config.angle2; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -370,7 +374,9 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .dt = config.dt, .timeout = config.timeout, .time_interrupt_ = config.time_interrupt_, - .time_over_ = config.time_over_ }; + .time_over_ = config.time_over_, + .angle1 = config.angle1, + .angle2 = config.angle2 }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers From da4a08594313fe3d1566938378207797f688b158 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 4 Apr 2024 23:57:14 +0800 Subject: [PATCH 13/51] Modify something wrong. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 1cd1c665..68b5e7f4 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -15,7 +15,7 @@ gen.add("delay", double_t, 0, "Delay of bullet firing", 0.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("time_interrupt_", double_t, 0, "The interrupt of the switching time", 0.0, 0.0, 2.0) -gen.add("time_over_", double_t, 0, "The cover if oversight", 0.0, 0.0, 2.0) +gen.add("time_over_", double_t, 0, "The over if oversight", 0.0, 0.0, 2.0) gen.add("angle1", double_t, 0, "Switch angle1", 40.0, 0.0, 90.0) gen.add("angle2", double_t, 0, "Switch angle2", 2.0, 0.0, 90.0) From 14f446723ce5c6b7ad678c46e770007f0dfc209e Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 5 Apr 2024 00:00:26 +0800 Subject: [PATCH 14/51] Add dynamic reconfig. --- rm_gimbal_controllers/src/bullet_solver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 7afa09c5..476cf53d 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -125,10 +125,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double r = r1; double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = track_target_ ? acos(r / target_rho) - 40 / 180 * M_PI + - (-acos(r / target_rho) + (40 / 180 * M_PI + 2 / 180 * M_PI)) * + double switch_armor_angle = track_target_ ? acos(r / target_rho) - config_.angle1 + + (-acos(r / target_rho) + (config_.angle1 + config_.angle2)) * std::abs(v_yaw) / max_track_target_vel_ : - 2 / 180 * M_PI; + config_.angle2; is_in_delay_before_switch_ = ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && From 99b4063e50c1cfd23828cd5ed37f8953bd628b4f Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 5 Apr 2024 00:02:02 +0800 Subject: [PATCH 15/51] Modify something wrong. --- rm_gimbal_controllers/src/bullet_solver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 476cf53d..b1aed307 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -359,7 +359,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.dt = init_config.dt; config.timeout = init_config.timeout; config.time_interrupt_ = init_config.time_interrupt_; - config.timeout = init_config.time_over_; + config.time_over_ = init_config.time_over_; config.angle1 = init_config.angle1; config.angle2 = init_config.angle2; dynamic_reconfig_initialized_ = true; From 47b616288d890c19ac25d53066bbfb5acc6e4b2e Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 5 Apr 2024 01:19:04 +0800 Subject: [PATCH 16/51] Modify something wrong. --- rm_gimbal_controllers/src/bullet_solver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index b1aed307..c02dc4ba 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -125,6 +125,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double r = r1; double z = pos.z; track_target_ = std::abs(v_yaw) < max_track_target_vel_; + config_.angle1 = config_.angle1 / 180 * M_PI; + config_.angle2 = config_.angle2 / 180 * M_PI; double switch_armor_angle = track_target_ ? acos(r / target_rho) - config_.angle1 + (-acos(r / target_rho) + (config_.angle1 + config_.angle2)) * std::abs(v_yaw) / max_track_target_vel_ : From e48f42e520d7362bfa80cfc664a52ee827ceaf25 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 5 Apr 2024 02:16:55 +0800 Subject: [PATCH 17/51] Add delay. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 1 + .../rm_gimbal_controllers/gimbal_base.h | 1 + rm_gimbal_controllers/src/bullet_solver.cpp | 12 ++++++------ rm_gimbal_controllers/src/gimbal_base.cpp | 18 ++++++++++++------ 4 files changed, 20 insertions(+), 12 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 531897f5..916f7175 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -8,5 +8,6 @@ gen = ParameterGenerator() gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0) gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0) +gen.add("delay", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, -1.0, 1.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) 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 47c90e31..9b79c985 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -63,6 +63,7 @@ struct GimbalConfig { // feedforward double yaw_k_v_, pitch_k_v_, k_chassis_vel_; + double delay; }; class ChassisVel diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index c02dc4ba..e69cce55 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -124,13 +124,13 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; + double angle1 = config_.angle1 / 180 * M_PI; + double angle2 = config_.angle2 / 180 * M_PI; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - config_.angle1 = config_.angle1 / 180 * M_PI; - config_.angle2 = config_.angle2 / 180 * M_PI; - double switch_armor_angle = track_target_ ? acos(r / target_rho) - config_.angle1 + - (-acos(r / target_rho) + (config_.angle1 + config_.angle2)) * - std::abs(v_yaw) / max_track_target_vel_ : - config_.angle2; + double switch_armor_angle = + track_target_ ? acos(r / target_rho) - angle1 + + (-acos(r / target_rho) + (angle1 + angle2)) * std::abs(v_yaw) / max_track_target_vel_ : + angle2; is_in_delay_before_switch_ = ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 7a263444..9967c3a4 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -79,7 +79,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.), .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.), - .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.) }; + .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.), + .delay = getParam(controller_nh, "delay", 0.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -283,10 +284,13 @@ void Controller::track(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - double yaw = data_track_.yaw + data_track_.v_yaw * (time - data_track_.header.stamp).toSec(); - target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; - target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; - target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec() + config_.delay); + target_pos.x += + target_vel.x * ((time - data_track_.header.stamp).toSec() + config_.delay) - odom2pitch_.transform.translation.x; + target_pos.y += + target_vel.y * ((time - data_track_.header.stamp).toSec() + config_.delay) - odom2pitch_.transform.translation.y; + target_pos.z += + target_vel.z * ((time - data_track_.header.stamp).toSec() + config_.delay) - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); @@ -544,11 +548,13 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; config.k_chassis_vel_ = init_config.k_chassis_vel_; + config.delay = init_config.delay; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, .pitch_k_v_ = config.pitch_k_v_, - .k_chassis_vel_ = config.k_chassis_vel_ }; + .k_chassis_vel_ = config.k_chassis_vel_, + .delay = config.delay }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From de6d6d74d4142b3471b5343059be947515035bae Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 5 Apr 2024 04:27:56 +0800 Subject: [PATCH 18/51] Delete switching armor target when in low v_yaw. --- rm_gimbal_controllers/src/bullet_solver.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index e69cce55..057c7651 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -135,8 +135,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && track_target_; - if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) + bool is_low_speed = std::abs(v_yaw) < 1.0; + if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && + !is_low_speed) { if (state_changed_) { From 5beff481f2aa92b805c61370f68fcd064f6063aa Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 5 Apr 2024 04:39:45 +0800 Subject: [PATCH 19/51] Delete delay for moving. --- rm_gimbal_controllers/src/gimbal_base.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 9967c3a4..758b8e24 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -285,12 +285,9 @@ void Controller::track(const ros::Time& time) ROS_WARN("%s", ex.what()); } double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec() + config_.delay); - target_pos.x += - target_vel.x * ((time - data_track_.header.stamp).toSec() + config_.delay) - odom2pitch_.transform.translation.x; - target_pos.y += - target_vel.y * ((time - data_track_.header.stamp).toSec() + config_.delay) - odom2pitch_.transform.translation.y; - target_pos.z += - target_vel.z * ((time - data_track_.header.stamp).toSec() + config_.delay) - odom2pitch_.transform.translation.z; + target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; + target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; + target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); target_vel.z -= chassis_vel_->linear_->z(); From d7c230602bc2130d482ade5a655a6513b3e2559b Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 6 Apr 2024 15:37:34 +0800 Subject: [PATCH 20/51] Delete pitch feedforward. --- rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 758b8e24..efd727ad 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -483,7 +483,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); +// ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) From f7a8fe510a4b8c12a936fe923ced2b93f0f19546 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 12:53:25 +0800 Subject: [PATCH 21/51] Delete friction feedforward. --- .../include/rm_gimbal_controllers/gimbal_base.h | 4 ---- rm_gimbal_controllers/src/gimbal_base.cpp | 7 +------ 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 9b79c985..8d083633 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -192,10 +192,6 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index efd727ad..ad3dace4 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -62,11 +62,6 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro gravity_ = enable_feedforward ? (double)xml_rpc_value["gravity"] : 0.; enable_gravity_compensation_ = enable_feedforward && (bool)xml_rpc_value["enable_gravity_compensation"]; - ros::NodeHandle resistance_compensation_nh(controller_nh, "yaw/resistance_compensation"); - yaw_resistance_ = getParam(resistance_compensation_nh, "resistance", 0.); - velocity_saturation_point_ = getParam(resistance_compensation_nh, "velocity_saturation_point", 0.); - effort_saturation_point_ = getParam(resistance_compensation_nh, "effort_saturation_point", 0.); - ros::NodeHandle chassis_vel_nh(controller_nh, "chassis_vel"); chassis_vel_ = std::make_shared(chassis_vel_nh); ros::NodeHandle nh_bullet_solver = ros::NodeHandle(controller_nh, "bullet_solver"); @@ -483,7 +478,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); -// ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); + // ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) From 4088582c2f2e42a21bfaae0c74c7fc09659326bc Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 13:06:19 +0800 Subject: [PATCH 22/51] Modify namespace. --- rm_gimbal_controllers/src/gimbal_base.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index ad3dace4..c91a304d 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -69,8 +69,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh_yaw = ros::NodeHandle(controller_nh, "yaw"); ros::NodeHandle nh_pitch = ros::NodeHandle(controller_nh, "pitch"); - ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "pid_yaw_pos"); - ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pid_pitch_pos"); + ros::NodeHandle nh_pid_yaw_pos = ros::NodeHandle(controller_nh, "yaw/pid_pos"); + ros::NodeHandle nh_pid_pitch_pos = ros::NodeHandle(controller_nh, "pitch/pid_pos"); config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.), .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.), @@ -138,10 +138,10 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - pid_yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher( - nh_pid_yaw_pos, "pid_yaw_pos_state", 1)); - pid_pitch_pos_state_pub_.reset(new realtime_tools::RealtimePublisher( - nh_pid_pitch_pos, "pid_pitch_pos_state", 1)); + pid_yaw_pos_state_pub_.reset( + new realtime_tools::RealtimePublisher(nh_yaw, "pos_state", 1)); + pid_pitch_pos_state_pub_.reset( + new realtime_tools::RealtimePublisher(nh_pitch, "pos_state", 1)); return true; } From 889ffecf791a2d2d7c708b1d4436424c28366d63 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 13:47:01 +0800 Subject: [PATCH 23/51] Modify some variable and function names. --- .../rm_gimbal_controllers/bullet_solver.h | 9 +++--- rm_gimbal_controllers/src/bullet_solver.cpp | 28 +++++++++---------- rm_gimbal_controllers/src/gimbal_base.cpp | 2 +- 3 files changed, 20 insertions(+), 19 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 fcfe65b1..97d1df01 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -49,6 +49,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -79,16 +80,16 @@ class BulletSolver void getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, geometry_msgs::Vector3& armor_vel, geometry_msgs::Point pos, geometry_msgs::Vector3 vel, double yaw, double v_yaw, double r1, double r2, double dz, int armors_num); - void IgnoreErrorToShoot(const ros::Time& time); + void judgeShootBeforehand(const ros::Time& time); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); - void IsVisionTargetChangedCallback(const std_msgs::Bool data); + void identifiedTargetChangeCB(const std_msgs::Bool data); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> control_fire_near_switching_pub_; + std::shared_ptr> shoot_beforehand_cmd_pub_; ros::Subscriber vision_target_changed_sub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; @@ -97,7 +98,7 @@ class BulletSolver bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; - double is_shoot_ignore_error_{}; + int shoot_beforehand_cmd_{}; ros::Time switch_angle_time_{}; int selected_armor_; bool track_target_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 057c7651..7ce9f7e2 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -84,11 +84,11 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - control_fire_near_switching_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); + shoot_beforehand_cmd_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); - vision_target_changed_sub_ = controller_nh.subscribe( - "/armor_processor/change", 10, &BulletSolver::IsVisionTargetChangedCallback, this); + vision_target_changed_sub_ = controller_nh.subscribe("/armor_processor/change", 10, + &BulletSolver::identifiedTargetChangeCB, this); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -323,27 +323,27 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec return error; } -void BulletSolver::IsVisionTargetChangedCallback(const std_msgs::Bool data) +void BulletSolver::identifiedTargetChangeCB(const std_msgs::Bool data) { if (data.data) state_changed_ = true; } -void BulletSolver::IgnoreErrorToShoot(const ros::Time& time) +void BulletSolver::judgeShootBeforehand(const ros::Time& time) { if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_interrupt_).toSec()) - is_shoot_ignore_error_ = -1.0; + shoot_beforehand_cmd_ = -1; else if (is_in_delay_before_switch_ && selected_armor_ == 0) - is_shoot_ignore_error_ = 0.0; + shoot_beforehand_cmd_ = 0; else if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_over_).toSec()) - is_shoot_ignore_error_ = 2.; + shoot_beforehand_cmd_ = 2.; else - is_shoot_ignore_error_ = 1.; - if (control_fire_near_switching_pub_->trylock()) + shoot_beforehand_cmd_ = 1.; + if (shoot_beforehand_cmd_pub_->trylock()) { - control_fire_near_switching_pub_->msg_.stamp = time; - control_fire_near_switching_pub_->msg_.error = is_shoot_ignore_error_; - control_fire_near_switching_pub_->unlockAndPublish(); + shoot_beforehand_cmd_pub_->msg_.stamp = time; + shoot_beforehand_cmd_pub_->msg_.cmd = shoot_beforehand_cmd_; + shoot_beforehand_cmd_pub_->unlockAndPublish(); } } diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index c91a304d..cc032dca 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -289,7 +289,7 @@ void Controller::track(const ros::Time& time) bool solve_success = bullet_solver_->solve(target_pos, target_vel, cmd_gimbal_.bullet_speed, yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); - bullet_solver_->IgnoreErrorToShoot(time); + bullet_solver_->judgeShootBeforehand(time); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) { From ccbf157975fbf1f493a74262f37cc59a1007f247 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 19:04:32 +0800 Subject: [PATCH 24/51] Modify variable name. --- .../rm_gimbal_controllers/bullet_solver.h | 10 +++++----- rm_gimbal_controllers/src/bullet_solver.cpp | 20 +++++++++---------- 2 files changed, 15 insertions(+), 15 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 97d1df01..5505d490 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -82,7 +82,7 @@ class BulletSolver double r1, double r2, double dz, int armors_num); void judgeShootBeforehand(const ros::Time& time); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); - void identifiedTargetChangeCB(const std_msgs::Bool data); + void identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -90,20 +90,20 @@ class BulletSolver std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; std::shared_ptr> shoot_beforehand_cmd_pub_; - ros::Subscriber vision_target_changed_sub_; + ros::Subscriber identified_target_change_sub_; + ros::Time switch_armor_time_{}; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; double max_track_target_vel_; - bool dynamic_reconfig_initialized_{}; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; int shoot_beforehand_cmd_{}; - ros::Time switch_angle_time_{}; int selected_armor_; bool track_target_; - bool state_changed_ = true; + bool identified_target_change_ = true; bool is_in_delay_before_switch_{}; + bool dynamic_reconfig_initialized_{}; geometry_msgs::Point target_pos_{}; double fly_time_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 7ce9f7e2..52aec52d 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -87,8 +87,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) shoot_beforehand_cmd_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); - vision_target_changed_sub_ = controller_nh.subscribe("/armor_processor/change", 10, - &BulletSolver::identifiedTargetChangeCB, this); + identified_target_change_sub_ = controller_nh.subscribe( + "/armor_processor/change", 10, &BulletSolver::identifiedTargetChangeCB, this); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -140,10 +140,10 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && !is_low_speed) { - if (state_changed_) + if (identified_target_change_) { - state_changed_ = false; - switch_angle_time_ = ros::Time::now(); + identified_target_change_ = false; + switch_armor_time_ = ros::Time::now(); } selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; @@ -323,19 +323,19 @@ double BulletSolver::getGimbalError(geometry_msgs::Point pos, geometry_msgs::Vec return error; } -void BulletSolver::identifiedTargetChangeCB(const std_msgs::Bool data) +void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) { - if (data.data) - state_changed_ = true; + if (msg->data) + identified_target_change_ = true; } void BulletSolver::judgeShootBeforehand(const ros::Time& time) { - if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_interrupt_).toSec()) + if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.time_interrupt_).toSec()) shoot_beforehand_cmd_ = -1; else if (is_in_delay_before_switch_ && selected_armor_ == 0) shoot_beforehand_cmd_ = 0; - else if ((ros::Time::now() - switch_angle_time_).toSec() < ros::Duration(config_.time_over_).toSec()) + else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.time_over_).toSec()) shoot_beforehand_cmd_ = 2.; else shoot_beforehand_cmd_ = 1.; From 5eb12e1d8158adb31eb5db06e0577beba85c672b Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 19:11:14 +0800 Subject: [PATCH 25/51] Modify expression of shoot beforehand cmd. --- rm_gimbal_controllers/src/bullet_solver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 52aec52d..2213aff4 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -332,13 +332,13 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) void BulletSolver::judgeShootBeforehand(const ros::Time& time) { if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.time_interrupt_).toSec()) - shoot_beforehand_cmd_ = -1; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; else if (is_in_delay_before_switch_ && selected_armor_ == 0) - shoot_beforehand_cmd_ = 0; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.time_over_).toSec()) - shoot_beforehand_cmd_ = 2.; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; else - shoot_beforehand_cmd_ = 1.; + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; if (shoot_beforehand_cmd_pub_->trylock()) { shoot_beforehand_cmd_pub_->msg_.stamp = time; From 96cb624c3da5e7bcfcca1998aa616195e4ff3bb2 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 19:24:55 +0800 Subject: [PATCH 26/51] Modify variable name. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 4 ++-- .../rm_gimbal_controllers/bullet_solver.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 16 ++++++++-------- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 68b5e7f4..1cfec7e3 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -14,8 +14,8 @@ 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("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("time_interrupt_", double_t, 0, "The interrupt of the switching time", 0.0, 0.0, 2.0) -gen.add("time_over_", double_t, 0, "The over if oversight", 0.0, 0.0, 2.0) +gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0) +gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.0) gen.add("angle1", double_t, 0, "Switch angle1", 40.0, 0.0, 90.0) gen.add("angle2", double_t, 0, "Switch angle2", 2.0, 0.0, 90.0) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 5505d490..261151b9 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -56,7 +56,7 @@ 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, dt, timeout, time_interrupt_, time_over_, angle1, angle2; + resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, angle1, angle2; }; class BulletSolver diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 2213aff4..e4057828 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -53,8 +53,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .delay = getParam(controller_nh, "delay", 0.), .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.), - .time_interrupt_ = getParam(controller_nh, "time_interrupt", 0.0), - .time_over_ = getParam(controller_nh, "time_over", 0.0), + .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), + .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0), .angle1 = getParam(controller_nh, "angle1", 40.0), .angle2 = getParam(controller_nh, "angle2", 2.0) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); @@ -331,11 +331,11 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) void BulletSolver::judgeShootBeforehand(const ros::Time& time) { - if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.time_interrupt_).toSec()) + if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec()) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; else if (is_in_delay_before_switch_ && selected_armor_ == 0) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.time_over_).toSec()) + else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec()) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; else shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; @@ -362,8 +362,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.delay = init_config.delay; config.dt = init_config.dt; config.timeout = init_config.timeout; - config.time_interrupt_ = init_config.time_interrupt_; - config.time_over_ = init_config.time_over_; + config.ban_shoot_duration = init_config.ban_shoot_duration; + config.gimbal_switch_duration = init_config.gimbal_switch_duration; config.angle1 = init_config.angle1; config.angle2 = init_config.angle2; dynamic_reconfig_initialized_ = true; @@ -377,8 +377,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .delay = config.delay, .dt = config.dt, .timeout = config.timeout, - .time_interrupt_ = config.time_interrupt_, - .time_over_ = config.time_over_, + .ban_shoot_duration = config.ban_shoot_duration, + .gimbal_switch_duration = config.gimbal_switch_duration, .angle1 = config.angle1, .angle2 = config.angle2 }; config_rt_buffer_.writeFromNonRT(config_non_rt); From 393241c27e922c38e029ba694d9978cb7210a165 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 19:35:58 +0800 Subject: [PATCH 27/51] Publish fly time. --- .../include/rm_gimbal_controllers/bullet_solver.h | 3 ++- rm_gimbal_controllers/src/bullet_solver.cpp | 6 ++++++ 2 files changed, 8 insertions(+), 1 deletion(-) 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 261151b9..33641784 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include #include @@ -90,6 +90,7 @@ class BulletSolver std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; std::shared_ptr> shoot_beforehand_cmd_pub_; + std::shared_ptr> fly_time_pub_; ros::Subscriber identified_target_change_sub_; ros::Time switch_armor_time_{}; realtime_tools::RealtimeBuffer config_rt_buffer_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index e4057828..b9226e5c 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -86,6 +86,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); shoot_beforehand_cmd_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); + fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); identified_target_change_sub_ = controller_nh.subscribe( "/armor_processor/change", 10, &BulletSolver::identifiedTargetChangeCB, this); @@ -202,6 +203,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d if (count >= 20 || std::isnan(error)) return false; } + if (fly_time_pub_->trylock()) + { + fly_time_pub_->msg_.data = fly_time_; + fly_time_pub_->unlockAndPublish(); + } return true; } From 440aa047fdef6093c9c3643b81f3cd352ce760c1 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 19:42:28 +0800 Subject: [PATCH 28/51] Modify description of variable in cfg. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 916f7175..af09b48f 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -8,6 +8,6 @@ gen = ParameterGenerator() gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0) gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0) -gen.add("delay", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, -1.0, 1.0) +gen.add("delay", double_t, 0, "Delay used to estimate current armor's yaw", 0.0, -1.0, 1.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) From 34e03f598dd2d2cedef36c9708a45d9b369ecda8 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Fri, 12 Apr 2024 20:21:07 +0800 Subject: [PATCH 29/51] Only use target vel for feedforward and enable gravity feedforward only when hitting buff. --- rm_gimbal_controllers/src/gimbal_base.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index cc032dca..2229e3a9 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -264,7 +264,9 @@ void Controller::track(const ros::Time& time) double yaw_compute = yaw_real; double pitch_compute = -pitch_real; geometry_msgs::Point target_pos = data_track_.position; - geometry_msgs::Vector3 target_vel = data_track_.velocity; + geometry_msgs::Vector3 target_vel{}; + if (data_track_.id != 12) + target_vel = data_track_.velocity; try { if (!data_track_.header.frame_id.empty()) @@ -478,7 +480,8 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); - // ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); + if (data_track_.id == 12) + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) From c20dfec0c6cf947d54f088f625d91ba463770058 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 16 Apr 2024 22:31:35 +0800 Subject: [PATCH 30/51] Thinking the situation of tracking center. --- rm_gimbal_controllers/src/bullet_solver.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index b9226e5c..97cec992 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -133,9 +133,8 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (-acos(r / target_rho) + (angle1 + angle2)) * std::abs(v_yaw) / max_track_target_vel_ : angle2; is_in_delay_before_switch_ = - ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && - track_target_; + (((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.); bool is_low_speed = std::abs(v_yaw) < 1.0; if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && @@ -337,7 +336,9 @@ void BulletSolver::identifiedTargetChangeCB(const std_msgs::BoolConstPtr& msg) void BulletSolver::judgeShootBeforehand(const ros::Time& time) { - if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec()) + if (!track_target_) + shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; + else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec()) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; else if (is_in_delay_before_switch_ && selected_armor_ == 0) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; From 27ed0d9dd792ee7604521c9dd30884e230d81a9b Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 16 Apr 2024 22:42:02 +0800 Subject: [PATCH 31/51] Add delay2 used to estimate current armor's position when moving. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 1 + .../include/rm_gimbal_controllers/gimbal_base.h | 2 +- rm_gimbal_controllers/src/gimbal_base.cpp | 13 +++++++++---- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index af09b48f..1fbf7f99 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -9,5 +9,6 @@ gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0) gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0) gen.add("delay", double_t, 0, "Delay used to estimate current armor's yaw", 0.0, -1.0, 1.0) +gen.add("delay2", double_t, 0, "Delay2 used to estimate current armor's position when moving", 0.0, -1.0, 1.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) 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 8d083633..ce8449fb 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -63,7 +63,7 @@ struct GimbalConfig { // feedforward double yaw_k_v_, pitch_k_v_, k_chassis_vel_; - double delay; + double delay, delay2; }; class ChassisVel diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 2229e3a9..bb942071 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -75,7 +75,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.), .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.), .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.), - .delay = getParam(controller_nh, "delay", 0.) }; + .delay = getParam(controller_nh, "delay", 0.), + .delay2 = getParam(controller_nh, "delay2", 0.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -282,8 +283,10 @@ void Controller::track(const ros::Time& time) ROS_WARN("%s", ex.what()); } double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec() + config_.delay); - target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; - target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; + target_pos.x += + target_vel.x * ((time - data_track_.header.stamp).toSec() + config_.delay2) - odom2pitch_.transform.translation.x; + target_pos.y += + target_vel.y * ((time - data_track_.header.stamp).toSec() + config_.delay2) - odom2pitch_.transform.translation.y; target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); @@ -544,12 +547,14 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin config.pitch_k_v_ = init_config.pitch_k_v_; config.k_chassis_vel_ = init_config.k_chassis_vel_; config.delay = init_config.delay; + config.delay2 = init_config.delay2; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, .pitch_k_v_ = config.pitch_k_v_, .k_chassis_vel_ = config.k_chassis_vel_, - .delay = config.delay }; + .delay = config.delay, + .delay2 = config.delay2 }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From 6c6c251948076bf4e73c8d97c8af8c3bdaca6127 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 24 Apr 2024 01:39:51 +0800 Subject: [PATCH 32/51] Make sure switch angle when facing different v_yaw's target. --- .../rm_gimbal_controllers/bullet_solver.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 20 ++++++------------- 2 files changed, 7 insertions(+), 15 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 33641784..94ae0527 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -56,7 +56,7 @@ 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, dt, timeout, ban_shoot_duration, gimbal_switch_duration, angle1, angle2; + resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration; }; class BulletSolver diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 97cec992..899dfdfc 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -54,9 +54,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.), .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), - .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0), - .angle1 = getParam(controller_nh, "angle1", 40.0), - .angle2 = getParam(controller_nh, "angle2", 2.0) }; + .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -125,13 +123,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; - double angle1 = config_.angle1 / 180 * M_PI; - double angle2 = config_.angle2 / 180 * M_PI; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = - track_target_ ? acos(r / target_rho) - angle1 + - (-acos(r / target_rho) + (angle1 + angle2)) * std::abs(v_yaw) / max_track_target_vel_ : - angle2; + double switch_armor_angle = track_target_ ? acos(r / target_rho) - 40 / 180 * M_PI + + (-acos(r / target_rho) + (40 / 180 * M_PI + 2 / 180 * M_PI)) * + std::abs(v_yaw) / max_track_target_vel_ : + 2 / 180 * M_PI; is_in_delay_before_switch_ = (((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.); @@ -371,8 +367,6 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.timeout = init_config.timeout; config.ban_shoot_duration = init_config.ban_shoot_duration; config.gimbal_switch_duration = init_config.gimbal_switch_duration; - config.angle1 = init_config.angle1; - config.angle2 = init_config.angle2; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -385,9 +379,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .dt = config.dt, .timeout = config.timeout, .ban_shoot_duration = config.ban_shoot_duration, - .gimbal_switch_duration = config.gimbal_switch_duration, - .angle1 = config.angle1, - .angle2 = config.angle2 }; + .gimbal_switch_duration = config.gimbal_switch_duration }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers From efb566f3a99678c416977d1f8ade41e6127b9c54 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 24 Apr 2024 01:49:46 +0800 Subject: [PATCH 33/51] Add GimbalBase.cfg in cmakelist. --- rm_gimbal_controllers/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index 98732896..995f5af7 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(catkin REQUIRED generate_dynamic_reconfigure_options( cfg/BulletSolver.cfg + cfg/GimbalBase.cfg ) ################################### From 56e9831f52365b58fdf9f1b73d91d6b2cb1c8ba3 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 27 Apr 2024 21:55:11 +0800 Subject: [PATCH 34/51] Add ramp filter for rate_yaw and rate_pitch. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 2 + .../rm_gimbal_controllers/bullet_solver.h | 2 +- .../rm_gimbal_controllers/gimbal_base.h | 8 ++- rm_gimbal_controllers/src/bullet_solver.cpp | 25 +++++--- rm_gimbal_controllers/src/gimbal_base.cpp | 59 +++++++++++-------- 5 files changed, 61 insertions(+), 35 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index 1fbf7f99..fb77480a 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -7,6 +7,8 @@ gen = ParameterGenerator() gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0) gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) +gen.add("accel_yaw_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 999.0) +gen.add("accel_pitch_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 999.0) gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0) gen.add("delay", double_t, 0, "Delay used to estimate current armor's yaw", 0.0, -1.0, 1.0) gen.add("delay2", double_t, 0, "Delay2 used to estimate current armor's position when moving", 0.0, -1.0, 1.0) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 94ae0527..33641784 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -56,7 +56,7 @@ 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, dt, timeout, ban_shoot_duration, gimbal_switch_duration; + resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, angle1, angle2; }; class BulletSolver 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 ce8449fb..89d8b5d2 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -61,8 +62,8 @@ namespace rm_gimbal_controllers { struct GimbalConfig { - // feedforward double yaw_k_v_, pitch_k_v_, k_chassis_vel_; + double accel_pitch_{}, accel_yaw_{}; double delay, delay2; }; @@ -168,7 +169,7 @@ class Controller : public controller_interface::MultiInterfaceController> pid_yaw_pos_state_pub_, + std::unique_ptr> pid_yaw_pos_state_pub_, pid_pitch_pos_state_pub_; std::shared_ptr> error_pub_; ros::Subscriber cmd_gimbal_sub_; @@ -182,6 +183,7 @@ class Controller : public controller_interface::MultiInterfaceController config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; + RampFilter*ramp_rate_pitch_{}, *ramp_rate_yaw_{}; + enum { RATE, diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 899dfdfc..abcd15b0 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -54,7 +54,9 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .dt = getParam(controller_nh, "dt", 0.), .timeout = getParam(controller_nh, "timeout", 0.), .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), - .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0) }; + .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0), + .angle1 = getParam(controller_nh, "angle1", 40.0), + .angle2 = getParam(controller_nh, "angle2", 2.0) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -123,14 +125,17 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; + double angle1 = config_.angle1 / 180 * M_PI; + double angle2 = config_.angle2 / 180 * M_PI; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = track_target_ ? acos(r / target_rho) - 40 / 180 * M_PI + - (-acos(r / target_rho) + (40 / 180 * M_PI + 2 / 180 * M_PI)) * - std::abs(v_yaw) / max_track_target_vel_ : - 2 / 180 * M_PI; + double switch_armor_angle = + track_target_ ? acos(r / target_rho) - angle1 + + (-acos(r / target_rho) + (angle1 + angle2)) * std::abs(v_yaw) / max_track_target_vel_ : + angle2; is_in_delay_before_switch_ = - (((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.); + ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || + (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && + track_target_; bool is_low_speed = std::abs(v_yaw) < 1.0; if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && @@ -367,6 +372,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.timeout = init_config.timeout; config.ban_shoot_duration = init_config.ban_shoot_duration; config.gimbal_switch_duration = init_config.gimbal_switch_duration; + config.angle1 = init_config.angle1; + config.angle2 = init_config.angle2; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -379,7 +386,9 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .dt = config.dt, .timeout = config.timeout, .ban_shoot_duration = config.ban_shoot_duration, - .gimbal_switch_duration = config.gimbal_switch_duration }; + .gimbal_switch_duration = config.gimbal_switch_duration, + .angle1 = config.angle1, + .angle2 = config.angle2 }; 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 bb942071..4e742e3b 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -75,8 +75,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro config_ = { .yaw_k_v_ = getParam(nh_yaw, "k_v", 0.), .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.), .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.), - .delay = getParam(controller_nh, "delay", 0.), - .delay2 = getParam(controller_nh, "delay2", 0.) }; + .accel_pitch_ = getParam(controller_nh, "pitch/accel", 99.), + .accel_yaw_ = getParam(controller_nh, "yaw/accel", 99.), + .delay = getParam(controller_nh, "delay", 0.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -139,10 +140,13 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - pid_yaw_pos_state_pub_.reset( - new realtime_tools::RealtimePublisher(nh_yaw, "pos_state", 1)); + pid_yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher(nh_yaw, "pos_state", 1)); pid_pitch_pos_state_pub_.reset( - new realtime_tools::RealtimePublisher(nh_pitch, "pos_state", 1)); + new realtime_tools::RealtimePublisher(nh_pitch, "pos_state", 1)); + + ramp_rate_pitch_ = new RampFilter(0, 0.001); + ramp_rate_yaw_ = new RampFilter(0, 0.001); + return true; } @@ -157,6 +161,12 @@ 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(); + ramp_rate_pitch_->setAcc(config_.accel_pitch_); + ramp_rate_yaw_->setAcc(config_.accel_yaw_); + ramp_rate_pitch_->input(cmd_gimbal_.rate_pitch); + ramp_rate_yaw_->input(cmd_gimbal_.rate_yaw); + cmd_gimbal_.rate_pitch = ramp_rate_pitch_->output(); + cmd_gimbal_.rate_yaw = ramp_rate_yaw_->output(); try { odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time); @@ -199,7 +209,8 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(base2gimbal_des), roll_temp, base2gimbal_current_des_pitch, base2gimbal_current_des_yaw); double pitch_real_des, yaw_real_des; - if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, pitch_joint_urdf_)) + pitch_des_in_limit = setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, pitch_joint_urdf_); + if (!pitch_des_in_limit) { double yaw_temp; tf2::Quaternion base2new_des; @@ -215,7 +226,8 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(odom2base * base2new_des), roll_temp, pitch_real_des, yaw_temp); } - if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, yaw_joint_urdf_)) + yaw_des_in_limit_ = setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, yaw_joint_urdf_); + if (!yaw_des_in_limit_) { double pitch_temp; tf2::Quaternion base2new_des; @@ -436,6 +448,13 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ROS_WARN("%s", ex.what()); } } + if (!pitch_des_in_limit) + pitch_vel_des = 0.; + if (!yaw_des_in_limit_) + yaw_vel_des = 0.; + + pid_pitch_pos_.computeCommand(pitch_angle_error, period); + pid_yaw_pos_.computeCommand(yaw_angle_error, period); // publish state if (loop_count_ % 10 == 0) @@ -444,33 +463,20 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) { pid_yaw_pos_state_pub_->msg_.header.stamp = time; pid_yaw_pos_state_pub_->msg_.set_point = yaw_des; + pid_yaw_pos_state_pub_->msg_.set_point_dot = yaw_vel_des; pid_yaw_pos_state_pub_->msg_.process_value = yaw_real; - pid_yaw_pos_state_pub_->msg_.process_value_dot = ctrl_yaw_.joint_.getVelocity(); - pid_yaw_pos_state_pub_->msg_.error = yaw_angle_error; - pid_yaw_pos_state_pub_->msg_.time_step = period.toSec(); + pid_yaw_pos_state_pub_->msg_.error = angles::shortest_angular_distance(yaw_real, yaw_des); pid_yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); - double dummy; - bool antiwindup; - pid_yaw_pos_.getGains(pid_yaw_pos_state_pub_->msg_.p, pid_yaw_pos_state_pub_->msg_.i, - pid_yaw_pos_state_pub_->msg_.d, pid_yaw_pos_state_pub_->msg_.i_clamp, dummy, antiwindup); - pid_yaw_pos_state_pub_->msg_.antiwindup = static_cast(antiwindup); pid_yaw_pos_state_pub_->unlockAndPublish(); } if (pid_pitch_pos_state_pub_ && pid_pitch_pos_state_pub_->trylock()) { pid_pitch_pos_state_pub_->msg_.header.stamp = time; pid_pitch_pos_state_pub_->msg_.set_point = pitch_des; + pid_pitch_pos_state_pub_->msg_.set_point_dot = pitch_vel_des; pid_pitch_pos_state_pub_->msg_.process_value = pitch_real; - pid_pitch_pos_state_pub_->msg_.process_value_dot = ctrl_pitch_.joint_.getVelocity(); - pid_pitch_pos_state_pub_->msg_.error = pitch_angle_error; - pid_pitch_pos_state_pub_->msg_.time_step = period.toSec(); + pid_pitch_pos_state_pub_->msg_.error = angles::shortest_angular_distance(pitch_real, pitch_des); pid_pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); - double dummy; - bool antiwindup; - pid_pitch_pos_.getGains(pid_pitch_pos_state_pub_->msg_.p, pid_pitch_pos_state_pub_->msg_.i, - pid_pitch_pos_state_pub_->msg_.d, pid_pitch_pos_state_pub_->msg_.i_clamp, dummy, - antiwindup); - pid_pitch_pos_state_pub_->msg_.antiwindup = static_cast(antiwindup); pid_pitch_pos_state_pub_->unlockAndPublish(); } } @@ -481,6 +487,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); + ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); if (data_track_.id == 12) @@ -546,6 +553,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin config.yaw_k_v_ = init_config.yaw_k_v_; config.pitch_k_v_ = init_config.pitch_k_v_; config.k_chassis_vel_ = init_config.k_chassis_vel_; + config.accel_pitch_ = init_config.accel_pitch_; + config.accel_yaw_ = init_config.accel_yaw_; config.delay = init_config.delay; config.delay2 = init_config.delay2; dynamic_reconfig_initialized_ = true; @@ -553,6 +562,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, .pitch_k_v_ = config.pitch_k_v_, .k_chassis_vel_ = config.k_chassis_vel_, + .accel_pitch_ = config.accel_pitch_, + .accel_yaw_ = config.accel_yaw_, .delay = config.delay, .delay2 = config.delay2 }; config_rt_buffer_.writeFromNonRT(config_non_rt); From a4cdb9906938900151d10ae85972c0e556fd5fac Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 27 Apr 2024 22:37:23 +0800 Subject: [PATCH 35/51] Add count to judge whether switch target. --- .../include/rm_gimbal_controllers/bullet_solver.h | 2 ++ rm_gimbal_controllers/src/bullet_solver.cpp | 14 ++++++++++---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 33641784..7ce514a6 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -101,6 +101,8 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; int shoot_beforehand_cmd_{}; int selected_armor_; + int count_; + int min_fit_switch_count_; bool track_target_; bool identified_target_change_ = true; bool is_in_delay_before_switch_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index abcd15b0..1b2e3bca 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -141,14 +141,20 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && !is_low_speed) { + count_++; if (identified_target_change_) { + count_ = 0; identified_target_change_ = false; - 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_ >= min_fit_switch_count_) + { + if (count_ == 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; + } } int count{}; double error = 999; From b2acbf117534c6b5832f048e34dcabe564ad3360 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 27 Apr 2024 22:46:03 +0800 Subject: [PATCH 36/51] Modify param name. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 4 ++-- .../rm_gimbal_controllers/bullet_solver.h | 3 ++- rm_gimbal_controllers/src/bullet_solver.cpp | 24 +++++++++---------- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 1cfec7e3..0a665a91 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -16,7 +16,7 @@ gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0) gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.0) -gen.add("angle1", double_t, 0, "Switch angle1", 40.0, 0.0, 90.0) -gen.add("angle2", double_t, 0, "Switch angle2", 2.0, 0.0, 90.0) +gen.add("max_switch_angle_", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) +gen.add("min_switch_angle_", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) 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 7ce514a6..eb71db45 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -56,7 +56,8 @@ namespace rm_gimbal_controllers struct Config { double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, angle1, angle2; + resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle_, + min_switch_angle_; }; class BulletSolver diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 1b2e3bca..e14503fb 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -55,8 +55,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .timeout = getParam(controller_nh, "timeout", 0.), .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0), - .angle1 = getParam(controller_nh, "angle1", 40.0), - .angle2 = getParam(controller_nh, "angle2", 2.0) }; + .max_switch_angle_ = getParam(controller_nh, "max_switch_angle_", 40.0), + .min_switch_angle_ = getParam(controller_nh, "min_switch_angle_", 2.0) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -125,13 +125,13 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; - double angle1 = config_.angle1 / 180 * M_PI; - double angle2 = config_.angle2 / 180 * M_PI; + double max_switch_angle_ = config_.max_switch_angle_ / 180 * M_PI; + double min_switch_angle_ = config_.min_switch_angle_ / 180 * M_PI; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = - track_target_ ? acos(r / target_rho) - angle1 + - (-acos(r / target_rho) + (angle1 + angle2)) * std::abs(v_yaw) / max_track_target_vel_ : - angle2; + double switch_armor_angle = track_target_ ? acos(r / target_rho) - max_switch_angle_ + + (-acos(r / target_rho) + (max_switch_angle_ + min_switch_angle_)) * + std::abs(v_yaw) / max_track_target_vel_ : + min_switch_angle_; is_in_delay_before_switch_ = ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && @@ -378,8 +378,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.timeout = init_config.timeout; config.ban_shoot_duration = init_config.ban_shoot_duration; config.gimbal_switch_duration = init_config.gimbal_switch_duration; - config.angle1 = init_config.angle1; - config.angle2 = init_config.angle2; + config.max_switch_angle_ = init_config.max_switch_angle_; + config.min_switch_angle_ = init_config.min_switch_angle_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -393,8 +393,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .timeout = config.timeout, .ban_shoot_duration = config.ban_shoot_duration, .gimbal_switch_duration = config.gimbal_switch_duration, - .angle1 = config.angle1, - .angle2 = config.angle2 }; + .max_switch_angle_ = config.max_switch_angle_, + .min_switch_angle_ = config.min_switch_angle_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers From 45ddcc427372fcc39f326d6b2a8c34c73ebd6218 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sat, 27 Apr 2024 23:00:59 +0800 Subject: [PATCH 37/51] Modify param name. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 4 ++-- .../rm_gimbal_controllers/gimbal_base.h | 2 +- rm_gimbal_controllers/src/gimbal_base.cpp | 22 ++++++++++--------- 3 files changed, 15 insertions(+), 13 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index fb77480a..db23eecf 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -10,7 +10,7 @@ gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) gen.add("accel_yaw_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 999.0) gen.add("accel_pitch_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 999.0) gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0) -gen.add("delay", double_t, 0, "Delay used to estimate current armor's yaw", 0.0, -1.0, 1.0) -gen.add("delay2", double_t, 0, "Delay2 used to estimate current armor's position when moving", 0.0, -1.0, 1.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) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) 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 89d8b5d2..56062f4d 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -64,7 +64,7 @@ struct GimbalConfig { double yaw_k_v_, pitch_k_v_, k_chassis_vel_; double accel_pitch_{}, accel_yaw_{}; - double delay, delay2; + double track_rotate_target_delay, track_move_target_delay; }; class ChassisVel diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 4e742e3b..71c0c043 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -77,7 +77,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.), .accel_pitch_ = getParam(controller_nh, "pitch/accel", 99.), .accel_yaw_ = getParam(controller_nh, "yaw/accel", 99.), - .delay = getParam(controller_nh, "delay", 0.) }; + .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), + .track_move_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -294,11 +295,12 @@ void Controller::track(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec() + config_.delay); - target_pos.x += - target_vel.x * ((time - data_track_.header.stamp).toSec() + config_.delay2) - odom2pitch_.transform.translation.x; - target_pos.y += - target_vel.y * ((time - data_track_.header.stamp).toSec() + config_.delay2) - odom2pitch_.transform.translation.y; + double yaw = data_track_.yaw + + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec() + config_.track_rotate_target_delay); + target_pos.x += target_vel.x * ((time - data_track_.header.stamp).toSec() + config_.track_move_target_delay) - + odom2pitch_.transform.translation.x; + target_pos.y += target_vel.y * ((time - data_track_.header.stamp).toSec() + config_.track_move_target_delay) - + odom2pitch_.transform.translation.y; target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); @@ -555,8 +557,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin config.k_chassis_vel_ = init_config.k_chassis_vel_; config.accel_pitch_ = init_config.accel_pitch_; config.accel_yaw_ = init_config.accel_yaw_; - config.delay = init_config.delay; - config.delay2 = init_config.delay2; + config.track_rotate_target_delay = init_config.track_rotate_target_delay; + config.track_move_target_delay = init_config.track_move_target_delay; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, @@ -564,8 +566,8 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin .k_chassis_vel_ = config.k_chassis_vel_, .accel_pitch_ = config.accel_pitch_, .accel_yaw_ = config.accel_yaw_, - .delay = config.delay, - .delay2 = config.delay2 }; + .track_rotate_target_delay = config.track_rotate_target_delay, + .track_move_target_delay = config.track_move_target_delay }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From 53b86d3604f6038531b34966bb455dd39d991780 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sun, 28 Apr 2024 12:45:14 +0800 Subject: [PATCH 38/51] Delete lp filter which used to estimate chassis angular vel. --- .../include/rm_gimbal_controllers/gimbal_base.h | 6 ------ rm_gimbal_controllers/src/gimbal_base.cpp | 3 +-- 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 56062f4d..3753a8c4 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -56,7 +56,6 @@ #include #include #include -#include namespace rm_gimbal_controllers { @@ -77,8 +76,6 @@ class ChassisVel nh.param("debug", is_debug_, true); linear_ = std::make_shared>(num_data); angular_ = std::make_shared>(num_data); - ros::NodeHandle nh_lp = ros::NodeHandle(nh, "lp"); - angular_lp_filter_ = std::make_unique(nh_lp); if (is_debug_) { real_pub_.reset(new realtime_tools::RealtimePublisher(nh, "real", 1)); @@ -87,7 +84,6 @@ class ChassisVel } std::shared_ptr> linear_; std::shared_ptr> angular_; - std::shared_ptr angular_lp_filter_; void update(double linear_vel[3], double angular_vel[3], double period) { if (period < 0) @@ -96,11 +92,9 @@ class ChassisVel { linear_->clear(); angular_->clear(); - angular_lp_filter_->reset(); } linear_->input(linear_vel); angular_->input(angular_vel); - angular_lp_filter_->input(angular_vel[2]); if (is_debug_ && loop_count_ % 10 == 0) { if (real_pub_->trylock()) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 71c0c043..ec1a50c8 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -484,8 +484,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) } loop_count_++; - ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - - config_.k_chassis_vel_ * chassis_vel_->angular_lp_filter_->output() + + ctrl_yaw_.setCommand(pid_yaw_pos_.getCurrentCmd() - config_.k_chassis_vel_ * chassis_vel_->angular_->z() + config_.yaw_k_v_ * yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); ctrl_pitch_.setCommand(pid_pitch_pos_.getCurrentCmd() + config_.pitch_k_v_ * pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); From f2c7935ff6a60d5b77dbf89a421a0e416fd43ae4 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sun, 28 Apr 2024 14:13:17 +0800 Subject: [PATCH 39/51] Add param to dynamic reconfigure. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 1 + .../include/rm_gimbal_controllers/bullet_solver.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 13 ++++++++----- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 0a665a91..9b8eb3db 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -18,5 +18,6 @@ gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.0) gen.add("max_switch_angle_", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) gen.add("min_switch_angle_", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) +gen.add("min_fit_switch_count_", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) 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 eb71db45..4056632b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -58,6 +58,7 @@ struct Config double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle_, min_switch_angle_; + int min_fit_switch_count_; }; class BulletSolver @@ -103,7 +104,6 @@ class BulletSolver int shoot_beforehand_cmd_{}; int selected_armor_; int count_; - int min_fit_switch_count_; bool track_target_; bool identified_target_change_ = true; bool is_in_delay_before_switch_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index e14503fb..f3644d0c 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -55,8 +55,9 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .timeout = getParam(controller_nh, "timeout", 0.), .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0), - .max_switch_angle_ = getParam(controller_nh, "max_switch_angle_", 40.0), - .min_switch_angle_ = getParam(controller_nh, "min_switch_angle_", 2.0) }; + .max_switch_angle_ = getParam(controller_nh, "max_switch_angle", 40.0), + .min_switch_angle_ = getParam(controller_nh, "min_switch_angle", 2.0), + .min_fit_switch_count_ = getParam(controller_nh, "min_fit_switch_count", 3) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -147,9 +148,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d count_ = 0; identified_target_change_ = false; } - if (count_ >= min_fit_switch_count_) + if (count_ >= config_.min_fit_switch_count_) { - if (count_ == 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; @@ -380,6 +381,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.gimbal_switch_duration = init_config.gimbal_switch_duration; config.max_switch_angle_ = init_config.max_switch_angle_; config.min_switch_angle_ = init_config.min_switch_angle_; + config.min_fit_switch_count_ = init_config.min_fit_switch_count_; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -394,7 +396,8 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .ban_shoot_duration = config.ban_shoot_duration, .gimbal_switch_duration = config.gimbal_switch_duration, .max_switch_angle_ = config.max_switch_angle_, - .min_switch_angle_ = config.min_switch_angle_ }; + .min_switch_angle_ = config.min_switch_angle_, + .min_fit_switch_count_ = config.min_fit_switch_count_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers From 700e6ec8d117af7f649d90b8bcdcb36074b992c2 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sun, 28 Apr 2024 14:49:15 +0800 Subject: [PATCH 40/51] Use base_link2pitch to calculate gravity compensation. --- rm_gimbal_controllers/src/gimbal_base.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index ec1a50c8..3ae44ff9 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -491,15 +491,14 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); - if (data_track_.id == 12) - ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time)); } double Controller::feedForward(const ros::Time& time) { Eigen::Vector3d gravity(0, 0, -gravity_); tf2::doTransform(gravity, gravity, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, "odom", time)); + robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, "base_link", time)); Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z); double feedforward = -mass_origin.cross(gravity).y(); if (enable_gravity_compensation_) From 94488c888f51ae6faaadead608cb60e54ea3b03d Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 29 Apr 2024 20:48:21 +0800 Subject: [PATCH 41/51] Modify topic's name. --- rm_gimbal_controllers/src/bullet_solver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index f3644d0c..cf134216 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -90,7 +90,7 @@ 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( - "/armor_processor/change", 10, &BulletSolver::identifiedTargetChangeCB, this); + "/change", 10, &BulletSolver::identifiedTargetChangeCB, this); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const From 03f03a0933adf7732d764dd9548b1d30941ecb6e Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 3 May 2024 19:16:59 +0800 Subject: [PATCH 42/51] Delete something unused. --- .../include/rm_gimbal_controllers/bullet_solver.h | 1 - 1 file changed, 1 deletion(-) 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 4056632b..81613e15 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include From 2e259a3cf7e92e35f3981a055cb8b5d7295d81ce Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 6 May 2024 01:40:47 +0800 Subject: [PATCH 43/51] Used for vision's topic which is called change. --- .../include/rm_gimbal_controllers/bullet_solver.h | 1 + 1 file changed, 1 insertion(+) 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 81613e15..4056632b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include From caa9190d56e10db5e8f6380354f53ae3c076415a Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 6 May 2024 01:43:40 +0800 Subject: [PATCH 44/51] Delete something unused. --- .../include/rm_gimbal_controllers/gimbal_base.h | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 3753a8c4..abac7561 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -37,7 +37,6 @@ #pragma once -#include #include #include #include From ae0ff13bdfc60ab3ca05f4b65ad6ab27cb567972 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 6 May 2024 18:43:16 +0800 Subject: [PATCH 45/51] Update cfg. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 9 ++++++--- rm_gimbal_controllers/cfg/GimbalBase.cfg | 2 -- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 9b8eb3db..44b1b1e1 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -16,8 +16,11 @@ gen.add("dt", double_t, 0, "Iteration interval", 0.01, 0.0001, 0.1) gen.add("timeout", double_t, 0, "Flight time exceeded", 2.0, 0.5, 3.0) gen.add("ban_shoot_duration", double_t, 0, "Ban shoot duration while beforehand shooting", 0.0, 0.0, 2.0) gen.add("gimbal_switch_duration", double_t, 0, "Gimbal switch duration", 0.0, 0.0, 2.0) -gen.add("max_switch_angle_", double_t, 0, "Max switch angle", 40.0, 0.0, 90.0) -gen.add("min_switch_angle_", double_t, 0, "Min switch angle", 2.0, 0.0, 90.0) -gen.add("min_fit_switch_count_", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) +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("min_fit_switch_count", int_t, 0, "Min count that current angle fit switch angle", 0, 3, 99) +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) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index db23eecf..b0463da0 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -10,7 +10,5 @@ gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) gen.add("accel_yaw_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 999.0) gen.add("accel_pitch_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 999.0) gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.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) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) From e0503cfd7a934b8d2e5c617facefeef60d0e1a3c Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 6 May 2024 18:44:20 +0800 Subject: [PATCH 46/51] Delete something unused and modify param name. --- .../include/rm_gimbal_controllers/gimbal_base.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index abac7561..0a166875 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -62,7 +62,6 @@ struct GimbalConfig { double yaw_k_v_, pitch_k_v_, k_chassis_vel_; double accel_pitch_{}, accel_yaw_{}; - double track_rotate_target_delay, track_move_target_delay; }; class ChassisVel @@ -162,8 +161,7 @@ class Controller : public controller_interface::MultiInterfaceController> pid_yaw_pos_state_pub_, - pid_pitch_pos_state_pub_; + std::unique_ptr> yaw_pos_state_pub_, pitch_pos_state_pub_; std::shared_ptr> error_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; @@ -176,11 +174,11 @@ class Controller : public controller_interface::MultiInterfaceController Date: Mon, 6 May 2024 18:44:57 +0800 Subject: [PATCH 47/51] Add some param and modify param name. --- .../include/rm_gimbal_controllers/bullet_solver.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 4056632b..7c6fc4ce 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -56,9 +56,9 @@ namespace rm_gimbal_controllers struct Config { double resistance_coff_qd_10, resistance_coff_qd_15, resistance_coff_qd_16, resistance_coff_qd_18, - resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle_, - min_switch_angle_; - int min_fit_switch_count_; + resistance_coff_qd_30, g, delay, dt, timeout, ban_shoot_duration, gimbal_switch_duration, max_switch_angle, + min_switch_angle, max_chassis_angular_vel, track_rotate_target_delay, track_move_target_delay; + int min_fit_switch_count; }; class BulletSolver @@ -67,7 +67,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 r1, double r2, double dz, int armors_num, double chassis_angular_z); 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; @@ -101,6 +101,7 @@ class BulletSolver double max_track_target_vel_; double output_yaw_{}, output_pitch_{}; double bullet_speed_{}, resistance_coff_{}; + double fly_time_; int shoot_beforehand_cmd_{}; int selected_armor_; int count_; @@ -110,7 +111,6 @@ class BulletSolver bool dynamic_reconfig_initialized_{}; geometry_msgs::Point target_pos_{}; - double fly_time_; visualization_msgs::Marker marker_desire_; visualization_msgs::Marker marker_real_; }; From 05eb17aac69d61a26f5b8a8e9df725ea7075a992 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 6 May 2024 18:47:04 +0800 Subject: [PATCH 48/51] Modify param name,delete something unused and add chassis vel into solver. --- rm_gimbal_controllers/src/gimbal_base.cpp | 68 ++++++++++------------- 1 file changed, 29 insertions(+), 39 deletions(-) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 3ae44ff9..770a0dff 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -76,9 +76,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .pitch_k_v_ = getParam(nh_pitch, "k_v", 0.), .k_chassis_vel_ = getParam(controller_nh, "yaw/k_chassis_vel", 0.), .accel_pitch_ = getParam(controller_nh, "pitch/accel", 99.), - .accel_yaw_ = getParam(controller_nh, "yaw/accel", 99.), - .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), - .track_move_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.) }; + .accel_yaw_ = getParam(controller_nh, "yaw/accel", 99.) }; config_rt_buffer_.initRT(config_); d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = @@ -141,9 +139,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); publish_rate_ = getParam(controller_nh, "publish_rate", 100.); error_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "error", 100)); - pid_yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher(nh_yaw, "pos_state", 1)); - pid_pitch_pos_state_pub_.reset( - new realtime_tools::RealtimePublisher(nh_pitch, "pos_state", 1)); + yaw_pos_state_pub_.reset(new realtime_tools::RealtimePublisher(nh_yaw, "pos_state", 1)); + pitch_pos_state_pub_.reset(new realtime_tools::RealtimePublisher(nh_pitch, "pos_state", 1)); ramp_rate_pitch_ = new RampFilter(0, 0.001); ramp_rate_yaw_ = new RampFilter(0, 0.001); @@ -210,8 +207,8 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) quatToRPY(toMsg(base2gimbal_des), roll_temp, base2gimbal_current_des_pitch, base2gimbal_current_des_yaw); double pitch_real_des, yaw_real_des; - pitch_des_in_limit = setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, pitch_joint_urdf_); - if (!pitch_des_in_limit) + pitch_des_in_limit_ = setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, pitch_joint_urdf_); + if (!pitch_des_in_limit_) { double yaw_temp; tf2::Quaternion base2new_des; @@ -295,19 +292,16 @@ void Controller::track(const ros::Time& time) { ROS_WARN("%s", ex.what()); } - double yaw = data_track_.yaw + - data_track_.v_yaw * ((time - data_track_.header.stamp).toSec() + config_.track_rotate_target_delay); - target_pos.x += target_vel.x * ((time - data_track_.header.stamp).toSec() + config_.track_move_target_delay) - - odom2pitch_.transform.translation.x; - target_pos.y += target_vel.y * ((time - data_track_.header.stamp).toSec() + config_.track_move_target_delay) - - odom2pitch_.transform.translation.y; + double yaw = data_track_.yaw + data_track_.v_yaw * ((time - data_track_.header.stamp).toSec()); + target_pos.x += target_vel.x * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.x; + target_pos.y += target_vel.y * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.y; target_pos.z += target_vel.z * (time - data_track_.header.stamp).toSec() - odom2pitch_.transform.translation.z; target_vel.x -= chassis_vel_->linear_->x(); target_vel.y -= chassis_vel_->linear_->y(); 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); + 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()); bullet_solver_->judgeShootBeforehand(time); if (publish_rate_ > 0.0 && last_publish_time_ + ros::Duration(1.0 / publish_rate_) < time) @@ -450,7 +444,7 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) ROS_WARN("%s", ex.what()); } } - if (!pitch_des_in_limit) + if (!pitch_des_in_limit_) pitch_vel_des = 0.; if (!yaw_des_in_limit_) yaw_vel_des = 0.; @@ -461,25 +455,25 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) // publish state if (loop_count_ % 10 == 0) { - if (pid_yaw_pos_state_pub_ && pid_yaw_pos_state_pub_->trylock()) + if (yaw_pos_state_pub_ && yaw_pos_state_pub_->trylock()) { - pid_yaw_pos_state_pub_->msg_.header.stamp = time; - pid_yaw_pos_state_pub_->msg_.set_point = yaw_des; - pid_yaw_pos_state_pub_->msg_.set_point_dot = yaw_vel_des; - pid_yaw_pos_state_pub_->msg_.process_value = yaw_real; - pid_yaw_pos_state_pub_->msg_.error = angles::shortest_angular_distance(yaw_real, yaw_des); - pid_yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); - pid_yaw_pos_state_pub_->unlockAndPublish(); + yaw_pos_state_pub_->msg_.header.stamp = time; + yaw_pos_state_pub_->msg_.set_point = yaw_des; + yaw_pos_state_pub_->msg_.set_point_dot = yaw_vel_des; + yaw_pos_state_pub_->msg_.process_value = yaw_real; + yaw_pos_state_pub_->msg_.error = angles::shortest_angular_distance(yaw_real, yaw_des); + yaw_pos_state_pub_->msg_.command = pid_yaw_pos_.getCurrentCmd(); + yaw_pos_state_pub_->unlockAndPublish(); } - if (pid_pitch_pos_state_pub_ && pid_pitch_pos_state_pub_->trylock()) + if (pitch_pos_state_pub_ && pitch_pos_state_pub_->trylock()) { - pid_pitch_pos_state_pub_->msg_.header.stamp = time; - pid_pitch_pos_state_pub_->msg_.set_point = pitch_des; - pid_pitch_pos_state_pub_->msg_.set_point_dot = pitch_vel_des; - pid_pitch_pos_state_pub_->msg_.process_value = pitch_real; - pid_pitch_pos_state_pub_->msg_.error = angles::shortest_angular_distance(pitch_real, pitch_des); - pid_pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); - pid_pitch_pos_state_pub_->unlockAndPublish(); + pitch_pos_state_pub_->msg_.header.stamp = time; + pitch_pos_state_pub_->msg_.set_point = pitch_des; + pitch_pos_state_pub_->msg_.set_point_dot = pitch_vel_des; + pitch_pos_state_pub_->msg_.process_value = pitch_real; + pitch_pos_state_pub_->msg_.error = angles::shortest_angular_distance(pitch_real, pitch_des); + pitch_pos_state_pub_->msg_.command = pid_pitch_pos_.getCurrentCmd(); + pitch_pos_state_pub_->unlockAndPublish(); } } loop_count_++; @@ -555,17 +549,13 @@ void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uin config.k_chassis_vel_ = init_config.k_chassis_vel_; config.accel_pitch_ = init_config.accel_pitch_; config.accel_yaw_ = init_config.accel_yaw_; - config.track_rotate_target_delay = init_config.track_rotate_target_delay; - config.track_move_target_delay = init_config.track_move_target_delay; dynamic_reconfig_initialized_ = true; } GimbalConfig config_non_rt{ .yaw_k_v_ = config.yaw_k_v_, .pitch_k_v_ = config.pitch_k_v_, .k_chassis_vel_ = config.k_chassis_vel_, .accel_pitch_ = config.accel_pitch_, - .accel_yaw_ = config.accel_yaw_, - .track_rotate_target_delay = config.track_rotate_target_delay, - .track_move_target_delay = config.track_move_target_delay }; + .accel_yaw_ = config.accel_yaw_ }; config_rt_buffer_.writeFromNonRT(config_non_rt); } From e9dad345701bf7d2e9d969600da2b2226d88db2c Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 6 May 2024 18:52:44 +0800 Subject: [PATCH 49/51] Modify param name,add delay when rotation and moving,and think about chassis vel to change switch angle and switch target from armor to center. --- rm_gimbal_controllers/src/bullet_solver.cpp | 74 +++++++++++++-------- 1 file changed, 46 insertions(+), 28 deletions(-) diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index cf134216..c65224a8 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -55,9 +55,12 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) .timeout = getParam(controller_nh, "timeout", 0.), .ban_shoot_duration = getParam(controller_nh, "ban_shoot_duration", 0.0), .gimbal_switch_duration = getParam(controller_nh, "gimbal_switch_duration", 0.0), - .max_switch_angle_ = getParam(controller_nh, "max_switch_angle", 40.0), - .min_switch_angle_ = getParam(controller_nh, "min_switch_angle", 2.0), - .min_fit_switch_count_ = getParam(controller_nh, "min_fit_switch_count", 3) }; + .max_switch_angle = getParam(controller_nh, "max_switch_angle", 40.0), + .min_switch_angle = getParam(controller_nh, "min_switch_angle", 2.0), + .max_chassis_angular_vel = getParam(controller_nh, "max_chassis_angular_vel", 8.5), + .track_rotate_target_delay = getParam(controller_nh, "track_rotate_target_delay", 0.), + .track_move_target_delay = getParam(controller_nh, "track_move_target_delay", 0.), + .min_fit_switch_count = getParam(controller_nh, "min_fit_switch_count", 3) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -89,8 +92,8 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); - identified_target_change_sub_ = controller_nh.subscribe( - "/change", 10, &BulletSolver::identifiedTargetChangeCB, this); + identified_target_change_sub_ = + controller_nh.subscribe("/change", 10, &BulletSolver::identifiedTargetChangeCB, this); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -111,7 +114,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 v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_z) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; @@ -126,21 +129,21 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = 0; double r = r1; double z = pos.z; - double max_switch_angle_ = config_.max_switch_angle_ / 180 * M_PI; - double min_switch_angle_ = config_.min_switch_angle_ / 180 * M_PI; + double max_switch_angle = config_.max_switch_angle / 180 * M_PI; + double min_switch_angle = config_.min_switch_angle / 180 * M_PI; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - double switch_armor_angle = track_target_ ? acos(r / target_rho) - max_switch_angle_ + - (-acos(r / target_rho) + (max_switch_angle_ + min_switch_angle_)) * - std::abs(v_yaw) / max_track_target_vel_ : - min_switch_angle_; - is_in_delay_before_switch_ = - ((((yaw + v_yaw * (rough_fly_time + config_.delay)) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || - (((yaw + v_yaw * (rough_fly_time + config_.delay)) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && - track_target_; - bool is_low_speed = std::abs(v_yaw) < 1.0; + if (std::abs(chassis_angular_z) >= config_.max_chassis_angular_vel) + track_target_ = 0; + double switch_armor_angle = + track_target_ ? + (acos(r / target_rho) - max_switch_angle + + (-acos(r / target_rho) + (max_switch_angle + min_switch_angle)) * std::abs(v_yaw) / max_track_target_vel_) * + (1 - std::abs(chassis_angular_z) / config_.max_chassis_angular_vel) + + min_switch_angle * std::abs(chassis_angular_z) / config_.max_chassis_angular_vel : + min_switch_angle; if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && - !is_low_speed) + std::abs(v_yaw) >= 1.0) { count_++; if (identified_target_change_) @@ -148,15 +151,26 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d count_ = 0; identified_target_change_ = false; } - if (count_ >= config_.min_fit_switch_count_) + if (count_ >= config_.min_fit_switch_count) { - 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; } } + is_in_delay_before_switch_ = + (((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) > + output_yaw_ + switch_armor_angle) && + v_yaw > 0.) || + ((((yaw + selected_armor_ * 2 * M_PI / armors_num) + v_yaw * (rough_fly_time + config_.delay)) < + output_yaw_ - switch_armor_angle) && + v_yaw < 0.)) && + track_target_; + 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_) @@ -348,10 +362,10 @@ void BulletSolver::judgeShootBeforehand(const ros::Time& time) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::JUDGE_BY_ERROR; else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.ban_shoot_duration).toSec()) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; - else if (is_in_delay_before_switch_ && selected_armor_ == 0) - shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::BAN_SHOOT; else if ((ros::Time::now() - switch_armor_time_).toSec() < ros::Duration(config_.gimbal_switch_duration).toSec()) shoot_beforehand_cmd_ = rm_msgs::ShootBeforehandCmd::ALLOW_SHOOT; + else 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()) @@ -379,9 +393,11 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.timeout = init_config.timeout; config.ban_shoot_duration = init_config.ban_shoot_duration; config.gimbal_switch_duration = init_config.gimbal_switch_duration; - config.max_switch_angle_ = init_config.max_switch_angle_; - config.min_switch_angle_ = init_config.min_switch_angle_; - config.min_fit_switch_count_ = init_config.min_fit_switch_count_; + config.max_switch_angle = init_config.max_switch_angle; + config.min_switch_angle = init_config.min_switch_angle; + config.max_chassis_angular_vel = init_config.max_chassis_angular_vel; + config.track_rotate_target_delay = init_config.track_rotate_target_delay; + config.min_fit_switch_count = init_config.min_fit_switch_count; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .resistance_coff_qd_10 = config.resistance_coff_qd_10, @@ -395,9 +411,11 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .timeout = config.timeout, .ban_shoot_duration = config.ban_shoot_duration, .gimbal_switch_duration = config.gimbal_switch_duration, - .max_switch_angle_ = config.max_switch_angle_, - .min_switch_angle_ = config.min_switch_angle_, - .min_fit_switch_count_ = config.min_fit_switch_count_ }; + .max_switch_angle = config.max_switch_angle, + .min_switch_angle = config.min_switch_angle, + .max_chassis_angular_vel = config.max_chassis_angular_vel, + .track_rotate_target_delay = config.track_rotate_target_delay, + .min_fit_switch_count = config.min_fit_switch_count }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers From 2fa2b16e19420e67ef4e2dfb4a9c14683b67cdda Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 9 May 2024 01:33:50 +0800 Subject: [PATCH 50/51] Modify cfg's description. --- rm_gimbal_controllers/cfg/GimbalBase.cfg | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg index b0463da0..debe59ca 100755 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ b/rm_gimbal_controllers/cfg/GimbalBase.cfg @@ -7,8 +7,8 @@ gen = ParameterGenerator() gen.add("yaw_k_v_", double_t, 0, "Yaw input feedforward scale", 0.0, 0, 1.0) gen.add("pitch_k_v_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 1.0) -gen.add("accel_yaw_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 999.0) -gen.add("accel_pitch_", double_t, 0, "Pitch input feedforward scale", 0.0, 0, 999.0) +gen.add("accel_yaw_", double_t, 0, "Acceleration of rate yaw", 0.0, 0, 999.0) +gen.add("accel_pitch_", double_t, 0, "Acceleration of rate pitch", 0.0, 0, 999.0) gen.add("k_chassis_vel_", double_t, 0, "Yaw chassis velocity feedforward scale", 0.0, 0, 1.0) exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) From 7364ccca7231183485f652990df14ab895c44571 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 9 May 2024 01:49:46 +0800 Subject: [PATCH 51/51] Add cfg and modify param name. --- .../include/rm_gimbal_controllers/bullet_solver.h | 2 +- rm_gimbal_controllers/src/bullet_solver.cpp | 10 ++++++---- 2 files changed, 7 insertions(+), 5 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 7c6fc4ce..33ab54f9 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -67,7 +67,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_z); + double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z); 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; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index c65224a8..15cda288 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -114,7 +114,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 chassis_angular_z) + double v_yaw, double r1, double r2, double dz, int armors_num, double chassis_angular_vel_z) { config_ = *config_rt_buffer_.readFromRT(); bullet_speed_ = bullet_speed; @@ -132,14 +132,14 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d double max_switch_angle = config_.max_switch_angle / 180 * M_PI; double min_switch_angle = config_.min_switch_angle / 180 * M_PI; track_target_ = std::abs(v_yaw) < max_track_target_vel_; - if (std::abs(chassis_angular_z) >= config_.max_chassis_angular_vel) + if (std::abs(chassis_angular_vel_z) >= config_.max_chassis_angular_vel) track_target_ = 0; double switch_armor_angle = track_target_ ? (acos(r / target_rho) - max_switch_angle + (-acos(r / target_rho) + (max_switch_angle + min_switch_angle)) * std::abs(v_yaw) / max_track_target_vel_) * - (1 - std::abs(chassis_angular_z) / config_.max_chassis_angular_vel) + - min_switch_angle * std::abs(chassis_angular_z) / config_.max_chassis_angular_vel : + (1 - std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel) + + min_switch_angle * std::abs(chassis_angular_vel_z) / config_.max_chassis_angular_vel : min_switch_angle; if (((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) && @@ -397,6 +397,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.min_switch_angle = init_config.min_switch_angle; 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; dynamic_reconfig_initialized_ = true; } @@ -415,6 +416,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .min_switch_angle = config.min_switch_angle, .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 }; config_rt_buffer_.writeFromNonRT(config_non_rt); }