diff --git a/rm_shooter_controllers/cfg/Shooter.cfg b/rm_shooter_controllers/cfg/Shooter.cfg index 4e2a2719..9465c224 100755 --- a/rm_shooter_controllers/cfg/Shooter.cfg +++ b/rm_shooter_controllers/cfg/Shooter.cfg @@ -14,5 +14,7 @@ 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 4cd20c85..69bd2545 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -57,7 +58,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; + double extra_wheel_speed, wheel_speed_drop_threshold, wheel_speed_raise_threshold; }; 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_{}; + int push_per_rotation_{}, count_{}; 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 @@ -106,6 +112,7 @@ 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 cd270d54..70efdcb9 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -52,13 +52,17 @@ 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.) }; + .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) }; 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); @@ -138,6 +142,7 @@ 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; @@ -252,7 +257,6 @@ 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"); @@ -274,6 +278,48 @@ 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"); @@ -289,6 +335,8 @@ 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, @@ -299,7 +347,9 @@ 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 }; + .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 }; config_rt_buffer.writeFromNonRT(config_non_rt); }