diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 42a493a2..4cd20c85 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -88,7 +88,7 @@ class Controller : public controller_interface::MultiInterfaceController wheel_speed_offset_l_, wheel_speed_offset_r_; int push_per_rotation_{}; double push_wheel_speed_threshold_{}; - double high_shoot_frequency_{}; + double freq_threshold_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; bool maybe_block_ = false; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 0a61c890..cd270d54 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -56,7 +56,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro 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.); - high_shoot_frequency_ = getParam(controller_nh, "high_shoot_frequency", 20.); + freq_threshold_ = getParam(controller_nh, "freq_threshold", 20.); cmd_subscriber_ = controller_nh.subscribe("command", 1, &Controller::commandCB, this); shoot_state_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "state", 10)); @@ -114,7 +114,7 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) (cmd_.mode == READY && (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < config_.exit_push_threshold || - cmd_.hz >= high_shoot_frequency_))) + cmd_.hz >= freq_threshold_))) { state_ = cmd_.mode; state_changed_ = true; @@ -199,7 +199,7 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) } if ((cmd_.wheel_speed == 0. || wheel_speed_ready) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! - if (cmd_.hz >= high_shoot_frequency_) + if (cmd_.hz >= freq_threshold_) { ctrl_trigger_.setCommand(ctrl_trigger_.command_struct_.position_ - 2. * M_PI / static_cast(push_per_rotation_),