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 1/5] 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 2/5] 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 3/5] 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 4/5] 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 5/5] 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_),