Modify something unreasonable.

rm-controls · Jan 18, 2024 · 99a9188 · 99a9188
1 parent a02d407
commit 99a9188
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 8 deletions.
diff --git 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
@@ -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)