Added feedback

Signed-off-by: ahcorde <[email protected]>

include/control_toolbox/pid.hpp

@@ -249,6 +249,11 @@ class Pid
    */
   double getCurrentCmd();
 
+  /*!
+   * \brief Return derivative error
+   */
+  double getDerivativeError();
+
   /*!
    * \brief Return PID error terms for the controller.
    * \param pe  The proportional error.
@@ -286,6 +291,7 @@ class Pid
   double i_error_;      /**< Integral of position error. */
   double d_error_;      /**< Derivative of position error. */
   double cmd_;          /**< Command to send. */
+  double error_dot_;    /**< Derivative error */
 };
 
 }  // namespace control_toolbox

include/control_toolbox/pidROS.hpp

@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2020, Open Source Robotics Foundation, Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     copyright notice, this list of conditions and the following
  *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
- *   * Neither the name of the Willow Garage nor the names of its
+ *   * Neither the name of the Open Source Robotics Foundation nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -69,7 +69,7 @@ class PidROS
   /*!
    * \brief Destructor of PidROS class.
    */
-  ~PidROS();
+  ~PidROS() = default;
 
   /*!
    * \brief Initialize the PID controller and set the paramaters
@@ -78,6 +78,7 @@ class PidROS
    * \param d The derivative gain.
    * \param i_max The max integral windup.
    * \param i_min The min integral windup.
+   * \param antiwindup antiwindup.
    */
   void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup);
 
@@ -117,6 +118,7 @@ class PidROS
    * \param d The derivative gain.
    * \param i_max The max integral windup.
    * \param i_min The min integral windup.
+   * \param antiwindup antiwindup.
    */
   void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
 
@@ -128,11 +130,13 @@ class PidROS
 
   /*!
    * \brief Set current command for this PID controller
+   * \param cmd command to set
    */
   void setCurrentCmd(double cmd);
 
   /*!
    * \brief Return current command for this PID controller
+   * \return current cmd
    */
   double getCurrentCmd();
 
@@ -154,7 +158,7 @@ class PidROS
 
   void publishPIDState(double cmd, double error, rclcpp::Duration dt);
 
-  void declare_param(const std::string & param_name, rclcpp::ParameterValue param_value);
+  void declareParam(const std::string & param_name, rclcpp::ParameterValue param_value);
 
   bool getDoubleParam(const std::string & param_name, double & value);
 

src/pid.cpp

@@ -137,15 +137,15 @@ double Pid::computeCommand(double error, double dt)
     return 0.0;
   }
 
-  double error_dot = d_error_;
+  error_dot_ = d_error_;
 
   // Calculate the derivative error
   if (dt > 0.0) {
-    error_dot = (error - p_error_last_) / dt;
+    error_dot_ = (error - p_error_last_) / dt;
     p_error_last_ = error;
   }
 
-  return computeCommand(error, error_dot, dt);
+  return computeCommand(error, error_dot_, dt);
 }
 
 double Pid::computeCommand(double error, double error_dot, double dt)
@@ -194,9 +194,20 @@ double Pid::computeCommand(double error, double error_dot, double dt)
   return cmd_;
 }
 
-void Pid::setCurrentCmd(double cmd) {cmd_ = cmd;}
+void Pid::setCurrentCmd(double cmd)
+{
+  cmd_ = cmd;
+}
+
+double Pid::getDerivativeError()
+{
+  return error_dot_;
+}
 
-double Pid::getCurrentCmd() {return cmd_;}
+double Pid::getCurrentCmd()
+{
+  return cmd_;
+}
 
 void Pid::getCurrentPIDErrors(double & pe, double & ie, double & de)
 {

src/pidROS.cpp

@@ -1,7 +1,7 @@
 /*********************************************************************
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2020, Open Source Robotics Foundation, Inc.
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -14,7 +14,7 @@
  *     copyright notice, this list of conditions and the following
  *     disclaimer in the documentation and/or other materials provided
  *     with the distribution.
- *   * Neither the name of the Willow Garage nor the names of its
+ *   * Neither the name of the Open Source Robotics Foundation nor the names of its
  *     contributors may be used to endorse or promote products derived
  *     from this software without specific prior written permission.
  *
@@ -32,12 +32,6 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/*
-  Author: Melonee Wise
-  Contributors: Dave Coleman, Jonathan Bohren, Bob Holmberg, Wim Meeussen
-  Desc: Implements a standard proportional-integral-derivative controller
-*/
-
 #include <algorithm>
 #include <cmath>
 #include <string>
@@ -64,10 +58,6 @@ PidROS::PidROS(rclcpp::Node::SharedPtr & node, std::string topic_prefix)
     new realtime_tools::RealtimePublisher<control_msgs::msg::PidState>(state_pub_));
 }
 
