void Pid::setGains(const Gains &gains)
{
gains_buffer_.writeFromNonRT(gains);
// Update dynamic reconfigure with the new gains
updateDynamicReconfig(gains);
}
void Pid::updateDynamicReconfig()
{
// Make sure dynamic reconfigure is initialized
if(!dynamic_reconfig_initialized_)
return;
// Get starting values
control_toolbox::ParametersConfig config;
// Get starting values
getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);
updateDynamicReconfig(config);
}
void Pid::initDynamicReconfig(ros::NodeHandle &node)
{
ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace "
<< node.getNamespace());
// Start dynamic reconfigure server
param_reconfig_server_.reset(new DynamicReconfigServer(param_reconfig_mutex_, node));
dynamic_reconfig_initialized_ = true;
// Set Dynamic Reconfigure's gains to Pid's values
updateDynamicReconfig();
// Set callback
param_reconfig_callback_ = boost::bind(&Pid::dynamicReconfigCallback, this, _1, _2);
param_reconfig_server_->setCallback(param_reconfig_callback_);
}
void Pid::updateDynamicReconfig(Gains gains_config)
{
// Make sure dynamic reconfigure is initialized
if(!dynamic_reconfig_initialized_)
return;
control_toolbox::ParametersConfig config;
// Convert to dynamic reconfigure format
config.p_gain = gains_config.p_gain_;
config.i_gain = gains_config.i_gain_;
config.d_gain = gains_config.d_gain_;
config.i_clamp_max = gains_config.i_max_;
config.i_clamp_min = gains_config.i_min_;
updateDynamicReconfig(config);
}
/**
* @brief AdmittanceParamManager::setAdmittanceParams
* Sends changes of the admittance parameters to the controller.
* @param active
* @param inertia
* @param damping
* @param stiffness
* @param dead_zone
*/
void AdmittanceParamManager::setParams(compliant_ros_controller::AdmittanceParamsConfig &config) {
controller_->activate(config.active);
controller_->setInertia(config.inertia);
controller_->setDamping(config.damping);
controller_->setStiffness(config.stiffness);
controller_->setTransDeadZone(config.dead_zone_trans);
controller_->setRotDeadZone(config.dead_zone_rot);
controller_->setMode(config.mode);
controller_->setTransSpeedLimit(config.speed_limit_trans);
controller_->setRotSpeedLimit(config.speed_limit_rot);
if (config.active) {
ROS_INFO_STREAM("Admittance params changed to: Active with "
<< config.inertia << ", " << config.damping << ", " << config.stiffness
<< ". Dead Zone: " << config.dead_zone_trans << " | " << config.dead_zone_rot
<< ", Speed Limit: " << config.speed_limit_trans << " | " << config.speed_limit_rot
<< " Mode: " << config.mode << ".");
} else {
ROS_INFO_STREAM("Admittance params changed to: Not active");
}
updateDynamicReconfig(config);
}
