From 24963cd5d31c412d21981f76e3733e00302518e1 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 25 Mar 2024 18:32:34 +0800 Subject: [PATCH 01/11] Dynamic switch angle. --- rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 ++ .../rm_gimbal_controllers/bullet_solver.h | 6 ++++- rm_gimbal_controllers/src/bullet_solver.cpp | 25 ++++++++++++++----- 3 files changed, 26 insertions(+), 7 deletions(-) diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 8f2b4f4a..628c46f3 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -14,5 +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("angle1", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0) +gen.add("angle2", double_t, 0, "Min switch angle", 15.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 5ed6d081..2fa7f115 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,13 +46,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, angle1, angle2; }; class BulletSolver @@ -85,6 +86,9 @@ class BulletSolver std::shared_ptr> path_real_pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; + rm_msgs::TrackData switch_target_; + ros::NodeHandle controller_nh_; + ros::Publisher switch_target_pub = controller_nh_.advertise("IsSwitchTarget", 100); Config config_{}; double max_track_target_vel_; bool dynamic_reconfig_initialized_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 94188c8a..76efd4ad 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -52,7 +52,9 @@ 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.), + .angle1 = getParam(controller_nh, "angle1", 15.), + .angle2 = getParam(controller_nh, "angle2", 15.) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -116,17 +118,24 @@ 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; + config_.angle1 = config_.angle1 * M_PI / 180; + config_.angle2 = config_.angle2 * M_PI / 180; + 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; + bool switch_target = 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.)) { selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; + switch_target = 1; } + switch_target_.yaw = switch_armor_angle; + switch_target_.id = switch_target; + switch_target_pub.publish(switch_target_); int count{}; double error = 999; if (track_target_) @@ -316,6 +325,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.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, @@ -326,7 +337,9 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .g = config.g, .delay = config.delay, .dt = config.dt, - .timeout = config.timeout }; + .timeout = config.timeout, + .angle1 = config.angle1, + .angle2 = config.angle2 }; config_rt_buffer_.writeFromNonRT(config_non_rt); } } // namespace rm_gimbal_controllers From cd1799ddec518de60ec2df002262bf4c8f86efdf Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 1 Apr 2024 13:14:31 +0800 Subject: [PATCH 02/11] Add angle limit for shooting. --- .../include/rm_gimbal_controllers/bullet_solver.h | 2 ++ rm_gimbal_controllers/src/bullet_solver.cpp | 8 ++++++++ 2 files changed, 10 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 2fa7f115..fb81421b 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -77,6 +77,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); + bool isShootAfterDelay(); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -96,6 +97,7 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; int selected_armor_; bool track_target_; + bool is_shoot_after_delay_; 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 76efd4ad..7d47c583 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -125,6 +125,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d std::abs(v_yaw) / max_track_target_vel_ : config_.angle2; bool switch_target = 0; + if ((((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.)) + is_shoot_after_delay_ = false; 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.)) { @@ -219,6 +222,11 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge } } +bool BulletSolver::isShootAfterDelay() +{ + return is_shoot_after_delay_; +} + void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) { marker_desire_.points.clear(); From c4eb15f70f78517112bae70c882c144daa86cfca Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 1 Apr 2024 16:23:09 +0800 Subject: [PATCH 03/11] Add angle limit for shooting. --- .../include/rm_gimbal_controllers/bullet_solver.h | 6 ++++-- rm_gimbal_controllers/src/bullet_solver.cpp | 14 +++++++++++--- rm_gimbal_controllers/src/gimbal_base.cpp | 1 + 3 files changed, 16 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 fb81421b..4b89ddc8 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -47,6 +47,7 @@ #include #include #include +#include namespace rm_gimbal_controllers { @@ -77,7 +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); - bool isShootAfterDelay(); + void isShootAfterDelay(const ros::Time& time); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -85,6 +86,7 @@ class BulletSolver private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; + std::shared_ptr> pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; rm_msgs::TrackData switch_target_; @@ -97,7 +99,7 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; int selected_armor_; bool track_target_; - bool is_shoot_after_delay_; + double is_shoot_after_delay_ = 1.; 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 7d47c583..7eb29b5e 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -82,6 +82,7 @@ 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)); + pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -127,7 +128,9 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d bool switch_target = 0; if ((((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.)) - is_shoot_after_delay_ = false; + is_shoot_after_delay_ = 0.; + else + is_shoot_after_delay_ = 1.; 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.)) { @@ -222,9 +225,14 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge } } -bool BulletSolver::isShootAfterDelay() +void BulletSolver::isShootAfterDelay(const ros::Time& time) { - return is_shoot_after_delay_; + if (pub_->trylock()) + { + pub_->msg_.stamp = time; + pub_->msg_.error = is_shoot_after_delay_; + pub_->unlockAndPublish(); + } } void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index a5e13561..8c5d5da2 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -305,6 +305,7 @@ void Controller::track(const ros::Time& time) error_pub_->unlockAndPublish(); } bullet_solver_->bulletModelPub(odom2pitch_, time); + bullet_solver_->isShootAfterDelay(time); last_publish_time_ = time; } From cd1430f4b158c9c1035866873c0f20b6039707bd Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 1 Apr 2024 18:43:14 +0800 Subject: [PATCH 04/11] Modify something unreasonable. --- 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 7eb29b5e..142ab1a8 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -126,11 +126,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d std::abs(v_yaw) / max_track_target_vel_ : config_.angle2; bool switch_target = 0; - if ((((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.)) + is_shoot_after_delay_ = 1.; + if (((((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_) is_shoot_after_delay_ = 0.; - else - is_shoot_after_delay_ = 1.; 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.)) { From 3cdb8d3fd94ab621553d1b1e0ac7e96426d75d11 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 1 Apr 2024 22:42:44 +0800 Subject: [PATCH 05/11] Delete something to dynamic reconfigure. --- .../include/rm_gimbal_controllers/bullet_solver.h | 6 +----- rm_gimbal_controllers/src/bullet_solver.cpp | 15 +++++---------- 2 files changed, 6 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 4b89ddc8..eef3783b 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 namespace rm_gimbal_controllers @@ -86,12 +85,9 @@ class BulletSolver private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> pub_; + std::shared_ptr> is_shoot_after_delay_pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; - rm_msgs::TrackData switch_target_; - ros::NodeHandle controller_nh_; - ros::Publisher switch_target_pub = controller_nh_.advertise("IsSwitchTarget", 100); Config config_{}; double max_track_target_vel_; bool dynamic_reconfig_initialized_{}; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index 142ab1a8..e6764d6a 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -82,7 +82,7 @@ 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)); - pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); + is_shoot_after_delay_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -125,7 +125,6 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d (-acos(r / target_rho) + config_.angle1 + config_.angle2) * std::abs(v_yaw) / max_track_target_vel_ : config_.angle2; - bool switch_target = 0; is_shoot_after_delay_ = 1.; if (((((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.)) && @@ -137,11 +136,7 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d selected_armor_ = v_yaw > 0. ? -1 : 1; r = armors_num == 4 ? r2 : r1; z = armors_num == 4 ? pos.z + dz : pos.z; - switch_target = 1; } - switch_target_.yaw = switch_armor_angle; - switch_target_.id = switch_target; - switch_target_pub.publish(switch_target_); int count{}; double error = 999; if (track_target_) @@ -227,11 +222,11 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge void BulletSolver::isShootAfterDelay(const ros::Time& time) { - if (pub_->trylock()) + if (is_shoot_after_delay_pub_->trylock()) { - pub_->msg_.stamp = time; - pub_->msg_.error = is_shoot_after_delay_; - pub_->unlockAndPublish(); + is_shoot_after_delay_pub_->msg_.stamp = time; + is_shoot_after_delay_pub_->msg_.error = is_shoot_after_delay_; + is_shoot_after_delay_pub_->unlockAndPublish(); } } From bb85debdc390f6a09dd9914c721baecd19a8920f Mon Sep 17 00:00:00 2001 From: ye-luo-xi-tui <74857762+ye-luo-xi-tui@users.noreply.github.com> Date: Tue, 2 Apr 2024 14:26:45 +0800 Subject: [PATCH 06/11] Revert "Gimbal cascade pid" --- rm_gimbal_controllers/CMakeLists.txt | 3 +- rm_gimbal_controllers/cfg/BulletSolver.cfg | 2 - rm_gimbal_controllers/cfg/GimbalBase.cfg | 11 -- .../rm_gimbal_controllers/bullet_solver.h | 6 +- .../rm_gimbal_controllers/gimbal_base.h | 37 ++-- rm_gimbal_controllers/src/bullet_solver.cpp | 36 +--- rm_gimbal_controllers/src/gimbal_base.cpp | 167 +++++------------- 7 files changed, 59 insertions(+), 203 deletions(-) delete mode 100755 rm_gimbal_controllers/cfg/GimbalBase.cfg diff --git a/rm_gimbal_controllers/CMakeLists.txt b/rm_gimbal_controllers/CMakeLists.txt index e4f8bd33..98732896 100644 --- a/rm_gimbal_controllers/CMakeLists.txt +++ b/rm_gimbal_controllers/CMakeLists.txt @@ -24,11 +24,10 @@ find_package(catkin REQUIRED visualization_msgs dynamic_reconfigure angles -) + ) generate_dynamic_reconfigure_options( cfg/BulletSolver.cfg - cfg/GimbalBase.cfg ) ################################### diff --git a/rm_gimbal_controllers/cfg/BulletSolver.cfg b/rm_gimbal_controllers/cfg/BulletSolver.cfg index 628c46f3..8f2b4f4a 100755 --- a/rm_gimbal_controllers/cfg/BulletSolver.cfg +++ b/rm_gimbal_controllers/cfg/BulletSolver.cfg @@ -14,7 +14,5 @@ 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("angle1", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0) -gen.add("angle2", double_t, 0, "Min switch angle", 15.0, 0.0, 90.0) exit(gen.generate(PACKAGE, "bullet_solver", "BulletSolver")) diff --git a/rm_gimbal_controllers/cfg/GimbalBase.cfg b/rm_gimbal_controllers/cfg/GimbalBase.cfg deleted file mode 100755 index 0dd3e3e2..00000000 --- a/rm_gimbal_controllers/cfg/GimbalBase.cfg +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python -PACKAGE = "rm_gimbal_controllers" - -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) - -exit(gen.generate(PACKAGE, "gimbal_base", "GimbalBase")) 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 eef3783b..5ed6d081 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h @@ -46,14 +46,13 @@ #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, angle1, angle2; + resistance_coff_qd_30, g, delay, dt, timeout; }; class BulletSolver @@ -77,7 +76,6 @@ 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 isShootAfterDelay(const ros::Time& time); void bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time); void reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, uint32_t); ~BulletSolver() = default; @@ -85,7 +83,6 @@ class BulletSolver private: std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; - std::shared_ptr> is_shoot_after_delay_pub_; realtime_tools::RealtimeBuffer config_rt_buffer_; dynamic_reconfigure::Server* d_srv_{}; Config config_{}; @@ -95,7 +92,6 @@ class BulletSolver double bullet_speed_{}, resistance_coff_{}; int selected_armor_; bool track_target_; - double is_shoot_after_delay_ = 1.; geometry_msgs::Point target_pos_{}; double fly_time_; diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h index 7001757e..1c919927 100644 --- a/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h +++ b/rm_gimbal_controllers/include/rm_gimbal_controllers/gimbal_base.h @@ -38,32 +38,22 @@ #pragma once #include -#include #include #include -#include #include -#include +#include +#include #include #include #include -#include +#include #include #include #include -#include -#include -#include -#include +#include namespace rm_gimbal_controllers { -struct GimbalConfig -{ - // Input feedforward - double yaw_k_v_, pitch_k_v_; -}; - class ChassisVel { public: @@ -149,42 +139,40 @@ class Controller : public controller_interface::MultiInterfaceController bullet_solver_; // ROS Interface ros::Time last_publish_time_{}; - std::unique_ptr> pid_yaw_pos_state_pub_, - pid_pitch_pos_state_pub_; std::shared_ptr> error_pub_; ros::Subscriber cmd_gimbal_sub_; ros::Subscriber data_track_sub_; realtime_tools::RealtimeBuffer cmd_rt_buffer_; realtime_tools::RealtimeBuffer track_rt_buffer_; - urdf::JointConstSharedPtr pitch_joint_urdf_, yaw_joint_urdf_; rm_msgs::GimbalCmd cmd_gimbal_; rm_msgs::TrackData data_track_; std::string gimbal_des_frame_id_{}, imu_name_{}; double publish_rate_{}; bool state_changed_{}; - int loop_count_{}; // Transform - geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_, last_odom2gimbal_des_; + geometry_msgs::TransformStamped odom2gimbal_des_, odom2pitch_, odom2base_, last_odom2base_; // Gravity Compensation geometry_msgs::Vector3 mass_origin_; double gravity_; bool enable_gravity_compensation_; + // Input feedforward + double yaw_k_v_; + double pitch_k_v_; + // Resistance compensation double yaw_resistance_; double velocity_saturation_point_, effort_saturation_point_; @@ -193,11 +181,6 @@ class Controller : public controller_interface::MultiInterfaceController chassis_vel_; - bool dynamic_reconfig_initialized_{}; - GimbalConfig config_{}; - realtime_tools::RealtimeBuffer config_rt_buffer_; - dynamic_reconfigure::Server* d_srv_{}; - enum { RATE, diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index e6764d6a..94188c8a 100644 --- a/rm_gimbal_controllers/src/bullet_solver.cpp +++ b/rm_gimbal_controllers/src/bullet_solver.cpp @@ -52,9 +52,7 @@ 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.), - .angle1 = getParam(controller_nh, "angle1", 15.), - .angle2 = getParam(controller_nh, "angle2", 15.) }; + .timeout = getParam(controller_nh, "timeout", 0.) }; max_track_target_vel_ = getParam(controller_nh, "max_track_target_vel", 5.0); config_rt_buffer_.initRT(config_); @@ -82,7 +80,6 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh) new realtime_tools::RealtimePublisher(controller_nh, "model_desire", 10)); path_real_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "model_real", 10)); - is_shoot_after_delay_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "allow_shoot", 10)); } double BulletSolver::getResistanceCoefficient(double bullet_speed) const @@ -119,17 +116,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_; - config_.angle1 = config_.angle1 * M_PI / 180; - config_.angle2 = config_.angle2 * M_PI / 180; - 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; - is_shoot_after_delay_ = 1.; - if (((((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_) - is_shoot_after_delay_ = 0.; + double switch_armor_angle = track_target_ ? + acos(r / target_rho) - M_PI / 12 + + (-acos(r / target_rho) + M_PI / 6) * std::abs(v_yaw) / max_track_target_vel_ : + M_PI / 12; if ((((yaw + v_yaw * rough_fly_time) > output_yaw_ + switch_armor_angle) && v_yaw > 0.) || (((yaw + v_yaw * rough_fly_time) < output_yaw_ - switch_armor_angle) && v_yaw < 0.)) { @@ -220,16 +210,6 @@ void BulletSolver::getSelectedArmorPosAndVel(geometry_msgs::Point& armor_pos, ge } } -void BulletSolver::isShootAfterDelay(const ros::Time& time) -{ - if (is_shoot_after_delay_pub_->trylock()) - { - is_shoot_after_delay_pub_->msg_.stamp = time; - is_shoot_after_delay_pub_->msg_.error = is_shoot_after_delay_; - is_shoot_after_delay_pub_->unlockAndPublish(); - } -} - void BulletSolver::bulletModelPub(const geometry_msgs::TransformStamped& odom2pitch, const ros::Time& time) { marker_desire_.points.clear(); @@ -336,8 +316,6 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, config.delay = init_config.delay; config.dt = init_config.dt; config.timeout = init_config.timeout; - 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, @@ -348,9 +326,7 @@ void BulletSolver::reconfigCB(rm_gimbal_controllers::BulletSolverConfig& config, .g = config.g, .delay = config.delay, .dt = config.dt, - .timeout = config.timeout, - .angle1 = config.angle1, - .angle2 = config.angle2 }; + .timeout = config.timeout }; 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 8c5d5da2..7208e7a9 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -75,22 +75,12 @@ 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"); - - config_ = { .yaw_k_v_ = getParam(nh_pitch, "k_v", 0.), .pitch_k_v_ = getParam(nh_yaw, "k_v", 0.) }; - config_rt_buffer_.initRT(config_); - d_srv_ = new dynamic_reconfigure::Server(controller_nh); - dynamic_reconfigure::Server::CallbackType cb = - [this](auto&& PH1, auto&& PH2) { reconfigCB(PH1, PH2); }; - d_srv_->setCallback(cb); - + yaw_k_v_ = getParam(nh_yaw, "k_v", 0.); + pitch_k_v_ = getParam(nh_pitch, "k_v", 0.); hardware_interface::EffortJointInterface* effort_joint_interface = robot_hw->get(); - if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch) || - !pid_yaw_pos_.init(nh_pid_yaw_pos) || !pid_pitch_pos_.init(nh_pid_pitch_pos)) + if (!ctrl_yaw_.init(effort_joint_interface, nh_yaw) || !ctrl_pitch_.init(effort_joint_interface, nh_pitch)) return false; - robot_state_handle_ = robot_hw->get()->getHandle("robot_state"); if (!controller_nh.hasParam("imu_name")) has_imu_ = false; @@ -106,45 +96,22 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ROS_INFO("Param imu_name has not set, use motors' data instead of imu."); } - // Get URDF info about joint - urdf::Model urdf; - if (!urdf.initParamWithNodeHandle("robot_description", controller_nh)) - { - ROS_ERROR("Failed to parse urdf file"); - return false; - } - pitch_joint_urdf_ = urdf.getJoint(ctrl_pitch_.getJointName()); - yaw_joint_urdf_ = urdf.getJoint(ctrl_yaw_.getJointName()); - if (!pitch_joint_urdf_) - { - ROS_ERROR("Could not find joint pitch in urdf"); - return false; - } - if (!yaw_joint_urdf_) - { - ROS_ERROR("Could not find joint yaw in urdf"); - return false; - } - - gimbal_des_frame_id_ = pitch_joint_urdf_->child_link_name + "_des"; + gimbal_des_frame_id_ = ctrl_pitch_.joint_urdf_->child_link_name + "_des"; odom2gimbal_des_.header.frame_id = "odom"; odom2gimbal_des_.child_frame_id = gimbal_des_frame_id_; odom2gimbal_des_.transform.rotation.w = 1.; odom2pitch_.header.frame_id = "odom"; - odom2pitch_.child_frame_id = pitch_joint_urdf_->child_link_name; + odom2pitch_.child_frame_id = ctrl_pitch_.joint_urdf_->child_link_name; odom2pitch_.transform.rotation.w = 1.; odom2base_.header.frame_id = "odom"; - odom2base_.child_frame_id = yaw_joint_urdf_->parent_link_name; + odom2base_.child_frame_id = ctrl_yaw_.joint_urdf_->parent_link_name; odom2base_.transform.rotation.w = 1.; cmd_gimbal_sub_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); data_track_sub_ = controller_nh.subscribe("/track", 1, &Controller::trackCB, this); 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)); + return true; } @@ -158,11 +125,10 @@ 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(); try { - odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time); - odom2base_ = robot_state_handle_.lookupTransform("odom", yaw_joint_urdf_->parent_link_name, time); + odom2pitch_ = robot_state_handle_.lookupTransform("odom", ctrl_pitch_.joint_urdf_->child_link_name, time); + odom2base_ = robot_state_handle_.lookupTransform("odom", ctrl_yaw_.joint_urdf_->parent_link_name, time); } catch (tf2::TransformException& ex) { @@ -201,13 +167,13 @@ 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_)) + if (!setDesIntoLimit(pitch_real_des, pitch_des, base2gimbal_current_des_pitch, ctrl_pitch_.joint_urdf_)) { double yaw_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->upper : 1e16; - lower_limit = pitch_joint_urdf_->limits ? pitch_joint_urdf_->limits->lower : -1e16; + upper_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->upper : 1e16; + lower_limit = ctrl_pitch_.joint_urdf_->limits ? ctrl_pitch_.joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_pitch, lower_limit)) ? @@ -217,13 +183,13 @@ 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_)) + if (!setDesIntoLimit(yaw_real_des, yaw_des, base2gimbal_current_des_yaw, ctrl_yaw_.joint_urdf_)) { double pitch_temp; tf2::Quaternion base2new_des; double upper_limit, lower_limit; - upper_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->upper : 1e16; - lower_limit = yaw_joint_urdf_->limits ? yaw_joint_urdf_->limits->lower : -1e16; + upper_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->upper : 1e16; + lower_limit = ctrl_yaw_.joint_urdf_->limits ? ctrl_yaw_.joint_urdf_->limits->lower : -1e16; base2new_des.setRPY(0, base2gimbal_current_des_pitch, std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, upper_limit)) < std::abs(angles::shortest_angular_distance(base2gimbal_current_des_yaw, lower_limit)) ? @@ -305,7 +271,6 @@ void Controller::track(const ros::Time& time) error_pub_->unlockAndPublish(); } bullet_solver_->bulletModelPub(odom2pitch_, time); - bullet_solver_->isShootAfterDelay(time); last_publish_time_ = time; } @@ -371,10 +336,10 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) try { tf2::doTransform(gyro, angular_vel_pitch, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); tf2::doTransform(gyro, angular_vel_yaw, - robot_state_handle_.lookupTransform(yaw_joint_urdf_->child_link_name, + robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->child_link_name, imu_sensor_handle_.getFrameId(), time)); } catch (tf2::TransformException& ex) @@ -388,13 +353,11 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) angular_vel_yaw.z = ctrl_yaw_.joint_.getVelocity(); angular_vel_pitch.y = ctrl_pitch_.joint_.getVelocity(); } - double roll_real, pitch_real, yaw_real, roll_des, pitch_des, yaw_des; - quatToRPY(odom2gimbal_des_.transform.rotation, roll_des, pitch_des, yaw_des); - quatToRPY(odom2pitch_.transform.rotation, roll_real, pitch_real, yaw_real); - double yaw_angle_error = angles::shortest_angular_distance(yaw_real, yaw_des); - double pitch_angle_error = angles::shortest_angular_distance(pitch_real, pitch_des); - pid_pitch_pos_.computeCommand(pitch_angle_error, period); - pid_yaw_pos_.computeCommand(yaw_angle_error, period); + geometry_msgs::TransformStamped base_frame2des; + base_frame2des = + robot_state_handle_.lookupTransform(ctrl_yaw_.joint_urdf_->parent_link_name, gimbal_des_frame_id_, time); + double roll_des, pitch_des, yaw_des; // desired position + quatToRPY(base_frame2des.transform.rotation, roll_des, pitch_des, yaw_des); double yaw_vel_des = 0., pitch_vel_des = 0.; if (state_ == RATE) @@ -410,18 +373,19 @@ void Controller::moveJoint(const ros::Time& time, const ros::Duration& period) data_track_.yaw, data_track_.v_yaw, data_track_.radius_1, data_track_.radius_2, data_track_.dz, data_track_.armors_num); tf2::Vector3 target_pos_tf, target_vel_tf; + try { geometry_msgs::TransformStamped transform = robot_state_handle_.lookupTransform( - yaw_joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); + ctrl_yaw_.joint_urdf_->parent_link_name, data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); tf2::fromMsg(target_vel, target_vel_tf); yaw_vel_des = target_pos_tf.cross(target_vel_tf).z() / std::pow((target_pos_tf.length()), 2); - transform = robot_state_handle_.lookupTransform(pitch_joint_urdf_->parent_link_name, data_track_.header.frame_id, - data_track_.header.stamp); + transform = robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->parent_link_name, + data_track_.header.frame_id, data_track_.header.stamp); tf2::doTransform(target_pos, target_pos, transform); tf2::doTransform(target_vel, target_vel, transform); tf2::fromMsg(target_pos, target_pos_tf); @@ -434,67 +398,35 @@ 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()) - { - 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_.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_.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_.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_.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(); - } - } - 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_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_.setCommand(yaw_des, yaw_vel_des + ctrl_yaw_.joint_.getVelocity() - angular_vel_yaw.z); + ctrl_pitch_.setCommand(pitch_des, pitch_vel_des + ctrl_pitch_.joint_.getVelocity() - angular_vel_pitch.y); ctrl_yaw_.update(time, period); ctrl_pitch_.update(time, period); + double resistance_compensation = 0.; + if (std::abs(ctrl_yaw_.joint_.getVelocity()) > velocity_saturation_point_) + resistance_compensation = (ctrl_yaw_.joint_.getVelocity() > 0 ? 1 : -1) * yaw_resistance_; + else if (std::abs(ctrl_yaw_.joint_.getCommand()) > effort_saturation_point_) + resistance_compensation = (ctrl_yaw_.joint_.getCommand() > 0 ? 1 : -1) * yaw_resistance_; + else + resistance_compensation = ctrl_yaw_.joint_.getCommand() * yaw_resistance_ / effort_saturation_point_; + ctrl_yaw_.joint_.setCommand(ctrl_yaw_.joint_.getCommand() - k_chassis_vel_ * chassis_vel_->angular_->z() + + yaw_k_v_ * yaw_vel_des + resistance_compensation); + ctrl_pitch_.joint_.setCommand(ctrl_pitch_.joint_.getCommand() + feedForward(time) + pitch_k_v_ * pitch_vel_des); } 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(ctrl_pitch_.joint_urdf_->child_link_name, "odom", time)); Eigen::Vector3d mass_origin(mass_origin_.x, 0, mass_origin_.z); double feedforward = -mass_origin.cross(gravity).y(); if (enable_gravity_compensation_) { Eigen::Vector3d gravity_compensation(0, 0, gravity_); tf2::doTransform(gravity_compensation, gravity_compensation, - robot_state_handle_.lookupTransform(pitch_joint_urdf_->child_link_name, - pitch_joint_urdf_->parent_link_name, time)); + robot_state_handle_.lookupTransform(ctrl_pitch_.joint_urdf_->child_link_name, + ctrl_pitch_.joint_urdf_->parent_link_name, time)); feedforward -= mass_origin.cross(gravity_compensation).y(); } return feedforward; @@ -532,23 +464,6 @@ void Controller::trackCB(const rm_msgs::TrackDataConstPtr& msg) track_rt_buffer_.writeFromNonRT(*msg); } -void Controller::reconfigCB(rm_gimbal_controllers::GimbalBaseConfig& config, uint32_t /*unused*/) -{ - ROS_INFO("[Gimbal Base] Dynamic params change"); - if (!dynamic_reconfig_initialized_) - { - 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_; - dynamic_reconfig_initialized_ = true; - } - GimbalConfig config_non_rt{ - .yaw_k_v_ = config.yaw_k_v_, - .pitch_k_v_ = config.pitch_k_v_, - }; - config_rt_buffer_.writeFromNonRT(config_non_rt); -} - } // namespace rm_gimbal_controllers PLUGINLIB_EXPORT_CLASS(rm_gimbal_controllers::Controller, controller_interface::ControllerBase) From dfb6803a7a2e8828b8e3242188b2b7a0706d1060 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Apr 2024 21:27:00 +0800 Subject: [PATCH 07/11] Modify something unreasonable. --- .../include/rm_shooter_controllers/standard.h | 2 +- rm_shooter_controllers/src/standard.cpp | 28 +++++++------------ 2 files changed, 11 insertions(+), 19 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 0b311f57..be0515b9 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -56,7 +56,7 @@ namespace rm_shooter_controllers struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, - forward_push_threshold,exit_push_threshold; + forward_push_threshold, exit_push_threshold; double extra_wheel_speed; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index b252f146..afa60ab5 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -111,8 +111,9 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) if (state_ != BLOCK) if ((state_ != PUSH || cmd_.mode != READY) || (cmd_.mode == READY && - std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < - config_.exit_push_threshold)) + (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < + config_.exit_push_threshold || + cmd_.hz >= 20))) { state_ = cmd_.mode; state_changed_ = true; @@ -199,26 +200,17 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) { // Time to shoot!!! if (cmd_.hz >= 20) { - config_.forward_push_threshold += 0.5; - if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < - config_.forward_push_threshold) - { ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_), - -1 * cmd_.hz * 2. * M_PI / static_cast(push_per_rotation_)); - last_shoot_time_ = time; - } - config_.forward_push_threshold -= 0.5; + -1 * cmd_.hz * 2. * M_PI / static_cast(push_per_rotation_)); + last_shoot_time_ = time; } - else + else if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < + config_.forward_push_threshold) { - if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < - config_.forward_push_threshold) - { - ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - - 2. * M_PI / static_cast(push_per_rotation_)); - last_shoot_time_ = time; - } + ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - + 2. * M_PI / static_cast(push_per_rotation_)); + last_shoot_time_ = time; } // Check block if ((ctrl_trigger_.joint_.getEffort() < -config_.block_effort && From 99e2fe163b79b495a17fe52662d1754c2c65756b Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Apr 2024 21:52:10 +0800 Subject: [PATCH 08/11] Modify something unreasonable. --- rm_shooter_controllers/src/standard.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index afa60ab5..8aa8c2f8 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -269,7 +269,8 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) void Controller::normalize() { double push_angle = 2. * M_PI / static_cast(push_per_rotation_); - ctrl_trigger_.setCommand(push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01) / push_angle)); + ctrl_trigger_.setCommand( + push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle)); } void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) From 6d107a5e3234b776ef4ce0c291c9c6373e250a92 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 11 Apr 2024 02:00:35 +0800 Subject: [PATCH 09/11] Modify wrong. --- rm_shooter_controllers/src/standard.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 8aa8c2f8..fc889124 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -200,8 +200,8 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) { // Time to shoot!!! if (cmd_.hz >= 20) { - ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - - 2. * M_PI / static_cast(push_per_rotation_), + ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - + 2. * M_PI / static_cast(push_per_rotation_), -1 * cmd_.hz * 2. * M_PI / static_cast(push_per_rotation_)); last_shoot_time_ = time; } From 1b67b3e18f7c7922a98084bee25464b665cbc76e Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 11 Apr 2024 15:28:31 +0800 Subject: [PATCH 10/11] Add a param. --- .../include/rm_shooter_controllers/standard.h | 1 + rm_shooter_controllers/src/standard.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index be0515b9..42a493a2 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -88,6 +88,7 @@ class Controller : public controller_interface::MultiInterfaceController wheel_speed_offset_l_, wheel_speed_offset_r_; int push_per_rotation_{}; double push_wheel_speed_threshold_{}; + double high_shoot_frequency_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; bool maybe_block_ = false; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index fc889124..0a61c890 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -56,6 +56,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro config_rt_buffer.initRT(config_); push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0); push_wheel_speed_threshold_ = getParam(controller_nh, "push_wheel_speed_threshold", 0.); + high_shoot_frequency_ = getParam(controller_nh, "high_shoot_frequency", 20.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); @@ -113,7 +114,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) (cmd_.mode == READY && (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < config_.exit_push_threshold || - cmd_.hz >= 20))) + cmd_.hz >= high_shoot_frequency_))) { state_ = cmd_.mode; state_changed_ = true; @@ -198,7 +199,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) } if ((cmd_.wheel_speed == 0. || wheel_speed_ready) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! - if (cmd_.hz >= 20) + if (cmd_.hz >= high_shoot_frequency_) { ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_), From 8a4b9fa90b4ba7e97b46780b032ff5cd7a2e554a Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Fri, 12 Apr 2024 03:55:26 +0800 Subject: [PATCH 11/11] Modify param_name. --- .../include/rm_shooter_controllers/standard.h | 2 +- rm_shooter_controllers/src/standard.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 42a493a2..4cd20c85 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -88,7 +88,7 @@ class Controller : public controller_interface::MultiInterfaceController wheel_speed_offset_l_, wheel_speed_offset_r_; int push_per_rotation_{}; double push_wheel_speed_threshold_{}; - double high_shoot_frequency_{}; + double freq_threshold_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; bool maybe_block_ = false; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 0a61c890..cd270d54 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -56,7 +56,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro config_rt_buffer.initRT(config_); push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0); push_wheel_speed_threshold_ = getParam(controller_nh, "push_wheel_speed_threshold", 0.); - high_shoot_frequency_ = getParam(controller_nh, "high_shoot_frequency", 20.); + freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); @@ -114,7 +114,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) (cmd_.mode == READY && (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < config_.exit_push_threshold || - cmd_.hz >= high_shoot_frequency_))) + cmd_.hz >= freq_threshold_))) { state_ = cmd_.mode; state_changed_ = true; @@ -199,7 +199,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) } if ((cmd_.wheel_speed == 0. || wheel_speed_ready) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! - if (cmd_.hz >= high_shoot_frequency_) + if (cmd_.hz >= freq_threshold_) { ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_),