Skip to content

Commit

Permalink
Modify variable position and name.
Browse files Browse the repository at this point in the history
  • Loading branch information
@liyixin135
liyixin135 committed Jan 16, 2024
1 parent d254b87 commit 008a898
Show file tree
Hide file tree
Showing 2 changed files with 29 additions and 28 deletions.
7 changes: 5 additions & 2 deletions rm_shooter_controllers/include/rm_shooter_controllers/standard.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,8 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
void reconfigCB(rm_shooter_controllers::ShooterConfig& config, uint32_t /*level*/);

hardware_interface::EffortJointInterface* effort_joint_interface_{};
std::vector<effort_controllers::JointVelocityController*> ctrl_friction_l_, ctrl_friction_r_;
XmlRpc::XmlRpcValue friction_left_, friction_right_;
std::vector<effort_controllers::JointVelocityController*> ctrls_friction_l_, ctrls_friction_r_;
effort_controllers::JointPositionController ctrl_trigger_;
int push_per_rotation_{};
double push_wheel_speed_threshold_{};
Expand All @@ -92,6 +93,8 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
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
Expand All @@ -111,4 +114,4 @@ class Controller : public controller_interface::MultiInterfaceController<hardwar
dynamic_reconfigure::Server<rm_shooter_controllers::ShooterConfig>* d_srv_{};
};

} // namespace rm_shooter_controllers
} // namespace rm_shooter_controllers
50 changes: 24 additions & 26 deletions rm_shooter_controllers/src/standard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<hardware_interface::EffortJointInterface>();
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;
}
Expand Down Expand Up @@ -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);
}
Expand All @@ -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());
}
Expand All @@ -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;
}
Expand Down Expand Up @@ -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_);
}
}

Expand Down Expand Up @@ -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)

0 comments on commit 008a898

Please sign in to comment.