void do_pid(const ros::TimerEvent&)
{
geometry_msgs::Twist output;
geometry_msgs::Point pt;
double x, y, z;
//Make sure we keep the rest of the output the same.
output = setpt_;
// Correct for the sign of the output
output.angular.z = -commandZ_;
if(output.angular.z > 0)
output.angular.z = output.angular.z + motor_zero_;
else
output.angular.z = output.angular.z - motor_zero_;
Pid_.getCurrentPIDErrors(&x, &y, &z);
pt.x = x;
pt.y = y;
pt.z = z;
debug_pub_.publish(pt);
if(!manual_override_ && !rc_override_ &&
(lastCommand_ + timeout_) > ros::Time::now()){
motor_pub_.publish(output);
} else {
Pid_.reset();
}
}
void FreeFloatingPids::InitPID(control_toolbox::Pid &_pid, const ros::NodeHandle&_node, const bool &_use_dynamic_reconfig)
{
if(_use_dynamic_reconfig)
{
// classical PID init
_pid.init(_node);
}
else
{
control_toolbox::Pid::Gains gains;
// Load PID gains from parameter server
if (!_node.getParam("p", gains.p_gain_))
{
ROS_ERROR("No p gain specified for pid. Namespace: %s", _node.getNamespace().c_str());
return;
}
// Only the P gain is required, the I and D gains are optional and default to 0:
_node.param("i", gains.i_gain_, 0.0);
_node.param("d", gains.d_gain_, 0.0);
// Load integral clamp from param server or default to 0
double i_clamp;
_node.param("i_clamp", i_clamp, 0.0);
gains.i_max_ = std::abs(i_clamp);
gains.i_min_ = -std::abs(i_clamp);
_pid.setGains(gains);
}
}
AngularRateControl() : nh_("~"),lastIMU_(0.0),
lastCommand_(0.0), timeout_(1.5),
joy_control_("/teleop/twistCommand"), auto_control_("/mux/safeCommand"),
manual_override_(false), rc_override_(true)
{
nh_.param("kp",kp_,1.0);
nh_.param("ki",ki_,0.0);
nh_.param("kd",kd_,0.0);
nh_.param("imax",imax_,1E9);
nh_.param("period",period_,0.05);
nh_.param("motor_zero",motor_zero_,0.0);
nh_.param("input_scale",input_scale_,1.0);
double dTimeout;
nh_.param("timeout",dTimeout,1.5);
timeout_ = ros::Duration(dTimeout);
Pid_.initPid(kp_,ki_,kd_,imax_,-imax_);
// Make sure TF is ready
ros::Duration(0.5).sleep();
// Subscribe to the velocity command and setup the motor publisher
imu_sub_ = nh_.subscribe("imu",1,&AngularRateControl::imu_callback,this);
cmd_sub_ = nh_.subscribe("command",1,&AngularRateControl::cmd_callback,this);
mux_sub_ = nh_.subscribe("/mux/selected",1,&AngularRateControl::mux_callback,this);
rc_sub_ = nh_.subscribe("/sense",1,&AngularRateControl::rc_callback,this);
motor_pub_ = nh_.advertise<geometry_msgs::Twist>("motors",1);
debug_pub_ = nh_.advertise<geometry_msgs::Point>("debug",1);
//Create a callback for the PID loop
pid_loop_ = nh_.createTimer(ros::Duration(period_), &AngularRateControl::do_pid, this);
f_ = boost::bind(&AngularRateControl::config_callback, this, _1, _2);
server_.setCallback(f_);
}
void imu_callback(const sensor_msgs::Imu& msg) {
if(lastIMU_ == ros::Time(0.0))
lastIMU_ = ros::Time::now() - ros::Duration(0.01);
// setpt_ and msg have opposite senses of Z, so we add
double error = setpt_.angular.z + msg.angular_velocity.z;
ros::Duration deltaT = msg.header.stamp - lastIMU_;
lastIMU_ = msg.header.stamp;
commandZ_ = Pid_.updatePid(error, deltaT);
}
void PositionCommand::pidGainLoad (control_toolbox::Pid &pid , control_toolbox::Pid::Gains & G, const ros::NodeHandle &n )
{
double Kp,Ki,Kd,IMax,IMin;
n.param<double>("Kp" ,G.p_gain_ , 0.0);
n.param<double>("Ki" ,G.i_gain_ , 0.0);
n.param<double>("Kd" ,G.d_gain_ , 0.0);
n.param<double>("IMax" ,G.i_max_ , 0.0);
n.param<double>("IMin" ,G.i_min_ , 0.0);
std::cout << "Kp = " << G.p_gain_
<< " Ki = " << G.i_gain_
<< " Kd = " << G.d_gain_
<< " IMax = " << G.i_max_
<< " IMin = " << G.i_min_
<< std::endl;
pid.setGains(Kp,Ki,Kd,IMax,IMin);
}
void config_callback(rate_control::RateControlConfig &config, uint32_t level)
{
Pid_.setGains(config.kp, config.ki, config.kd, config.imax, -config.imax);
motor_zero_ = config.motor_zero;
input_scale_ = config.input_scale;
}