diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 9465c224..4e2a2719 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -14,7 +14,5 @@ gen.add("anti_block_threshold", double_t, 0, "Trigger anti block error threshold gen.add("forward_push_threshold",double_t,0,"The trigger position threshold to push forward in push mode",0.01,0.0,1) gen.add("exit_push_threshold",double_t,0,"The trigger position threshold to exit push mode",0.02,0.0,1) gen.add("extra_wheel_speed", double_t, 0, "Friction wheel extra rotation speed", 0.0, -999, 999) -gen.add("wheel_speed_drop_threshold", double_t, 0, "Wheel speed drop threshold", 50.0, 0.0, 999) -gen.add("wheel_speed_raise_threshold", double_t, 0, "Wheel speed raise threshold", 50.0, 0.0, 999) exit(gen.generate(PACKAGE, "shooter", "Shooter")) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 61f2bba8..4cd20c85 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -48,7 +48,6 @@ #include #include #include -#include #include @@ -58,7 +57,7 @@ struct Config { double block_effort, block_speed, block_duration, block_overtime, anti_block_angle, anti_block_threshold, forward_push_threshold, exit_push_threshold; - double extra_wheel_speed, wheel_speed_drop_threshold, wheel_speed_raise_threshold; + double extra_wheel_speed; }; class Controller : public controller_interface::MultiInterfaceController ctrls_friction_l_, ctrls_friction_r_; effort_controllers::JointPositionController ctrl_trigger_; std::vector wheel_speed_offset_l_, wheel_speed_offset_r_; - int push_per_rotation_{}, count_{}; + int push_per_rotation_{}; double push_wheel_speed_threshold_{}; double freq_threshold_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; bool maybe_block_ = false; - bool has_shoot_ = false, has_shoot_last_ = false; - bool wheel_speed_raise_ = true, wheel_speed_drop_ = true; - double last_wheel_speed_{}; - ros::Time last_shoot_time_, block_time_, last_block_time_; enum { @@ -113,7 +106,6 @@ class Controller : public controller_interface::MultiInterfaceController config_rt_buffer; realtime_tools::RealtimeBuffer cmd_rt_buffer_; rm_msgs::ShootCmd cmd_; - std::shared_ptr> local_heat_state_pub_; std::shared_ptr> shoot_state_pub_; ros::Subscriber cmd_subscriber_; dynamic_reconfigure::Server* d_srv_{}; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 70efdcb9..cd270d54 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -52,17 +52,13 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro .anti_block_threshold = getParam(controller_nh, "anti_block_threshold", 0.), .forward_push_threshold = getParam(controller_nh, "forward_push_threshold", 0.1), .exit_push_threshold = getParam(controller_nh, "exit_push_threshold", 0.1), - .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.), - .wheel_speed_drop_threshold = getParam(controller_nh, "wheel_speed_drop_threshold", 10.), - .wheel_speed_raise_threshold = getParam(controller_nh, "wheel_speed_raise_threshold", 3.1) }; + .extra_wheel_speed = getParam(controller_nh, "extra_wheel_speed", 0.) }; config_rt_buffer.initRT(config_); 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.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); - local_heat_state_pub_.reset(new realtime_tools::RealtimePublisher( - controller_nh, "/local_heat_state/shooter_state", 10)); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); // Init dynamic reconfigure d_srv_ = new dynamic_reconfigure::Server(controller_nh); @@ -142,7 +138,6 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) block(time, period); break; } - judgeBulletShoot(time, period); if (shoot_state_pub_->trylock()) { shoot_state_pub_->msg_.stamp = time; @@ -257,6 +252,7 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) (time - last_block_time_).toSec() > config_.block_overtime) { normalize(); + state_ = PUSH; state_changed_ = true; ROS_INFO("[Shooter] Exit BLOCK"); @@ -278,48 +274,6 @@ void Controller::normalize() push_angle * std::floor((ctrl_trigger_.joint_.getPosition() + 0.01 + config_.exit_push_threshold) / push_angle)); } -void Controller::judgeBulletShoot(const ros::Time& time, const ros::Duration& period) -{ - if (state_ != STOP) - { - if (abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_ > config_.wheel_speed_raise_threshold && - wheel_speed_drop_) - { - wheel_speed_raise_ = true; - wheel_speed_drop_ = false; - } - - if (last_wheel_speed_ - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > config_.wheel_speed_drop_threshold && - abs(ctrls_friction_l_[0]->joint_.getVelocity()) > 300. && wheel_speed_raise_) - { - wheel_speed_drop_ = true; - wheel_speed_raise_ = false; - has_shoot_ = true; - } - } - double friction_change_vel = abs(ctrls_friction_l_[0]->joint_.getVelocity()) - last_wheel_speed_; - last_wheel_speed_ = abs(ctrls_friction_l_[0]->joint_.getVelocity()); - count_++; - if (has_shoot_last_) - { - has_shoot_ = true; - } - has_shoot_last_ = has_shoot_; - if (count_ == 2) - { - if (local_heat_state_pub_->trylock()) - { - local_heat_state_pub_->msg_.stamp = time; - local_heat_state_pub_->msg_.has_shoot = has_shoot_; - local_heat_state_pub_->msg_.friction_change_vel = friction_change_vel; - local_heat_state_pub_->unlockAndPublish(); - } - has_shoot_last_ = false; - count_ = 0; - } - if (has_shoot_) - has_shoot_ = false; -} void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/) { ROS_INFO("[Shooter] Dynamic params change"); @@ -335,8 +289,6 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 config.forward_push_threshold = init_config.forward_push_threshold; config.exit_push_threshold = init_config.exit_push_threshold; config.extra_wheel_speed = init_config.extra_wheel_speed; - config.wheel_speed_drop_threshold = init_config.wheel_speed_drop_threshold; - config.wheel_speed_raise_threshold = init_config.wheel_speed_raise_threshold; dynamic_reconfig_initialized_ = true; } Config config_non_rt{ .block_effort = config.block_effort, @@ -347,9 +299,7 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 .anti_block_threshold = config.anti_block_threshold, .forward_push_threshold = config.forward_push_threshold, .exit_push_threshold = config.exit_push_threshold, - .extra_wheel_speed = config.extra_wheel_speed, - .wheel_speed_drop_threshold = config.wheel_speed_drop_threshold, - .wheel_speed_raise_threshold = config.wheel_speed_raise_threshold }; + .extra_wheel_speed = config.extra_wheel_speed }; config_rt_buffer.writeFromNonRT(config_non_rt); }