-PidROS::~PidROS()
-{
-}
-
 bool PidROS::getBooleanParam(const std::string & param_name, bool & value)
 {
   rclcpp::Parameter param;
@@ -102,25 +92,25 @@ bool PidROS::initPid()
 {
   double p, i, d, i_min, i_max;
   bool antiwindup = false;
-  bool all_param_available = true;
-  all_param_available &= getDoubleParam(topic_prefix_ + "p", p);
-  all_param_available &= getDoubleParam(topic_prefix_ + "i", i);
-  all_param_available &= getDoubleParam(topic_prefix_ + "d", d);
-  all_param_available &= getDoubleParam(topic_prefix_ + "i_clamp_max", i_max);
-  all_param_available &= getDoubleParam(topic_prefix_ + "i_clamp_min", i_min);
+  bool all_params_available = true;
+  all_params_available &= getDoubleParam(topic_prefix_ + "p", p);
+  all_params_available &= getDoubleParam(topic_prefix_ + "i", i);
+  all_params_available &= getDoubleParam(topic_prefix_ + "d", d);
+  all_params_available &= getDoubleParam(topic_prefix_ + "i_clamp_max", i_max);
+  all_params_available &= getDoubleParam(topic_prefix_ + "i_clamp_min", i_min);
 
   getBooleanParam(topic_prefix_ + "antiwindup", antiwindup);
 
-  if (all_param_available) {
+  if (all_params_available) {
     setParameterEventCallback();
   }
 
   pid_.initPid(p, i, d, i_max, i_min, antiwindup);
 
-  return all_param_available;
+  return all_params_available;
 }
 
-void PidROS::declare_param(const std::string & param_name, rclcpp::ParameterValue param_value)
+void PidROS::declareParam(const std::string & param_name, rclcpp::ParameterValue param_value)
 {
   if (!node_->has_parameter(param_name)) {
     node_->declare_parameter(param_name, param_value);
@@ -131,12 +121,12 @@ void PidROS::initPid(double p, double i, double d, double i_max, double i_min, b
 {
   pid_.initPid(p, i, d, i_max, i_min, antiwindup);
 
-  declare_param("p", rclcpp::ParameterValue(p));
-  declare_param("i", rclcpp::ParameterValue(i));
-  declare_param("d", rclcpp::ParameterValue(d));
-  declare_param("i_clamp_max", rclcpp::ParameterValue(i_max));
-  declare_param("i_clamp_min", rclcpp::ParameterValue(i_min));
-  declare_param("antiwindup", rclcpp::ParameterValue(antiwindup));
+  declareParam("p", rclcpp::ParameterValue(p));
+  declareParam("i", rclcpp::ParameterValue(i));
+  declareParam("d", rclcpp::ParameterValue(d));
+  declareParam("i_clamp_max", rclcpp::ParameterValue(i_max));
+  declareParam("i_clamp_min", rclcpp::ParameterValue(i_min));
+  declareParam("antiwindup", rclcpp::ParameterValue(antiwindup));
 
   setParameterEventCallback();
 }
@@ -188,7 +178,7 @@ void PidROS::publishPIDState(double cmd, double error, rclcpp::Duration dt)
       rt_state_pub_->msg_.header.stamp = rclcpp::Clock().now();
       rt_state_pub_->msg_.timestep = dt;
       rt_state_pub_->msg_.error = error;
-      // rt_state_pub_->msg_.error_dot = error_dot;
+      rt_state_pub_->msg_.error_dot = pid_.getDerivativeError();
       rt_state_pub_->msg_.p_error = p_error_;
       rt_state_pub_->msg_.i_error = i_error_;
       rt_state_pub_->msg_.d_error = d_error_;