From fb7513c76b1e184539835bcbd8845762685a44da Mon Sep 17 00:00:00 2001 From: youjian <1124895509@qq.com> Date: Mon, 29 Jul 2024 04:55:40 +0800 Subject: [PATCH 1/6] Add method to avoid friction wheels block. --- .../include/rm_shooter_controllers/standard.h | 1 + rm_shooter_controllers/src/standard.cpp | 16 ++++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 69bd2545..93629a44 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -95,6 +95,7 @@ class Controller : public controller_interface::MultiInterfaceController("command", 1, &Controller::commandCB, this); local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( @@ -184,6 +186,7 @@ void Controller::ready(const ros::Duration& period) void Controller::push(const ros::Time& time, const ros::Duration& period) { + static int friction_block_count = 0; if (state_changed_) { // on enter state_changed_ = false; @@ -240,7 +243,20 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) maybe_block_ = false; } else + { ROS_DEBUG("[Shooter] Wait for friction wheel"); + double command = (friction_block_count <= anti_friction_block_duty_cycle_ * 1000) ? anti_friction_block_vel_ : 0; + for (auto& ctrl_friction_l : ctrls_friction_l_) + { + ctrl_friction_l->setCommand(command); + } + for (auto& ctrl_friction_r : ctrls_friction_r_) + { + ctrl_friction_r->setCommand(command); + } + friction_block_count = (friction_block_count + 1) % 1000; + } +} } void Controller::block(const ros::Time& time, const ros::Duration& period) From 347333ee3ae4591077c1c7bb68c8ef976f784b72 Mon Sep 17 00:00:00 2001 From: youjian <1124895509@qq.com> Date: Mon, 29 Jul 2024 05:09:10 +0800 Subject: [PATCH 2/6] Fix decimal points missing. --- rm_shooter_controllers/src/standard.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index fa2de64e..0a055093 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -60,7 +60,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro push_wheel_speed_threshold_ = getParam(controller_nh, "push_wheel_speed_threshold", 0.); freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.); anti_friction_block_duty_cycle_ = getParam(controller_nh, "anti_friction_block_duty_cycle", 0.5); - anti_friction_block_vel_ = getParam(controller_nh, "anti_friction_block_vel", 810); + anti_friction_block_vel_ = getParam(controller_nh, "anti_friction_block_vel", 810.0); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( @@ -245,7 +245,9 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) else { ROS_DEBUG("[Shooter] Wait for friction wheel"); - double command = (friction_block_count <= anti_friction_block_duty_cycle_ * 1000) ? anti_friction_block_vel_ : 0; + double command = (friction_block_count <= static_cast(anti_friction_block_duty_cycle_ * 1000)) ? + anti_friction_block_vel_ : + 0.; for (auto& ctrl_friction_l : ctrls_friction_l_) { ctrl_friction_l->setCommand(command); From 2f58ec258eb5fed5982621e27247fcbc07d7506c Mon Sep 17 00:00:00 2001 From: youjian <1124895509@qq.com> Date: Mon, 29 Jul 2024 05:15:42 +0800 Subject: [PATCH 3/6] Run pre-commit. --- rm_shooter_controllers/src/standard.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 0a055093..349cb7c4 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -259,7 +259,6 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) friction_block_count = (friction_block_count + 1) % 1000; } } -} void Controller::block(const ros::Time& time, const ros::Duration& period) { From 8139399e4e362ebb4b68788d4534f8c733d59a14 Mon Sep 17 00:00:00 2001 From: youjian <1124895509@qq.com> Date: Mon, 29 Jul 2024 18:28:21 +0800 Subject: [PATCH 4/6] Modify logic of friction block. --- .../include/rm_shooter_controllers/standard.h | 1 + rm_shooter_controllers/src/standard.cpp | 57 ++++++++++++------- 2 files changed, 39 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 93629a44..5f26e5ca 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -95,6 +95,7 @@ class Controller : public controller_interface::MultiInterfaceController("command", 1, &Controller::commandCB, this); local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( @@ -186,7 +188,6 @@ void Controller::ready(const ros::Duration& period) void Controller::push(const ros::Time& time, const ros::Duration& period) { - static int friction_block_count = 0; if (state_changed_) { // on enter state_changed_ = false; @@ -243,21 +244,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) maybe_block_ = false; } else - { ROS_DEBUG("[Shooter] Wait for friction wheel"); - double command = (friction_block_count <= static_cast(anti_friction_block_duty_cycle_ * 1000)) ? - anti_friction_block_vel_ : - 0.; - for (auto& ctrl_friction_l : ctrls_friction_l_) - { - ctrl_friction_l->setCommand(command); - } - for (auto& ctrl_friction_r : ctrls_friction_r_) - { - ctrl_friction_r->setCommand(command); - } - friction_block_count = (friction_block_count + 1) % 1000; - } } void Controller::block(const ros::Time& time, const ros::Duration& period) @@ -282,10 +269,42 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { - for (size_t i = 0; i < ctrls_friction_l_.size(); i++) - ctrls_friction_l_[i]->setCommand(cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offset_l_[i]); - for (size_t i = 0; i < ctrls_friction_r_.size(); i++) - ctrls_friction_r_[i]->setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed - wheel_speed_offset_r_[i]); + static int friction_block_count = 0; + bool wheel_speed_ready = true; + for (auto& ctrl_friction_l : ctrls_friction_l_) + { + if (ctrl_friction_l->joint_.getVelocity() <= friction_block_vel_ && + abs(ctrl_friction_l->joint_.getEffort()) >= friction_block_effort_ && cmd.wheel_speed != 0) + wheel_speed_ready = false; + } + for (auto& ctrl_friction_r : ctrls_friction_r_) + { + if (ctrl_friction_r->joint_.getVelocity() >= -1.0 * friction_block_vel_ && + abs(ctrl_friction_r->joint_.getVelocity()) >= friction_block_effort_ && cmd.wheel_speed != 0) + wheel_speed_ready = false; + } + if (wheel_speed_ready) + { + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) + ctrls_friction_l_[i]->setCommand(cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offset_l_[i]); + for (size_t i = 0; i < ctrls_friction_r_.size(); i++) + ctrls_friction_r_[i]->setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed - wheel_speed_offset_r_[i]); + } + else + { + double command = (friction_block_count <= static_cast(anti_friction_block_duty_cycle_ * 1000)) ? + anti_friction_block_vel_ : + 0.; + for (auto& ctrl_friction_l : ctrls_friction_l_) + { + ctrl_friction_l->setCommand(command); + } + for (auto& ctrl_friction_r : ctrls_friction_r_) + { + ctrl_friction_r->setCommand(command); + } + friction_block_count = (friction_block_count + 1) % 1000; + } } void Controller::normalize() From 8f5ea5f61c153c9067a735eb5532f62cf6f1a80b Mon Sep 17 00:00:00 2001 From: youjian <1124895509@qq.com> Date: Mon, 29 Jul 2024 18:33:46 +0800 Subject: [PATCH 5/6] Modify the variable name. --- rm_shooter_controllers/src/standard.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index b05e3bc2..efbc98ac 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -270,20 +270,20 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { static int friction_block_count = 0; - bool wheel_speed_ready = true; + bool friction_wheel_block = false; for (auto& ctrl_friction_l : ctrls_friction_l_) { if (ctrl_friction_l->joint_.getVelocity() <= friction_block_vel_ && abs(ctrl_friction_l->joint_.getEffort()) >= friction_block_effort_ && cmd.wheel_speed != 0) - wheel_speed_ready = false; + friction_wheel_block = true; } for (auto& ctrl_friction_r : ctrls_friction_r_) { if (ctrl_friction_r->joint_.getVelocity() >= -1.0 * friction_block_vel_ && abs(ctrl_friction_r->joint_.getVelocity()) >= friction_block_effort_ && cmd.wheel_speed != 0) - wheel_speed_ready = false; + friction_wheel_block = true; } - if (wheel_speed_ready) + if (!friction_wheel_block) { for (size_t i = 0; i < ctrls_friction_l_.size(); i++) ctrls_friction_l_[i]->setCommand(cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offset_l_[i]); From 20dbe441af6e1043dcc2a80e2f5c789184b4a475 Mon Sep 17 00:00:00 2001 From: WUYOUJIAN <117335706+YoujianWu@users.noreply.github.com> Date: Tue, 30 Jul 2024 15:59:50 +0800 Subject: [PATCH 6/6] Fix logic bugs --- rm_shooter_controllers/src/standard.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index efbc98ac..70423055 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -280,7 +280,7 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) for (auto& ctrl_friction_r : ctrls_friction_r_) { if (ctrl_friction_r->joint_.getVelocity() >= -1.0 * friction_block_vel_ && - abs(ctrl_friction_r->joint_.getVelocity()) >= friction_block_effort_ && cmd.wheel_speed != 0) + abs(ctrl_friction_r->joint_.getEffort()) >= friction_block_effort_ && cmd.wheel_speed != 0) friction_wheel_block = true; } if (!friction_wheel_block)