From a23f13a3971c14b7546377ed9d2eb64de8918910 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 15 Apr 2024 23:22:12 +0800 Subject: [PATCH 01/13] Calculate bullet heat locally. --- rm_shooter_controllers/cfg/Shooter.cfg | 2 ++ .../include/rm_shooter_controllers/standard.h | 6 +++-- rm_shooter_controllers/src/standard.cpp | 24 +++++++++++++++++-- 3 files changed, 28 insertions(+), 4 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 4e2a2719..ac7eefda 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -14,5 +14,7 @@ gen.add("anti_block_threshold", double_t, 0, "Trigger anti block error threshold gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to push forward in push mode",0.01,0.0,1) gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) +gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999) +gen.add("time_out", double_t, 0, "Max duration waiting for wheel speed drop", 0.05, 0.0, 1.0) exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 0b311f57..5d9781bc 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -56,8 +56,8 @@ 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; - double extra_wheel_speed; + forward_push_threshold, exit_push_threshold; + double extra_wheel_speed, wheel_speed_drop_threshold, time_out; }; class Controller : public controller_interface::MultiInterfaceControllermsg_.stamp = time; shoot_state_pub_->msg_.state = state_; + shoot_state_pub_->msg_.has_shoot = has_shoot_; shoot_state_pub_->unlockAndPublish(); } for (auto& ctrl_friction_l : ctrls_friction_l_) @@ -182,6 +185,8 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) state_changed_ = false; ROS_INFO("[Shooter] Enter PUSH"); } + if (has_shoot_) + has_shoot_ = false; bool wheel_speed_ready = true; for (auto& ctrl_friction_l : ctrls_friction_l_) { @@ -207,6 +212,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) 2. * M_PI / static_cast(push_per_rotation_), -1 * cmd_.hz * 2. * M_PI / static_cast(push_per_rotation_)); last_shoot_time_ = time; + maybe_shoot_ = true; } config_.forward_push_threshold -= 0.5; } @@ -218,6 +224,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_)); last_shoot_time_ = time; + maybe_shoot_ = true; } } // Check block @@ -235,6 +242,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) { state_ = BLOCK; state_changed_ = true; + maybe_shoot_ = false; ROS_INFO("[Shooter] Exit PUSH"); } } @@ -243,6 +251,14 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) } else ROS_DEBUG("[Shooter] Wait for friction wheel"); + if (maybe_shoot_ && + abs(ctrls_friction_l_[0]->command_ - ctrls_friction_l_[0]->joint_.getVelocity()) < + config_.wheel_speed_drop_threshold && + (time - last_shoot_time_).toSec() < config_.time_out) + { + has_shoot_ = true; + maybe_shoot_ = false; + } } void Controller::block(const ros::Time& time, const ros::Duration& period) @@ -295,6 +311,8 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 config.forward_push_threshold = init_config.forward_push_threshold; config.exit_push_threshold = init_config.exit_push_threshold; config.extra_wheel_speed = init_config.extra_wheel_speed; + config.wheel_speed_drop_threshold = init_config.wheel_speed_drop_threshold; + config.time_out = init_config.time_out; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .block_effort = config.block_effort, @@ -305,7 +323,9 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .anti_block_threshold = config.anti_block_threshold, .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, - .extra_wheel_speed = config.extra_wheel_speed }; + .extra_wheel_speed = config.extra_wheel_speed, + .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold, + .time_out = config.time_out }; config_rt_buffer.writeFromNonRT(config_non_rt); } From d5c46c2f55d0df91cebdd301ab0a765ee720365c Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 16 Apr 2024 12:37:25 +0800 Subject: [PATCH 02/13] Delete unnecessary param. --- rm_shooter_controllers/cfg/Shooter.cfg | 1 - .../include/rm_shooter_controllers/standard.h | 2 +- rm_shooter_controllers/src/standard.cpp | 9 +++------ 3 files changed, 4 insertions(+), 8 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index ac7eefda..dbb30c52 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -15,6 +15,5 @@ gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to p gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999) -gen.add("time_out", double_t, 0, "Max duration waiting for wheel speed drop", 0.05, 0.0, 1.0) exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 5d9781bc..3f210c9d 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -57,7 +57,7 @@ struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, forward_push_threshold, exit_push_threshold; - double extra_wheel_speed, wheel_speed_drop_threshold, time_out; + double extra_wheel_speed, wheel_speed_drop_threshold; }; class Controller : public controller_interface::MultiInterfaceControllercommand_ - ctrls_friction_l_[0]->joint_.getVelocity()) < config_.wheel_speed_drop_threshold && - (time - last_shoot_time_).toSec() < config_.time_out) + (time - last_shoot_time_).toSec() < 1. / cmd_.hz) { has_shoot_ = true; maybe_shoot_ = false; @@ -312,7 +311,6 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 config.exit_push_threshold = init_config.exit_push_threshold; config.extra_wheel_speed = init_config.extra_wheel_speed; config.wheel_speed_drop_threshold = init_config.wheel_speed_drop_threshold; - config.time_out = init_config.time_out; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .block_effort = config.block_effort, @@ -324,8 +322,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, .extra_wheel_speed = config.extra_wheel_speed, - .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold, - .time_out = config.time_out }; + .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold }; config_rt_buffer.writeFromNonRT(config_non_rt); } From 2acecd05b8eeb55b97caf3edc380d1c0e57b2b38 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 22 Apr 2024 13:47:07 +0800 Subject: [PATCH 03/13] Calculate heat when double shoot. --- .../include/rm_shooter_controllers/standard.h | 1 + rm_shooter_controllers/src/standard.cpp | 18 ++++++++++++------ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 3f210c9d..38e3292b 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -93,6 +93,7 @@ class Controller : public controller_interface::MultiInterfaceControllercommand_ - ctrls_friction_l_[0]->joint_.getVelocity()) < - config_.wheel_speed_drop_threshold && - (time - last_shoot_time_).toSec() < 1. / cmd_.hz) + if (maybe_shoot_) { - has_shoot_ = true; - maybe_shoot_ = false; + if (abs(ctrls_friction_l_[0]->command_ - ctrls_friction_l_[0]->joint_.getVelocity()) > + config_.wheel_speed_drop_threshold) + wheel_speed_drop_ = true; + if (wheel_speed_drop_ && + ctrls_friction_l_[0]->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrls_friction_l_[0]->command_) + { + has_shoot_ = true; + wheel_speed_drop_ = false; + } + if ((time - last_shoot_time_).toSec() >= 1. / cmd_.hz) + maybe_shoot_ = false; } } From 39eef726048b3f075ac8b89036e2ac795388db6f Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Mon, 22 Apr 2024 17:15:08 +0800 Subject: [PATCH 04/13] Modify calculate code pos. --- rm_shooter_controllers/src/standard.cpp | 32 ++++++++++++------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 9fff0f60..df594110 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -137,6 +137,20 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) block(time, period); break; } + if (maybe_shoot_) + { + if (abs(ctrls_friction_l_[0]->command_ - ctrls_friction_l_[0]->joint_.getVelocity()) > + config_.wheel_speed_drop_threshold) + wheel_speed_drop_ = true; + if (wheel_speed_drop_ && + ctrls_friction_l_[0]->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrls_friction_l_[0]->command_) + { + has_shoot_ = true; + wheel_speed_drop_ = false; + } + if ((time - last_shoot_time_).toSec() >= 1. / cmd_.hz) + maybe_shoot_ = false; + } if (shoot_state_pub_->trylock()) { shoot_state_pub_->msg_.stamp = time; @@ -144,6 +158,8 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) shoot_state_pub_->msg_.has_shoot = has_shoot_; shoot_state_pub_->unlockAndPublish(); } + if (has_shoot_) + has_shoot_ = false; for (auto& ctrl_friction_l : ctrls_friction_l_) ctrl_friction_l->update(time, period); for (auto& ctrl_friction_r : ctrls_friction_r_) @@ -184,8 +200,6 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) state_changed_ = false; ROS_INFO("[Shooter] Enter PUSH"); } - if (has_shoot_) - has_shoot_ = false; bool wheel_speed_ready = true; for (auto& ctrl_friction_l : ctrls_friction_l_) { @@ -250,20 +264,6 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) } else ROS_DEBUG("[Shooter] Wait for friction wheel"); - if (maybe_shoot_) - { - if (abs(ctrls_friction_l_[0]->command_ - ctrls_friction_l_[0]->joint_.getVelocity()) > - config_.wheel_speed_drop_threshold) - wheel_speed_drop_ = true; - if (wheel_speed_drop_ && - ctrls_friction_l_[0]->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrls_friction_l_[0]->command_) - { - has_shoot_ = true; - wheel_speed_drop_ = false; - } - if ((time - last_shoot_time_).toSec() >= 1. / cmd_.hz) - maybe_shoot_ = false; - } } void Controller::block(const ros::Time& time, const ros::Duration& period) From 8178fc99ac97eed37002cccfdac3a52cf229e529 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Sat, 4 May 2024 23:19:00 +0800 Subject: [PATCH 05/13] Calculate heat with trigger. --- .../include/rm_shooter_controllers/standard.h | 8 +- rm_shooter_controllers/src/standard.cpp | 78 +++++++++---------- 2 files changed, 43 insertions(+), 43 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 38e3292b..fe2439c8 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -80,20 +80,22 @@ class Controller : public controller_interface::MultiInterfaceController ctrls_friction_l_, ctrls_friction_r_; effort_controllers::JointPositionController ctrl_trigger_; std::vector wheel_speed_offset_l_, wheel_speed_offset_r_; - int push_per_rotation_{}; + int push_per_rotation_{}, count_{}; double push_wheel_speed_threshold_{}; + double freq_threshold_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; bool maybe_block_ = false; bool maybe_shoot_ = false; bool has_shoot_ = false; - bool wheel_speed_drop_ = false; + bool has_shoot_last_ = false; ros::Time last_shoot_time_, block_time_, last_block_time_; enum @@ -109,7 +111,7 @@ class Controller : public controller_interface::MultiInterfaceController cmd_rt_buffer_; rm_msgs::ShootCmd cmd_; std::shared_ptr> shoot_state_pub_; - ros::Subscriber cmd_subscriber_; + ros::Subscriber cmd_subscriber_, heat_sub_; dynamic_reconfigure::Server* d_srv_{}; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index df594110..424e4338 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -57,9 +57,11 @@ 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.); + 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)); + // Init dynamic reconfigure d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = [this](auto&& PH1, auto&& PH2) { @@ -112,8 +114,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 >= freq_threshold_))) { state_ = cmd_.mode; state_changed_ = true; @@ -139,24 +142,27 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) } if (maybe_shoot_) { - if (abs(ctrls_friction_l_[0]->command_ - ctrls_friction_l_[0]->joint_.getVelocity()) > - config_.wheel_speed_drop_threshold) - wheel_speed_drop_ = true; - if (wheel_speed_drop_ && - ctrls_friction_l_[0]->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrls_friction_l_[0]->command_) - { - has_shoot_ = true; - wheel_speed_drop_ = false; - } - if ((time - last_shoot_time_).toSec() >= 1. / cmd_.hz) - maybe_shoot_ = false; + has_shoot_ = true; + maybe_shoot_ = false; } - if (shoot_state_pub_->trylock()) + count_++; + if (has_shoot_last_ == true) { - shoot_state_pub_->msg_.stamp = time; - shoot_state_pub_->msg_.state = state_; - shoot_state_pub_->msg_.has_shoot = has_shoot_; - shoot_state_pub_->unlockAndPublish(); + has_shoot_ = true; + } + has_shoot_last_ = has_shoot_; + if (count_ == 5) + { + if (shoot_state_pub_->trylock()) + { + ROS_INFO("count:%d", count_); + shoot_state_pub_->msg_.has_shoot = has_shoot_; + shoot_state_pub_->msg_.stamp = time; + shoot_state_pub_->msg_.state = state_; + shoot_state_pub_->unlockAndPublish(); + } + has_shoot_last_ = false; + count_ = 0; } if (has_shoot_) has_shoot_ = false; @@ -215,30 +221,21 @@ 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 >= freq_threshold_) { - 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; - maybe_shoot_ = true; - } - config_.forward_push_threshold -= 0.5; + 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; + maybe_shoot_ = true; } - 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; - maybe_shoot_ = true; - } + ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - + 2. * M_PI / static_cast(push_per_rotation_)); + last_shoot_time_ = time; + maybe_shoot_ = true; } // Check block if ((ctrl_trigger_.joint_.getEffort() < -config_.block_effort && @@ -298,7 +295,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 0f38d9d15b95e38af76d17eea84d23e13853d288 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Sun, 5 May 2024 18:23:23 +0800 Subject: [PATCH 06/13] Calculate heat with friction. --- rm_shooter_controllers/cfg/Shooter.cfg | 1 + .../include/rm_shooter_controllers/standard.h | 10 ++- rm_shooter_controllers/src/standard.cpp | 73 +++++++++++-------- 3 files changed, 49 insertions(+), 35 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index dbb30c52..9465c224 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -15,5 +15,6 @@ gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to p gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999) +gen.add("wheel_speed_raise_threshold", double_t, 0, "Wheel speed raise threshold", 50.0, 0.0, 999) exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index fe2439c8..1e1b917c 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -57,7 +57,7 @@ struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, forward_push_threshold, exit_push_threshold; - double extra_wheel_speed, wheel_speed_drop_threshold; + double extra_wheel_speed, wheel_speed_drop_threshold, wheel_speed_raise_threshold; }; class Controller : public controller_interface::MultiInterfaceControllertrylock()) - { - ROS_INFO("count:%d", count_); - shoot_state_pub_->msg_.has_shoot = has_shoot_; - shoot_state_pub_->msg_.stamp = time; - shoot_state_pub_->msg_.state = state_; - shoot_state_pub_->unlockAndPublish(); - } - has_shoot_last_ = false; - count_ = 0; - } - if (has_shoot_) - has_shoot_ = false; + localHeat(time, period); for (auto& ctrl_friction_l : ctrls_friction_l_) ctrl_friction_l->update(time, period); for (auto& ctrl_friction_r : ctrls_friction_r_) @@ -227,7 +203,6 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) 2. * M_PI / static_cast(push_per_rotation_), -1 * cmd_.hz * 2. * M_PI / static_cast(push_per_rotation_)); last_shoot_time_ = time; - maybe_shoot_ = true; } else if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < config_.forward_push_threshold) @@ -235,7 +210,6 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_)); last_shoot_time_ = time; - maybe_shoot_ = true; } // Check block if ((ctrl_trigger_.joint_.getEffort() < -config_.block_effort && @@ -252,7 +226,6 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) { state_ = BLOCK; state_changed_ = true; - maybe_shoot_ = false; ROS_INFO("[Shooter] Exit PUSH"); } } @@ -299,6 +272,42 @@ void Controller::normalize() push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle)); } +void Controller::localHeat(const ros::Time& time, const ros::Duration& period) +{ + if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_vel_l_ > config_.wheel_speed_raise_threshold && drop_flag_) + { + raise_flag_ = true; + drop_flag_ = false; + } + + if (last_vel_l_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && + abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && raise_flag_) + { + drop_flag_ = true; + raise_flag_ = false; + has_shoot_ = true; + } + last_vel_l_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); + count_++; + if (has_shoot_last_) + { + has_shoot_ = true; + } + has_shoot_last_ = has_shoot_; + if (count_ == 5) + { + if (shoot_state_pub_->trylock()) + { + ROS_INFO("count:%d", count_); + shoot_state_pub_->msg_.has_shoot = has_shoot_; + shoot_state_pub_->msg_.stamp = time; + shoot_state_pub_->msg_.state = state_; + shoot_state_pub_->unlockAndPublish(); + } + has_shoot_last_ = false; + count_ = 0; + } +} void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) { ROS_INFO("[Shooter] Dynamic params change"); @@ -315,6 +324,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 config.exit_push_threshold = init_config.exit_push_threshold; config.extra_wheel_speed = init_config.extra_wheel_speed; config.wheel_speed_drop_threshold = init_config.wheel_speed_drop_threshold; + config.wheel_speed_raise_threshold = init_config.wheel_speed_raise_threshold; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .block_effort = config.block_effort, @@ -326,7 +336,8 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, .extra_wheel_speed = config.extra_wheel_speed, - .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold }; + .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold, + .wheel_speed_raise_threshold = config.wheel_speed_raise_threshold }; config_rt_buffer.writeFromNonRT(config_non_rt); } From 47119f8b8d35839459c3f87784dc5eb2e0f6678e Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Wed, 8 May 2024 19:13:12 +0800 Subject: [PATCH 07/13] Change place of function. --- .../include/rm_shooter_controllers/standard.h | 4 ++- rm_shooter_controllers/src/standard.cpp | 26 ++++++++++++------- 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 1e1b917c..66a44682 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -112,8 +113,9 @@ class Controller : public controller_interface::MultiInterfaceController config_rt_buffer; realtime_tools::RealtimeBuffer cmd_rt_buffer_; rm_msgs::ShootCmd cmd_; + std::shared_ptr> local_heat_state_pub_; std::shared_ptr> shoot_state_pub_; - ros::Subscriber cmd_subscriber_, heat_sub_; + ros::Subscriber cmd_subscriber_; dynamic_reconfigure::Server* d_srv_{}; }; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index e046ad25..9b57edd6 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -53,7 +53,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1), .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1), .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.), - .wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 0.), + .wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.), .wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) }; config_rt_buffer.initRT(config_); push_per_rotation_ = getParam(controller_nh, "push_per_rotation", 0); @@ -61,8 +61,9 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); + local_heat_state_pub_.reset( + new realtime_tools::RealtimePublisher(controller_nh, "/local_heat_state", 10)); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); - // Init dynamic reconfigure d_srv_ = new dynamic_reconfigure::Server(controller_nh); dynamic_reconfigure::Server::CallbackType cb = [this](auto&& PH1, auto&& PH2) { @@ -142,6 +143,12 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) break; } localHeat(time, period); + if (shoot_state_pub_->trylock()) + { + shoot_state_pub_->msg_.stamp = time; + shoot_state_pub_->msg_.state = state_; + shoot_state_pub_->unlockAndPublish(); + } for (auto& ctrl_friction_l : ctrls_friction_l_) ctrl_friction_l->update(time, period); for (auto& ctrl_friction_r : ctrls_friction_r_) @@ -250,7 +257,6 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) (time - last_block_time_).toSec() > config_.block_overtime) { normalize(); - state_ = PUSH; state_changed_ = true; ROS_INFO("[Shooter] Exit BLOCK"); @@ -287,6 +293,7 @@ void Controller::localHeat(const ros::Time& time, const ros::Duration& period) raise_flag_ = false; has_shoot_ = true; } + double friction_change_vel_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_vel_l_; last_vel_l_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); count_++; if (has_shoot_last_) @@ -296,17 +303,18 @@ void Controller::localHeat(const ros::Time& time, const ros::Duration& period) has_shoot_last_ = has_shoot_; if (count_ == 5) { - if (shoot_state_pub_->trylock()) + if (local_heat_state_pub_->trylock()) { - ROS_INFO("count:%d", count_); - shoot_state_pub_->msg_.has_shoot = has_shoot_; - shoot_state_pub_->msg_.stamp = time; - shoot_state_pub_->msg_.state = state_; - shoot_state_pub_->unlockAndPublish(); + local_heat_state_pub_->msg_.stamp = time; + local_heat_state_pub_->msg_.has_shoot = has_shoot_; + local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel_; + local_heat_state_pub_->unlockAndPublish(); } has_shoot_last_ = false; count_ = 0; } + if (has_shoot_) + has_shoot_ = false; } void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) { From 74f423a595c4e69d32eabea2a5b85d1bbdf93a34 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Wed, 8 May 2024 22:31:34 +0800 Subject: [PATCH 08/13] Change variable name. --- .../include/rm_shooter_controllers/standard.h | 6 ++--- rm_shooter_controllers/src/standard.cpp | 23 ++++++++++--------- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 66a44682..61f2bba8 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -77,7 +77,7 @@ class Controller : public controller_interface::MultiInterfaceControllertrylock()) { shoot_state_pub_->msg_.stamp = time; @@ -278,23 +278,24 @@ void Controller::normalize() push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle)); } -void Controller::localHeat(const ros::Time& time, const ros::Duration& period) +void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period) { - if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_vel_l_ > config_.wheel_speed_raise_threshold && drop_flag_) + if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold && + wheel_speed_drop_) { - raise_flag_ = true; - drop_flag_ = false; + wheel_speed_raise_ = true; + wheel_speed_drop_ = false; } - if (last_vel_l_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && raise_flag_) + if (last_wheel_speed_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && + abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && wheel_speed_raise_) { - drop_flag_ = true; - raise_flag_ = false; + wheel_speed_drop_ = true; + wheel_speed_raise_ = false; has_shoot_ = true; } - double friction_change_vel_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_vel_l_; - last_vel_l_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); + double friction_change_vel_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_; + last_wheel_speed_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); count_++; if (has_shoot_last_) { From 5bca06ebb82ab3d48bfdb9ca79bba93fbb0326a6 Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Mon, 10 Jun 2024 15:57:29 +0800 Subject: [PATCH 09/13] Change publishing frequency. --- rm_shooter_controllers/src/standard.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 8c4cbcb4..acefb4e8 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -61,8 +61,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - local_heat_state_pub_.reset( - new realtime_tools::RealtimePublisher(controller_nh, "/local_heat_state", 10)); + local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( + controller_nh, "/local_heat_state/shooter_state", 10)); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure d_srv_ = new dynamic_reconfigure::Server(controller_nh); @@ -294,7 +294,7 @@ void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& pe wheel_speed_raise_ = false; has_shoot_ = true; } - double friction_change_vel_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_; + double friction_change_vel = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_; last_wheel_speed_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); count_++; if (has_shoot_last_) @@ -302,13 +302,13 @@ void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& pe has_shoot_ = true; } has_shoot_last_ = has_shoot_; - if (count_ == 5) + if (count_ == 2) { if (local_heat_state_pub_->trylock()) { local_heat_state_pub_->msg_.stamp = time; local_heat_state_pub_->msg_.has_shoot = has_shoot_; - local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel_; + local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel; local_heat_state_pub_->unlockAndPublish(); } has_shoot_last_ = false; From c29b2b965b216bc7ca700839980a604d5377f33e Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Wed, 10 Jul 2024 09:06:04 +0800 Subject: [PATCH 10/13] Fix the issue of incorrect heat when closing the friction wheel. --- rm_shooter_controllers/src/standard.cpp | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index acefb4e8..70efdcb9 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -280,19 +280,22 @@ void Controller::normalize() void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period) { - if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold && - wheel_speed_drop_) + if (state_ != STOP) { - wheel_speed_raise_ = true; - wheel_speed_drop_ = false; - } + if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold && + wheel_speed_drop_) + { + wheel_speed_raise_ = true; + wheel_speed_drop_ = false; + } - if (last_wheel_speed_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && wheel_speed_raise_) - { - wheel_speed_drop_ = true; - wheel_speed_raise_ = false; - has_shoot_ = true; + if (last_wheel_speed_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && + abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && wheel_speed_raise_) + { + wheel_speed_drop_ = true; + wheel_speed_raise_ = false; + has_shoot_ = true; + } } double friction_change_vel = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_; last_wheel_speed_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); From 2b489035122d4d360d12b7b029c28bab013520e9 Mon Sep 17 00:00:00 2001 From: QiayuanLiao Date: Mon, 22 Jul 2024 14:48:45 -0700 Subject: [PATCH 11/13] Update package.xml --- rm_chassis_controllers/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index ac4b6d69..bed77aad 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -19,6 +19,7 @@ controller_interface effort_controllers tf2_geometry_msgs + nav_msgs angles robot_localization From 8ebbacb5c7c9a9bfa08bf5c5982bd44df5add149 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E7=8E=8B=E6=B9=98=E9=88=9C?= <91529566+d0h0s@users.noreply.github.com> Date: Thu, 25 Jul 2024 23:42:11 +0800 Subject: [PATCH 12/13] Revert "Calculate heat with friction." --- rm_shooter_controllers/cfg/Shooter.cfg | 2 - .../include/rm_shooter_controllers/standard.h | 12 +--- rm_shooter_controllers/src/standard.cpp | 56 +------------------ 3 files changed, 5 insertions(+), 65 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 9465c224..4e2a2719 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -14,7 +14,5 @@ gen.add("anti_block_threshold", double_t, 0, "Trigger anti block error threshold gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to push forward in push mode",0.01,0.0,1) gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) -gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999) -gen.add("wheel_speed_raise_threshold", double_t, 0, "Wheel speed raise threshold", 50.0, 0.0, 999) exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 61f2bba8..4cd20c85 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -48,7 +48,6 @@ #include #include #include -#include #include @@ -58,7 +57,7 @@ struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, forward_push_threshold, exit_push_threshold; - double extra_wheel_speed, wheel_speed_drop_threshold, wheel_speed_raise_threshold; + double extra_wheel_speed; }; class Controller : public controller_interface::MultiInterfaceController ctrls_friction_l_, ctrls_friction_r_; effort_controllers::JointPositionController ctrl_trigger_; std::vector wheel_speed_offset_l_, wheel_speed_offset_r_; - int push_per_rotation_{}, count_{}; + int push_per_rotation_{}; double push_wheel_speed_threshold_{}; double freq_threshold_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; bool maybe_block_ = false; - bool has_shoot_ = false, has_shoot_last_ = false; - bool wheel_speed_raise_ = true, wheel_speed_drop_ = true; - double last_wheel_speed_{}; - ros::Time last_shoot_time_, block_time_, last_block_time_; enum { @@ -113,7 +106,6 @@ class Controller : public controller_interface::MultiInterfaceController config_rt_buffer; realtime_tools::RealtimeBuffer cmd_rt_buffer_; rm_msgs::ShootCmd cmd_; - std::shared_ptr> local_heat_state_pub_; std::shared_ptr> shoot_state_pub_; ros::Subscriber cmd_subscriber_; dynamic_reconfigure::Server* d_srv_{}; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 70efdcb9..cd270d54 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -52,17 +52,13 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .anti_block_threshold = getParam(controller_nh, "anti_block_threshold", 0.), .forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1), .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1), - .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.), - .wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.), - .wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) }; + .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.) }; 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.); freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( - controller_nh, "/local_heat_state/shooter_state", 10)); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure d_srv_ = new dynamic_reconfigure::Server(controller_nh); @@ -142,7 +138,6 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) block(time, period); break; } - judgeBulletShoot(time, period); if (shoot_state_pub_->trylock()) { shoot_state_pub_->msg_.stamp = time; @@ -257,6 +252,7 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) (time - last_block_time_).toSec() > config_.block_overtime) { normalize(); + state_ = PUSH; state_changed_ = true; ROS_INFO("[Shooter] Exit BLOCK"); @@ -278,48 +274,6 @@ void Controller::normalize() push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle)); } -void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period) -{ - if (state_ != STOP) - { - if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold && - wheel_speed_drop_) - { - wheel_speed_raise_ = true; - wheel_speed_drop_ = false; - } - - if (last_wheel_speed_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && wheel_speed_raise_) - { - wheel_speed_drop_ = true; - wheel_speed_raise_ = false; - has_shoot_ = true; - } - } - double friction_change_vel = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_; - last_wheel_speed_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); - count_++; - if (has_shoot_last_) - { - has_shoot_ = true; - } - has_shoot_last_ = has_shoot_; - if (count_ == 2) - { - if (local_heat_state_pub_->trylock()) - { - local_heat_state_pub_->msg_.stamp = time; - local_heat_state_pub_->msg_.has_shoot = has_shoot_; - local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel; - local_heat_state_pub_->unlockAndPublish(); - } - has_shoot_last_ = false; - count_ = 0; - } - if (has_shoot_) - has_shoot_ = false; -} void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) { ROS_INFO("[Shooter] Dynamic params change"); @@ -335,8 +289,6 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 config.forward_push_threshold = init_config.forward_push_threshold; config.exit_push_threshold = init_config.exit_push_threshold; config.extra_wheel_speed = init_config.extra_wheel_speed; - config.wheel_speed_drop_threshold = init_config.wheel_speed_drop_threshold; - config.wheel_speed_raise_threshold = init_config.wheel_speed_raise_threshold; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .block_effort = config.block_effort, @@ -347,9 +299,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .anti_block_threshold = config.anti_block_threshold, .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, - .extra_wheel_speed = config.extra_wheel_speed, - .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold, - .wheel_speed_raise_threshold = config.wheel_speed_raise_threshold }; + .extra_wheel_speed = config.extra_wheel_speed }; config_rt_buffer.writeFromNonRT(config_non_rt); } From 3d3dedbd0d88d309516972f5dd9fbc83022df0bd Mon Sep 17 00:00:00 2001 From: BlanchardLj <1714148437@qq.com> Date: Fri, 26 Jul 2024 00:57:54 +0800 Subject: [PATCH 13/13] Fix pre-commit problem. --- rm_shooter_controllers/cfg/Shooter.cfg | 2 + .../include/rm_shooter_controllers/standard.h | 11 +++- rm_shooter_controllers/src/standard.cpp | 56 ++++++++++++++++++- 3 files changed, 64 insertions(+), 5 deletions(-) diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 4e2a2719..9465c224 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -14,5 +14,7 @@ gen.add("anti_block_threshold", double_t, 0, "Trigger anti block error threshold gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to push forward in push mode",0.01,0.0,1) gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) +gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999) +gen.add("wheel_speed_raise_threshold", double_t, 0, "Wheel speed raise threshold", 50.0, 0.0, 999) exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 4cd20c85..69bd2545 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -57,7 +58,7 @@ struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, forward_push_threshold, exit_push_threshold; - double extra_wheel_speed; + double extra_wheel_speed, wheel_speed_drop_threshold, wheel_speed_raise_threshold; }; class Controller : public controller_interface::MultiInterfaceController ctrls_friction_l_, ctrls_friction_r_; effort_controllers::JointPositionController ctrl_trigger_; std::vector wheel_speed_offset_l_, wheel_speed_offset_r_; - int push_per_rotation_{}; + int push_per_rotation_{}, count_{}; double push_wheel_speed_threshold_{}; double freq_threshold_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; bool maybe_block_ = false; + bool has_shoot_ = false, has_shoot_last_ = false; + bool wheel_speed_raise_ = true, wheel_speed_drop_ = true; + double last_wheel_speed_{}; ros::Time last_shoot_time_, block_time_, last_block_time_; enum @@ -106,6 +112,7 @@ class Controller : public controller_interface::MultiInterfaceController config_rt_buffer; realtime_tools::RealtimeBuffer cmd_rt_buffer_; rm_msgs::ShootCmd cmd_; + std::shared_ptr> local_heat_state_pub_; std::shared_ptr> shoot_state_pub_; ros::Subscriber cmd_subscriber_; dynamic_reconfigure::Server* d_srv_{}; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index cd270d54..70efdcb9 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -52,13 +52,17 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .anti_block_threshold = getParam(controller_nh, "anti_block_threshold", 0.), .forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1), .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1), - .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.) }; + .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.), + .wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.), + .wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) }; 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.); freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); + local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( + controller_nh, "/local_heat_state/shooter_state", 10)); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure d_srv_ = new dynamic_reconfigure::Server(controller_nh); @@ -138,6 +142,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) block(time, period); break; } + judgeBulletShoot(time, period); if (shoot_state_pub_->trylock()) { shoot_state_pub_->msg_.stamp = time; @@ -252,7 +257,6 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) (time - last_block_time_).toSec() > config_.block_overtime) { normalize(); - state_ = PUSH; state_changed_ = true; ROS_INFO("[Shooter] Exit BLOCK"); @@ -274,6 +278,48 @@ void Controller::normalize() push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle)); } +void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period) +{ + if (state_ != STOP) + { + if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold && + wheel_speed_drop_) + { + wheel_speed_raise_ = true; + wheel_speed_drop_ = false; + } + + if (last_wheel_speed_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && + abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && wheel_speed_raise_) + { + wheel_speed_drop_ = true; + wheel_speed_raise_ = false; + has_shoot_ = true; + } + } + double friction_change_vel = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_; + last_wheel_speed_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); + count_++; + if (has_shoot_last_) + { + has_shoot_ = true; + } + has_shoot_last_ = has_shoot_; + if (count_ == 2) + { + if (local_heat_state_pub_->trylock()) + { + local_heat_state_pub_->msg_.stamp = time; + local_heat_state_pub_->msg_.has_shoot = has_shoot_; + local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel; + local_heat_state_pub_->unlockAndPublish(); + } + has_shoot_last_ = false; + count_ = 0; + } + if (has_shoot_) + has_shoot_ = false; +} void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) { ROS_INFO("[Shooter] Dynamic params change"); @@ -289,6 +335,8 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 config.forward_push_threshold = init_config.forward_push_threshold; config.exit_push_threshold = init_config.exit_push_threshold; config.extra_wheel_speed = init_config.extra_wheel_speed; + config.wheel_speed_drop_threshold = init_config.wheel_speed_drop_threshold; + config.wheel_speed_raise_threshold = init_config.wheel_speed_raise_threshold; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .block_effort = config.block_effort, @@ -299,7 +347,9 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .anti_block_threshold = config.anti_block_threshold, .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, - .extra_wheel_speed = config.extra_wheel_speed }; + .extra_wheel_speed = config.extra_wheel_speed, + .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold, + .wheel_speed_raise_threshold = config.wheel_speed_raise_threshold }; config_rt_buffer.writeFromNonRT(config_non_rt); }