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/3] 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<hardwar
   bool dynamic_reconfig_initialized_ = false;
   bool state_changed_ = false;
   bool maybe_block_ = false;
+  double anti_friction_block_duty_cycle_{}, anti_friction_block_vel_{};
   bool has_shoot_ = false, has_shoot_last_ = false;
   bool wheel_speed_raise_ = true, wheel_speed_drop_ = true;
   double last_wheel_speed_{};
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index 70efdcb9..fa2de64e 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -59,6 +59,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
   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.);
+  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);
 
   cmd_subscriber_ = controller_nh.subscribe<rm_msgs::ShootCmd>("command", 1, &Controller::commandCB, this);
   local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::LocalHeatState>(
@@ -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/3] 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<rm_msgs::ShootCmd>("command", 1, &Controller::commandCB, this);
   local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::LocalHeatState>(
@@ -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<int>(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/3] 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)
 {