Skip to content

Commit

Permalink
Modify param_name.
Browse files Browse the repository at this point in the history
  • Loading branch information
liyixin135 committed Apr 11, 2024
1 parent 1b67b3e commit 8a4b9fa
Show file tree
Hide file tree
Showing 2 changed files with 4 additions and 4 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
std::vector<double> 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;
Expand Down
6 changes: 3 additions & 3 deletions rm_shooter_controllers/src/standard.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<rm_msgs::ShootCmd>("command", 1, &Controller::commandCB, this);
shoot_state_pub_.reset(new realtime_tools::RealtimePublisher<rm_msgs::ShootState>(controller_nh, "state", 10));
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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<double>(push_per_rotation_),
Expand Down

0 comments on commit 8a4b9fa

Please sign in to comment.