From 008a898784c15c0e2a31bfb5a9e32532371e3717 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 16 Jan 2024 15:50:37 +0800 Subject: [PATCH] Modify variable position and name. --- .../include/rm_shooter_controllers/standard.h | 7 ++- rm_shooter_controllers/src/standard.cpp | 50 +++++++++---------- 2 files changed, 29 insertions(+), 28 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 9f834c4a..7d0aa5cd 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -83,7 +83,8 @@ class Controller : public controller_interface::MultiInterfaceController ctrl_friction_l_, ctrl_friction_r_; + XmlRpc::XmlRpcValue friction_left_, friction_right_; + std::vector ctrls_friction_l_, ctrls_friction_r_; effort_controllers::JointPositionController ctrl_trigger_; int push_per_rotation_{}; double push_wheel_speed_threshold_{}; @@ -92,6 +93,8 @@ class Controller : public controller_interface::MultiInterfaceController* d_srv_{}; }; -} // namespace rm_shooter_controllers +} // namespace rm_shooter_controllers \ No newline at end of file diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 54ebb14b..e4376469 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -67,26 +67,24 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro d_srv_->setCallback(cb); effort_joint_interface_ = robot_hw->get(); - XmlRpc::XmlRpcValue friction_left, friction_right; - bool friction_left_init_state = false; - bool friction_right_init_state = false; - controller_nh.getParam("friction_left", friction_left); - controller_nh.getParam("friction_right", friction_right); - for (auto it : friction_left) + + controller_nh.getParam("friction_left", friction_left_); + controller_nh.getParam("friction_right", friction_right_); + for (auto it : friction_left_) { ros::NodeHandle nh = ros::NodeHandle(controller_nh, "friction_left/" + it.first); effort_controllers::JointVelocityController* ctrl_friction_l = new effort_controllers::JointVelocityController; - if (!(friction_left_init_state &= ctrl_friction_l->init(effort_joint_interface_, nh))) - ctrl_friction_l_.push_back(ctrl_friction_l); + if (!(friction_left_init_state_ &= ctrl_friction_l->init(effort_joint_interface_, nh))) + ctrls_friction_l_.push_back(ctrl_friction_l); else return false; } - for (auto it : friction_right) + for (auto it : friction_right_) { ros::NodeHandle nh = ros::NodeHandle(controller_nh, "friction_right/" + it.first); effort_controllers::JointVelocityController* ctrl_friction_r = new effort_controllers::JointVelocityController; - if (!(friction_right_init_state &= ctrl_friction_r->init(effort_joint_interface_, nh))) - ctrl_friction_r_.push_back(ctrl_friction_r); + if (!(friction_right_init_state_ &= ctrl_friction_r->init(effort_joint_interface_, nh))) + ctrls_friction_r_.push_back(ctrl_friction_r); else return false; } @@ -141,10 +139,10 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) shoot_state_pub_->msg_.state = state_; shoot_state_pub_->unlockAndPublish(); } - for (size_t i = 0; i < ctrl_friction_l_.size(); i++) + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) { - ctrl_friction_l_[i]->setCommand(0.); - ctrl_friction_r_[i]->setCommand(0.); + ctrls_friction_l_[i]->update(time, period); + ctrls_friction_r_[i]->update(time, period); } ctrl_trigger_.update(time, period); } @@ -156,10 +154,10 @@ void Controller::stop(const ros::Time& time, const ros::Duration& period) state_changed_ = false; ROS_INFO("[Shooter] Enter STOP"); - for (size_t i = 0; i < ctrl_friction_l_.size(); i++) + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) { - ctrl_friction_l_[i]->setCommand(0.); - ctrl_friction_r_[i]->setCommand(0.); + ctrls_friction_l_[i]->setCommand(0.); + ctrls_friction_r_[i]->setCommand(0.); } ctrl_trigger_.setCommand(ctrl_trigger_.joint_.getPosition()); } @@ -183,13 +181,13 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) state_changed_ = false; ROS_INFO("[Shooter] Enter PUSH"); } - for (size_t i = 0; i < ctrl_friction_l_.size(); i++) + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) { if (cmd_.wheel_speed == 0. || - (ctrl_friction_l_[i]->joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l_[i]->command_ && - ctrl_friction_l_[i]->joint_.getVelocity() > M_PI && - ctrl_friction_r_[i]->joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r_[i]->command_ && - ctrl_friction_r_[i]->joint_.getVelocity() < -M_PI)) + (ctrls_friction_l_[i]->joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrls_friction_l_[i]->command_ && + ctrls_friction_l_[i]->joint_.getVelocity() > M_PI && + ctrls_friction_r_[i]->joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrls_friction_r_[i]->command_ && + ctrls_friction_r_[i]->joint_.getVelocity() < -M_PI)) { is_rotate_ = true; } @@ -253,14 +251,14 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { - for (size_t i = 0; i < ctrl_friction_l_.size(); i++) + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) { if (i == 1) offset_wheel_speed_ = 220; else offset_wheel_speed_ = 0; - ctrl_friction_l_[i]->setCommand(cmd_.wheel_speed + config_.extra_wheel_speed - offset_wheel_speed_); - ctrl_friction_r_[i]->setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed + offset_wheel_speed_); + ctrls_friction_l_[i]->setCommand(cmd_.wheel_speed + config_.extra_wheel_speed - offset_wheel_speed_); + ctrls_friction_r_[i]->setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed + offset_wheel_speed_); } } @@ -301,4 +299,4 @@ void Controller::reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint3 } // namespace rm_shooter_controllers -PLUGINLIB_EXPORT_CLASS(rm_shooter_controllers::Controller, controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(rm_shooter_controllers::Controller, controller_interface::ControllerBase) \ No newline at end of file