bool Pid::init(const ros::NodeHandle &node)
{
ros::NodeHandle nh(node);
Gains gains;
// Load PID gains from parameter server
if (!nh.getParam("p", gains.p_gain_))
{
ROS_ERROR("No p gain specified for pid. Namespace: %s", nh.getNamespace().c_str());
return false;
}
// Only the P gain is required, the I and D gains are optional and default to 0:
nh.param("i", gains.i_gain_, 0.0);
nh.param("d", gains.d_gain_, 0.0);
// Load integral clamp from param server or default to 0
double i_clamp;
nh.param("i_clamp", i_clamp, 0.0);
gains.i_max_ = std::abs(i_clamp);
gains.i_min_ = -std::abs(i_clamp);
setGains(gains);
reset();
initDynamicReconfig(nh);
return true;
}
void Pid::initPid(double p, double i, double d, double i_max, double i_min,
const ros::NodeHandle &node)
{
initPid(p, i, d, i_max, i_min);
// Create node handle for dynamic reconfigure
ros::NodeHandle nh(DEFAULT_NAMESPACE);
initDynamicReconfig(nh);
}
bool Pid::init(const ros::NodeHandle &node, const bool quiet)
{
ros::NodeHandle nh(node);
Gains gains;
// Load PID gains from parameter server
if (!nh.getParam("p", gains.p_gain_))
{
if (!quiet) {
ROS_ERROR("No p gain specified for pid. Namespace: %s", nh.getNamespace().c_str());
}
return false;
}
// Only the P gain is required, the I and D gains are optional and default to 0:
nh.param("i", gains.i_gain_, 0.0);
nh.param("d", gains.d_gain_, 0.0);
// Load integral clamp from param server or default to 0
double i_clamp;
nh.param("i_clamp", i_clamp, 0.0);
gains.i_max_ = std::abs(i_clamp);
gains.i_min_ = -std::abs(i_clamp);
if(nh.hasParam("i_clamp_min"))
{
nh.param("i_clamp_min", gains.i_min_, gains.i_min_); // use i_clamp_min parameter, otherwise keep -i_clamp
gains.i_min_ = -std::abs(gains.i_min_); // make sure the value is <= 0
}
if(nh.hasParam("i_clamp_max"))
{
nh.param("i_clamp_max", gains.i_max_, gains.i_max_); // use i_clamp_max parameter, otherwise keep i_clamp
gains.i_max_ = std::abs(gains.i_max_); // make sure the value is >= 0
}
setGains(gains);
reset();
initDynamicReconfig(nh);
return true;
}
bool Pid::initXml(TiXmlElement *config)
{
// Create node handle for dynamic reconfigure
ros::NodeHandle nh(DEFAULT_NAMESPACE);
double i_clamp;
i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
setGains(
config->Attribute("p") ? atof(config->Attribute("p")) : 0.0,
config->Attribute("i") ? atof(config->Attribute("i")) : 0.0,
config->Attribute("d") ? atof(config->Attribute("d")) : 0.0,
std::abs(i_clamp),
-std::abs(i_clamp)
);
reset();
initDynamicReconfig(nh);
return true;
}
