From edc57bd9c1dc9e4f7246dc37b83453eee6493a0f Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Tue, 8 Apr 2025 02:29:06 +0800 Subject: [PATCH 1/2] Fix the shooter controller logic wrong. --- .../include/rm_shooter_controllers/standard.h | 1 + rm_shooter_controllers/src/standard.cpp | 47 +++++++++---------- 2 files changed, 24 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 f2c34daa..e4e44c0d 100644 --- a/rm_shooter_controllers/include/rm_shooter_controllers/standard.h +++ b/rm_shooter_controllers/include/rm_shooter_controllers/standard.h @@ -89,6 +89,7 @@ class Controller : public controller_interface::MultiInterfaceController> ctrls_friction_; effort_controllers::JointPositionController ctrl_trigger_; std::vector> wheel_speed_offsets_; + std::vector> wheel_speed_directions_; int push_per_rotation_{}, count_{}; double push_wheel_speed_threshold_{}; double freq_threshold_{}; diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 5ea06cd6..5b1cbb39 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -77,16 +77,20 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro XmlRpc::XmlRpcValue friction; double wheel_speed_offset; + double wheel_speed_direction; effort_joint_interface_ = robot_hw->get(); controller_nh.getParam("friction", friction); for (const auto& its : friction) { std::vector wheel_speed_offset_temp; + std::vector wheel_speed_direction_temp; std::vector ctrl_frictions; for (const auto& it : its.second) { ros::NodeHandle nh = ros::NodeHandle(controller_nh, "friction/" + its.first + "/" + it.first); wheel_speed_offset_temp.push_back(nh.getParam("wheel_speed_offset", wheel_speed_offset) ? wheel_speed_offset : 0.); + wheel_speed_direction_temp.push_back( + nh.getParam("wheel_speed_direction", wheel_speed_direction) ? wheel_speed_direction : 1.); effort_controllers::JointVelocityController* ctrl_friction = new effort_controllers::JointVelocityController; if (ctrl_friction->init(effort_joint_interface_, nh)) ctrl_frictions.push_back(ctrl_friction); @@ -95,6 +99,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro } ctrls_friction_.push_back(ctrl_frictions); wheel_speed_offsets_.push_back(wheel_speed_offset_temp); + wheel_speed_directions_.push_back(wheel_speed_direction_temp); } ros::NodeHandle nh_trigger = ros::NodeHandle(controller_nh, "trigger"); return ctrl_trigger_.init(effort_joint_interface_, nh_trigger); @@ -194,12 +199,11 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) ROS_INFO("[Shooter] Enter PUSH"); } bool wheel_speed_ready = true; - for (auto& ctrl_frictions : ctrls_friction_) - { - for (auto& ctrl_friction : ctrl_frictions) - { - if (ctrl_friction->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrl_friction->command_ || - ctrl_friction->joint_.getVelocity() <= M_PI) + for (size_t i = 0; i < ctrls_friction_.size(); i++) { + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { + if (wheel_speed_directions_[i][j] * ctrls_friction_[i][j]->joint_.getVelocity() < + push_wheel_speed_threshold_ * ctrls_friction_[i][j]->command_ || + wheel_speed_directions_[i][j] * ctrls_friction_[i][j]->joint_.getVelocity() <= M_PI) wheel_speed_ready = false; } } @@ -268,12 +272,10 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { static int friction_block_count = 0; bool friction_wheel_block = false; - for (auto& ctrl_frictions : ctrls_friction_) - { - for (auto& ctrl_friction : ctrl_frictions) - { - if (ctrl_friction->joint_.getVelocity() <= friction_block_vel_ && - abs(ctrl_friction->joint_.getEffort()) >= friction_block_effort_ && cmd.wheel_speed != 0) + for (size_t i = 0; i < ctrls_friction_.size(); i++) { + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { + if (wheel_speed_directions_[i][j] * ctrls_friction_[i][j]->joint_.getVelocity() <= friction_block_vel_ && + abs(ctrls_friction_[i][j]->joint_.getEffort()) >= friction_block_effort_ && cmd.wheel_speed != 0) friction_wheel_block = true; } } @@ -283,20 +285,17 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { - ctrls_friction_[i][j]->setCommand(cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offsets_[i][j]); + ctrls_friction_[i][j]->setCommand(wheel_speed_directions_[i][j] * + (cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offsets_[i][j])); } } - } - else - { - double command = (friction_block_count <= static_cast(anti_friction_block_duty_cycle_ * 1000)) ? - anti_friction_block_vel_ : - 0.; - for (auto& ctrl_frictions : ctrls_friction_) - { - for (auto& ctrl_friction : ctrl_frictions) - { - ctrl_friction->setCommand(command); + } else { + double command = (friction_block_count <= static_cast(anti_friction_block_duty_cycle_ * 1000)) + ? anti_friction_block_vel_ + : 0.; + for (size_t i = 0; i < ctrls_friction_.size(); i++) { + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { + ctrls_friction_[i][j]->setCommand(command); } } friction_block_count = (friction_block_count + 1) % 1000; From db61b20b79cbb3fd2e59912bbddc842bfed7a2b7 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Tue, 8 Apr 2025 02:35:13 +0800 Subject: [PATCH 2/2] Fix code format. --- rm_shooter_controllers/src/standard.cpp | 38 +++++++++++++++---------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/rm_shooter_controllers/src/standard.cpp b/rm_shooter_controllers/src/standard.cpp index 5b1cbb39..0b0673e7 100644 --- a/rm_shooter_controllers/src/standard.cpp +++ b/rm_shooter_controllers/src/standard.cpp @@ -90,7 +90,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ros::NodeHandle nh = ros::NodeHandle(controller_nh, "friction/" + its.first + "/" + it.first); wheel_speed_offset_temp.push_back(nh.getParam("wheel_speed_offset", wheel_speed_offset) ? wheel_speed_offset : 0.); wheel_speed_direction_temp.push_back( - nh.getParam("wheel_speed_direction", wheel_speed_direction) ? wheel_speed_direction : 1.); + nh.getParam("wheel_speed_direction", wheel_speed_direction) ? wheel_speed_direction : 1.); effort_controllers::JointVelocityController* ctrl_friction = new effort_controllers::JointVelocityController; if (ctrl_friction->init(effort_joint_interface_, nh)) ctrl_frictions.push_back(ctrl_friction); @@ -199,11 +199,13 @@ void Controller::push(const ros::Time& time, const ros::Duration& period) ROS_INFO("[Shooter] Enter PUSH"); } bool wheel_speed_ready = true; - for (size_t i = 0; i < ctrls_friction_.size(); i++) { - for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { + for (size_t i = 0; i < ctrls_friction_.size(); i++) + { + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) + { if (wheel_speed_directions_[i][j] * ctrls_friction_[i][j]->joint_.getVelocity() < - push_wheel_speed_threshold_ * ctrls_friction_[i][j]->command_ || - wheel_speed_directions_[i][j] * ctrls_friction_[i][j]->joint_.getVelocity() <= M_PI) + push_wheel_speed_threshold_ * ctrls_friction_[i][j]->command_ || + wheel_speed_directions_[i][j] * ctrls_friction_[i][j]->joint_.getVelocity() <= M_PI) wheel_speed_ready = false; } } @@ -272,10 +274,12 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) { static int friction_block_count = 0; bool friction_wheel_block = false; - for (size_t i = 0; i < ctrls_friction_.size(); i++) { - for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { + for (size_t i = 0; i < ctrls_friction_.size(); i++) + { + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) + { if (wheel_speed_directions_[i][j] * ctrls_friction_[i][j]->joint_.getVelocity() <= friction_block_vel_ && - abs(ctrls_friction_[i][j]->joint_.getEffort()) >= friction_block_effort_ && cmd.wheel_speed != 0) + abs(ctrls_friction_[i][j]->joint_.getEffort()) >= friction_block_effort_ && cmd.wheel_speed != 0) friction_wheel_block = true; } } @@ -286,15 +290,19 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { ctrls_friction_[i][j]->setCommand(wheel_speed_directions_[i][j] * - (cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offsets_[i][j])); + (cmd_.wheel_speed + config_.extra_wheel_speed + wheel_speed_offsets_[i][j])); } } - } else { - double command = (friction_block_count <= static_cast(anti_friction_block_duty_cycle_ * 1000)) - ? anti_friction_block_vel_ - : 0.; - for (size_t i = 0; i < ctrls_friction_.size(); i++) { - for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { + } + else + { + double command = (friction_block_count <= static_cast(anti_friction_block_duty_cycle_ * 1000)) ? + anti_friction_block_vel_ : + 0.; + for (size_t i = 0; i < ctrls_friction_.size(); i++) + { + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) + { ctrls_friction_[i][j]->setCommand(command); } }