Modify logic of friction block.

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 friction_block_effort_{}, friction_block_vel_{};
   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;

rm_shooter_controllers/src/standard.cpp

@@ -61,6 +61,8 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
   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.0);
+  friction_block_effort_ = getParam(controller_nh, "friction_block_effort", 0.2);
+  friction_block_vel_ = getParam(controller_nh, "friction_block_vel", 1.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>(
@@ -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<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);
-    }
-    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<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);
+    }
+    for (auto& ctrl_friction_r : ctrls_friction_r_)
+    {
+      ctrl_friction_r->setCommand(command);
+    }
+    friction_block_count = (friction_block_count + 1) % 1000;
+  }
 }
 
 void Controller::normalize()