Add method to avoid friction wheels 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 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_{};

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)