Merge pull request #46 from clearpathrobotics/further-reuse

Remove code repetition

Author: Adolfo Rodriguez Tsouroukdissian

include/control_toolbox/pid.h

@@ -117,10 +117,10 @@ class Pid
   /*!
    * \brief Store gains in a struct to allow easier realtime buffer usage
    */
-  struct Gains 
+  struct Gains
   {
     // Optional constructor for passing in values
-    Gains(double p, double i, double d, double i_max, double i_min) 
+    Gains(double p, double i, double d, double i_max, double i_min)
       : p_gain_(p),
         i_gain_(i),
         d_gain_(d),
@@ -338,7 +338,7 @@ class Pid
    */
   void getCurrentPIDErrors(double *pe, double *ie, double *de);
 
-  
+
   /*!
    * \brief Print to console the current parameters
    */
@@ -355,7 +355,7 @@ class Pid
 
     // Copy the realtime buffer to then new PID class
     gains_buffer_ = source.gains_buffer_;
-    
+
     // Reset the state of this PID controller
     reset();
 
@@ -366,7 +366,7 @@ class Pid
 
   // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
   // blocking the realtime update loop
-  realtime_tools::RealtimeBuffer<Gains> gains_buffer_; 
+  realtime_tools::RealtimeBuffer<Gains> gains_buffer_;
 
   double p_error_last_; /**< _Save position state for derivative state calculation. */
   double p_error_; /**< Position error. */
@@ -376,7 +376,7 @@ class Pid
 
   // Dynamics reconfigure
   bool dynamic_reconfig_initialized_;
-  typedef dynamic_reconfigure::Server<control_toolbox::ParametersConfig> DynamicReconfigServer;                             
+  typedef dynamic_reconfigure::Server<control_toolbox::ParametersConfig> DynamicReconfigServer;
   boost::shared_ptr<DynamicReconfigServer> param_reconfig_server_;
   DynamicReconfigServer::CallbackType param_reconfig_callback_;
 

src/pid.cpp

@@ -67,7 +67,7 @@ Pid::~Pid()
 {
 }
 
-void Pid::initPid(double p, double i, double d, double i_max, double i_min, 
+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);
@@ -97,7 +97,7 @@ bool Pid::init(const ros::NodeHandle &node, const bool quiet)
   Gains gains;
 
   // Load PID gains from parameter server
-  if (!nh.getParam("p", gains.p_gain_)) 
+  if (!nh.getParam("p", gains.p_gain_))
   {
     if (!quiet) {
       ROS_ERROR("No p gain specified for pid.  Namespace: %s", nh.getNamespace().c_str());
@@ -123,7 +123,7 @@ bool Pid::init(const ros::NodeHandle &node, const bool quiet)
     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();
@@ -140,7 +140,7 @@ bool Pid::initXml(TiXmlElement *config)
   double i_clamp;
   i_clamp = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
 
-  setGains( 
+  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,
@@ -156,19 +156,19 @@ bool Pid::initXml(TiXmlElement *config)
 
 void Pid::initDynamicReconfig(ros::NodeHandle &node)
 {
-  ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace " 
+  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_); 
+  param_reconfig_server_->setCallback(param_reconfig_callback_);
 }
 
 void Pid::reset()
@@ -191,15 +191,15 @@ void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min
   i_min = gains.i_min_;
 }
 
-Pid::Gains Pid::getGains() 
+Pid::Gains Pid::getGains()
 {
   return *gains_buffer_.readFromRT();
 }
 
 void Pid::setGains(double p, double i, double d, double i_max, double i_min)
 {
   Gains gains(p,i,d,i_max,i_min);
-  
+
   setGains(gains);
 }
 
@@ -217,10 +217,10 @@ void Pid::updateDynamicReconfig()
   if(!dynamic_reconfig_initialized_)
     return;
 
-  // Get starting values 
+  // Get starting values
   control_toolbox::ParametersConfig config;
 
-  // Get starting values   
+  // Get starting values
   getGains(config.p, config.i, config.d, config.i_clamp_max, config.i_clamp_min);
 
   updateDynamicReconfig(config);
@@ -251,9 +251,9 @@ void Pid::updateDynamicReconfig(control_toolbox::ParametersConfig config)
     return;
 
   // Set starting values, using a shared mutex with dynamic reconfig
-  param_reconfig_mutex_.lock(); 
+  param_reconfig_mutex_.lock();
   param_reconfig_server_->updateConfig(config);
-  param_reconfig_mutex_.unlock(); 
+  param_reconfig_mutex_.unlock();
 }
 
 void Pid::dynamicReconfigCallback(control_toolbox::ParametersConfig &config, uint32_t level)
@@ -284,19 +284,7 @@ double Pid::computeCommand(double error, ros::Duration dt)
 
 double Pid::updatePid(double error, ros::Duration dt)
 {
-  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error))
-    return 0.0;
-
-  double error_dot = d_error_;
-
-  // Calculate the derivative error
-  if (dt.toSec() > 0.0)
-  {
-    error_dot = (error - p_error_last_) / dt.toSec();
-    p_error_last_ = error;
-  }
-
-  return updatePid(error, error_dot, dt);
+  return -computeCommand(error, dt);
 }
 
 double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
@@ -317,7 +305,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
 
   // Calculate the integral of the position error
   i_error_ += dt.toSec() * p_error_;
-  
+
   // Calculate integral contribution to command
   i_term = gains.i_gain_ * i_error_;
 
@@ -335,36 +323,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt)
 
 double Pid::updatePid(double error, double error_dot, ros::Duration dt)
 {
-  // Get the gain parameters from the realtime buffer
-  Gains gains = *gains_buffer_.readFromRT();
-
-  double p_term, d_term, i_term;
-  p_error_ = error; //this is pError = pState-pTarget
-  d_error_ = error_dot;
-
-  if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
-    return 0.0;
-
-
-  // Calculate proportional contribution to command
-  p_term = gains.p_gain_ * p_error_;
-
-  // Calculate the integral of the position error
-  i_error_ += dt.toSec() * p_error_;
-  
-  // Calculate integral contribution to command
-  i_term = gains.i_gain_ * i_error_;
-
-  // Limit i_term so that the limit is meaningful in the output
-  i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) );
-
-  // Calculate derivative contribution to command
-  d_term = gains.d_gain_ * d_error_;
-
-  // Compute the command
-  cmd_ = - p_term - i_term - d_term;
-
-  return cmd_;
+  return -computeCommand(error, error_dot, dt);
 }
 
 void Pid::setCurrentCmd(double cmd)