From 99a91884334b90c2147e8e01b5976ad1fef68124 Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Thu, 18 Jan 2024 10:45:07 +0800
Subject: [PATCH] Modify something unreasonable.

---
 .../include/rm_shooter_controllers/standard.h  |  4 ++--
 rm_shooter_controllers/src/standard.cpp        | 18 ++++++++++++------
 2 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
index 48256622..cbc4f3af 100644
--- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
+++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h
@@ -91,8 +91,8 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
   bool dynamic_reconfig_initialized_ = false;
   bool state_changed_ = false;
   bool maybe_block_ = false;
-  bool friction_left_rotate_state_ = false;
-  bool friction_right_rotate_state_ = false;
+  bool friction_left_rotate_state_ = true;
+  bool friction_right_rotate_state_ = true;
   bool friction_left_init_state_ = false;
   bool friction_right_init_state_ = false;
 
diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp
index 9a01cc15..895fad56 100644
--- a/rm_shooter_controllers/src/standard.cpp
+++ b/rm_shooter_controllers/src/standard.cpp
@@ -189,15 +189,21 @@ void Controller::push(const ros::Time& time, const ros::Duration& period)
   }
   for (auto& ctrl_friction_l : ctrls_friction_l_)
   {
-    friction_left_rotate_state_ =
-        ctrl_friction_l->joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l->command_ &&
-        ctrl_friction_l->joint_.getVelocity() > M_PI;
+    if (ctrl_friction_l->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrl_friction_l->command_ ||
+        ctrl_friction_l->joint_.getVelocity() <= M_PI)
+    {
+      friction_left_rotate_state_ = false;
+      break;
+    }
   }
   for (auto& ctrl_friction_r : ctrls_friction_r_)
   {
-    friction_right_rotate_state_ =
-        ctrl_friction_r->joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r->command_ &&
-        ctrl_friction_r->joint_.getVelocity() < -M_PI;
+    if (ctrl_friction_r->joint_.getVelocity() > push_wheel_speed_threshold_ * ctrl_friction_r->command_ ||
+        ctrl_friction_r->joint_.getVelocity() >= -M_PI)
+    {
+      friction_right_rotate_state_ = false;
+      break;
+    }
   }
   if ((cmd_.wheel_speed == 0. || (friction_left_rotate_state_ && friction_right_rotate_state_)) &&
       (time - last_shoot_time_).toSec() >= 1. / cmd_.hz)