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..0b0673e7 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,13 @@ 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 (size_t i = 0; i < ctrls_friction_.size(); i++) { - for (auto& ctrl_friction : ctrl_frictions) + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { - if (ctrl_friction->joint_.getVelocity() < push_wheel_speed_threshold_ * ctrl_friction->command_ || - ctrl_friction->joint_.getVelocity() <= M_PI) + 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 +274,12 @@ 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 (size_t i = 0; i < ctrls_friction_.size(); i++) { - for (auto& ctrl_friction : ctrl_frictions) + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { - if (ctrl_friction->joint_.getVelocity() <= friction_block_vel_ && - abs(ctrl_friction->joint_.getEffort()) >= friction_block_effort_ && cmd.wheel_speed != 0) + 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,7 +289,8 @@ 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])); } } } @@ -292,11 +299,11 @@ void Controller::setSpeed(const rm_msgs::ShootCmd& cmd) 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 (size_t i = 0; i < ctrls_friction_.size(); i++) { - for (auto& ctrl_friction : ctrl_frictions) + for (size_t j = 0; j < ctrls_friction_[i].size(); j++) { - ctrl_friction->setCommand(command); + ctrls_friction_[i][j]->setCommand(command); } } friction_block_count = (friction_block_count + 1) % 1000;