From ea5df94ee72208befab2b9eca2a6a65a4bb4fec3 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 16 Jan 2024 16:11:56 +0800 Subject: [PATCH 1/9] Achieved double stage friction wheel shooting. --- .../include/rm_shooter_controllers/standard.h | 7 +- rm_shooter_controllers/src/standard.cpp | 74 ++++++++++++++----- 2 files changed, 62 insertions(+), 19 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 0c9b1328..b20f3cfb 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -83,13 +83,18 @@ class Controller : public controller_interface::MultiInterfaceController ctrls_friction_l_, ctrls_friction_r_; effort_controllers::JointPositionController ctrl_trigger_; int push_per_rotation_{}; + double offset_wheel_speed_{}; double push_wheel_speed_threshold_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; bool maybe_block_ = false; + bool is_rotate_ = false; + bool friction_left_init_state_ = false; + bool friction_right_init_state_ = false; ros::Time last_shoot_time_, block_time_, last_block_time_; enum diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index ec77bfde..c85b8604 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -66,13 +66,30 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro }; d_srv_->setCallback(cb); - ros::NodeHandle nh_friction_l = ros::NodeHandle(controller_nh, "friction_left"); - ros::NodeHandle nh_friction_r = ros::NodeHandle(controller_nh, "friction_right"); - ros::NodeHandle nh_trigger = ros::NodeHandle(controller_nh, "trigger"); effort_joint_interface_ = robot_hw->get(); - return ctrl_friction_l_.init(effort_joint_interface_, nh_friction_l) && - ctrl_friction_r_.init(effort_joint_interface_, nh_friction_r) && - ctrl_trigger_.init(effort_joint_interface_, nh_trigger); + 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))) + ctrls_friction_l_.push_back(ctrl_friction_l); + else + return false; + } + 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))) + ctrls_friction_r_.push_back(ctrl_friction_r); + else + return false; + } + + ros::NodeHandle nh_trigger = ros::NodeHandle(controller_nh, "trigger"); + return ctrl_trigger_.init(effort_joint_interface_, nh_trigger); } void Controller::starting(const ros::Time& /*time*/) @@ -121,8 +138,11 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) shoot_state_pub_->msg_.state = state_; shoot_state_pub_->unlockAndPublish(); } - ctrl_friction_l_.update(time, period); - ctrl_friction_r_.update(time, period); + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) + { + ctrls_friction_l_[i]->update(time, period); + ctrls_friction_r_[i]->update(time, period); + } ctrl_trigger_.update(time, period); } @@ -133,8 +153,11 @@ void Controller::stop(const ros::Time& time, const ros::Duration& period) state_changed_ = false; ROS_INFO("[Shooter] Enter STOP"); - ctrl_friction_l_.setCommand(0.); - ctrl_friction_r_.setCommand(0.); + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) + { + ctrls_friction_l_[i]->setCommand(0.); + ctrls_friction_r_[i]->setCommand(0.); + } ctrl_trigger_.setCommand(ctrl_trigger_.joint_.getPosition()); } } @@ -157,12 +180,20 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) state_changed_ = false; ROS_INFO("[Shooter] Enter PUSH"); } - if ((cmd_.wheel_speed == 0. || - (ctrl_friction_l_.joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l_.command_ && - ctrl_friction_l_.joint_.getVelocity() > M_PI && - ctrl_friction_r_.joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r_.command_ && - ctrl_friction_r_.joint_.getVelocity() < -M_PI)) && - (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) + { + if (cmd_.wheel_speed == 0. || + (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; + } + else + is_rotate_ = false; + } + if (is_rotate_ && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < config_.forward_push_threshold) @@ -219,8 +250,15 @@ void Controller::block(const ros::Time& time, const ros::Duration& period) void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { - ctrl_friction_l_.setCommand(cmd_.wheel_speed + config_.extra_wheel_speed); - ctrl_friction_r_.setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed); + for (size_t i = 0; i < ctrls_friction_l_.size(); i++) + { + if (i == 1) + offset_wheel_speed_ = 220; + else + offset_wheel_speed_ = 0; + 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_); + } } void Controller::normalize() From a02d407fa853dfbe7231e9309024d4a125a66fa2 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 17 Jan 2024 21:21:48 +0800 Subject: [PATCH 2/9] Modify code specifications. --- .../include/rm_shooter_controllers/standard.h | 4 +- rm_shooter_controllers/src/standard.cpp | 51 +++++++++++-------- 2 files changed, 31 insertions(+), 24 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index b20f3cfb..48256622 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -83,7 +83,6 @@ class Controller : public controller_interface::MultiInterfaceController ctrls_friction_l_, ctrls_friction_r_; effort_controllers::JointPositionController ctrl_trigger_; int push_per_rotation_{}; @@ -92,7 +91,8 @@ class Controller : public controller_interface::MultiInterfaceControllersetCallback(cb); effort_joint_interface_ = robot_hw->get(); - controller_nh.getParam("friction_left", friction_left_); - controller_nh.getParam("friction_right", friction_right_); - for (auto it : friction_left_) + XmlRpc::XmlRpcValue friction_left, friction_right; + controller_nh.getParam("friction_left", friction_left); + controller_nh.getParam("friction_right", friction_right); + for (const 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; @@ -78,7 +79,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro else return false; } - for (auto it : friction_right_) + for (const 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; @@ -138,10 +139,13 @@ 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 < ctrls_friction_l_.size(); i++) + for (auto& ctrl_friction_l : ctrls_friction_l_) + { + ctrl_friction_l->update(time, period); + } + for (auto& ctrl_friction_r : ctrls_friction_r_) { - ctrls_friction_l_[i]->update(time, period); - ctrls_friction_r_[i]->update(time, period); + ctrl_friction_r->update(time, period); } ctrl_trigger_.update(time, period); } @@ -153,10 +157,13 @@ 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 < ctrls_friction_l_.size(); i++) + for (auto& ctrl_friction_l : ctrls_friction_l_) { - ctrls_friction_l_[i]->setCommand(0.); - ctrls_friction_r_[i]->setCommand(0.); + ctrl_friction_l->setCommand(0.); + } + for (auto& ctrl_friction_r : ctrls_friction_r_) + { + ctrl_friction_r->setCommand(0.); } ctrl_trigger_.setCommand(ctrl_trigger_.joint_.getPosition()); } @@ -180,20 +187,20 @@ 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 < ctrls_friction_l_.size(); i++) + for (auto& ctrl_friction_l : ctrls_friction_l_) { - if (cmd_.wheel_speed == 0. || - (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; - } - else - is_rotate_ = false; + friction_left_rotate_state_ = + ctrl_friction_l->joint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l->command_ && + ctrl_friction_l->joint_.getVelocity() > M_PI; + } + for (auto& ctrl_friction_r : ctrls_friction_r_) + { + friction_right_rotate_state_ = + ctrl_friction_r->joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r->command_ && + ctrl_friction_r->joint_.getVelocity() < -M_PI; } - if (is_rotate_ && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) + if ((cmd_.wheel_speed == 0. || (friction_left_rotate_state_ && friction_right_rotate_state_)) && + (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < config_.forward_push_threshold) From 99a91884334b90c2147e8e01b5976ad1fef68124 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 18 Jan 2024 10:45:07 +0800 Subject: [PATCH 3/9] Modify something unreasonable. --- .../include/rm_shooter_controllers/standard.h | 4 ++-- rm_shooter_controllers/src/standard.cpp | 18 ++++++++++++------ 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 48256622..cbc4f3af 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -91,8 +91,8 @@ class Controller : public controller_interface::MultiInterfaceControllerjoint_.getVelocity() >= push_wheel_speed_threshold_ * ctrl_friction_l->command_ && - ctrl_friction_l->joint_.getVelocity() > M_PI; + if (ctrl_friction_l->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrl_friction_l->command_ || + ctrl_friction_l->joint_.getVelocity() <= M_PI) + { + friction_left_rotate_state_ = false; + break; + } } for (auto& ctrl_friction_r : ctrls_friction_r_) { - friction_right_rotate_state_ = - ctrl_friction_r->joint_.getVelocity() <= push_wheel_speed_threshold_ * ctrl_friction_r->command_ && - ctrl_friction_r->joint_.getVelocity() < -M_PI; + if (ctrl_friction_r->joint_.getVelocity() > push_wheel_speed_threshold_ * ctrl_friction_r->command_ || + ctrl_friction_r->joint_.getVelocity() >= -M_PI) + { + friction_right_rotate_state_ = false; + break; + } } if ((cmd_.wheel_speed == 0. || (friction_left_rotate_state_ && friction_right_rotate_state_)) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) From 5d531fa850303eb5e0e69606646627380182d941 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 23 Jan 2024 17:41:07 +0800 Subject: [PATCH 4/9] Remove flag bits. --- .../include/rm_shooter_controllers/standard.h | 2 -- rm_shooter_controllers/src/standard.cpp | 4 ++-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index cbc4f3af..5b8e0d73 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -93,8 +93,6 @@ class Controller : public controller_interface::MultiInterfaceControllerinit(effort_joint_interface_, nh))) + if (ctrl_friction_l->init(effort_joint_interface_, nh)) ctrls_friction_l_.push_back(ctrl_friction_l); else return false; @@ -83,7 +83,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro { 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))) + if (ctrl_friction_r->init(effort_joint_interface_, nh)) ctrls_friction_r_.push_back(ctrl_friction_r); else return false; From 5c31a637cd9c655123f221bd5ac61e0ca9e66ac2 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Tue, 23 Jan 2024 17:50:59 +0800 Subject: [PATCH 5/9] Modify code specifications. --- rm_shooter_controllers/src/standard.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 443c5cf7..6a2c5425 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -140,13 +140,9 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) shoot_state_pub_->unlockAndPublish(); } for (auto& ctrl_friction_l : ctrls_friction_l_) - { ctrl_friction_l->update(time, period); - } for (auto& ctrl_friction_r : ctrls_friction_r_) - { ctrl_friction_r->update(time, period); - } ctrl_trigger_.update(time, period); } @@ -158,13 +154,9 @@ void Controller::stop(const ros::Time& time, const ros::Duration& period) ROS_INFO("[Shooter] Enter STOP"); for (auto& ctrl_friction_l : ctrls_friction_l_) - { ctrl_friction_l->setCommand(0.); - } for (auto& ctrl_friction_r : ctrls_friction_r_) - { ctrl_friction_r->setCommand(0.); - } ctrl_trigger_.setCommand(ctrl_trigger_.joint_.getPosition()); } } From 338919bf0af9395e44ba0092c07048c8cfea6bbf Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 24 Jan 2024 15:24:24 +0800 Subject: [PATCH 6/9] Realize the speed difference between front and rear friction wheels. --- .../include/rm_shooter_controllers/standard.h | 2 +- rm_shooter_controllers/src/standard.cpp | 14 ++++++-------- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 5b8e0d73..9338b9a5 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -85,8 +85,8 @@ 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_{}; - double offset_wheel_speed_{}; double push_wheel_speed_threshold_{}; bool dynamic_reconfig_initialized_ = false; bool state_changed_ = false; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 6a2c5425..8f3f4d23 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -70,9 +70,11 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro XmlRpc::XmlRpcValue friction_left, friction_right; controller_nh.getParam("friction_left", friction_left); controller_nh.getParam("friction_right", friction_right); + double wheel_speed_offset; for (const auto& it : friction_left) { ros::NodeHandle nh = ros::NodeHandle(controller_nh, "friction_left/" + it.first); + wheel_speed_offset_l_.push_back(nh.getParam("wheel_speed_offset", wheel_speed_offset) ? wheel_speed_offset : 0.); effort_controllers::JointVelocityController* ctrl_friction_l = new effort_controllers::JointVelocityController; if (ctrl_friction_l->init(effort_joint_interface_, nh)) ctrls_friction_l_.push_back(ctrl_friction_l); @@ -82,6 +84,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro for (const auto& it : friction_right) { ros::NodeHandle nh = ros::NodeHandle(controller_nh, "friction_right/" + it.first); + wheel_speed_offset_r_.push_back(nh.getParam("wheel_speed_offset", wheel_speed_offset) ? wheel_speed_offset : 0.); effort_controllers::JointVelocityController* ctrl_friction_r = new effort_controllers::JointVelocityController; if (ctrl_friction_r->init(effort_joint_interface_, nh)) ctrls_friction_r_.push_back(ctrl_friction_r); @@ -256,14 +259,9 @@ 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 < ctrls_friction_l_.size(); i++) - { - if (i == 1) - offset_wheel_speed_ = 220; - else - offset_wheel_speed_ = 0; - 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_); - } + ctrls_friction_l_[i]->setCommand(cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offset_l_[i]); + for (size_t i = 0; i < ctrls_friction_r_.size(); i++) + ctrls_friction_r_[i]->setCommand(-cmd_.wheel_speed - config_.extra_wheel_speed - wheel_speed_offset_r_[i]); } void Controller::normalize() From 60a22a7763602b3cdc07040143b1cdb8c720d79f Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Mon, 29 Jan 2024 12:10:41 +0800 Subject: [PATCH 7/9] Modify frictions_rotate's flag bit. --- .../include/rm_shooter_controllers/standard.h | 2 -- rm_shooter_controllers/src/standard.cpp | 8 ++++---- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h index 9338b9a5..be0515b9 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -91,8 +91,6 @@ class Controller : public controller_interface::MultiInterfaceControllerjoint_.getVelocity() < push_wheel_speed_threshold_ * ctrl_friction_l->command_ || ctrl_friction_l->joint_.getVelocity() <= M_PI) { - friction_left_rotate_state_ = false; + friction_rotate_state = false; break; } } @@ -196,12 +197,11 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) if (ctrl_friction_r->joint_.getVelocity() > push_wheel_speed_threshold_ * ctrl_friction_r->command_ || ctrl_friction_r->joint_.getVelocity() >= -M_PI) { - friction_right_rotate_state_ = false; + friction_rotate_state = false; break; } } - if ((cmd_.wheel_speed == 0. || (friction_left_rotate_state_ && friction_right_rotate_state_)) && - (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) + if ((cmd_.wheel_speed == 0. || friction_rotate_state) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < config_.forward_push_threshold) From 471fc7907e4dc42372f672852f3be17e3cad4d71 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sun, 25 Feb 2024 15:07:07 +0800 Subject: [PATCH 8/9] Delete something unnecessary. --- rm_shooter_controllers/src/standard.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 0c24df09..137193d3 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -187,19 +187,13 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) { if (ctrl_friction_l->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrl_friction_l->command_ || ctrl_friction_l->joint_.getVelocity() <= M_PI) - { friction_rotate_state = false; - break; - } } for (auto& ctrl_friction_r : ctrls_friction_r_) { if (ctrl_friction_r->joint_.getVelocity() > push_wheel_speed_threshold_ * ctrl_friction_r->command_ || ctrl_friction_r->joint_.getVelocity() >= -M_PI) - { friction_rotate_state = false; - break; - } } if ((cmd_.wheel_speed == 0. || friction_rotate_state) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! From 67d55feb35b5285f178e38b1f21b34f540a96a98 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sun, 3 Mar 2024 17:24:57 +0800 Subject: [PATCH 9/9] Modify some code specifications. --- rm_shooter_controllers/src/standard.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 137193d3..d8db8893 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -177,25 +177,25 @@ void Controller::ready(const ros::Duration& period) void Controller::push(const ros::Time& time, const ros::Duration& period) { - bool friction_rotate_state = true; if (state_changed_) { // on enter state_changed_ = false; ROS_INFO("[Shooter] Enter PUSH"); } + bool wheel_speed_ready = true; for (auto& ctrl_friction_l : ctrls_friction_l_) { if (ctrl_friction_l->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrl_friction_l->command_ || ctrl_friction_l->joint_.getVelocity() <= M_PI) - friction_rotate_state = false; + wheel_speed_ready = false; } for (auto& ctrl_friction_r : ctrls_friction_r_) { if (ctrl_friction_r->joint_.getVelocity() > push_wheel_speed_threshold_ * ctrl_friction_r->command_ || ctrl_friction_r->joint_.getVelocity() >= -M_PI) - friction_rotate_state = false; + wheel_speed_ready = false; } - if ((cmd_.wheel_speed == 0. || friction_rotate_state) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) + if ((cmd_.wheel_speed == 0. || wheel_speed_ready) && (time - last_shoot_time_).toSec() >= 1. / cmd_.hz) { // Time to shoot!!! if (std::fmod(std::abs(ctrl_trigger_.command_struct_.position_ - ctrl_trigger_.getPosition()), 2. * M_PI) < config_.forward_push_threshold)