Use RealtimeThreadSafeBox for PID class (backport #387) (#413)

Co-authored-by: Sai Kishor Kothakota <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>

Author: 3 people

control_toolbox/include/control_toolbox/pid.hpp

@@ -41,7 +41,7 @@
 
 #include "fmt/format.h"
 #include "rclcpp/duration.hpp"
-#include "realtime_tools/realtime_buffer.hpp"
+#include "realtime_tools/realtime_thread_safe_box.hpp"
 
 namespace control_toolbox
 {
@@ -273,7 +273,7 @@ class Pid
 {
 public:
   /*!
-   * \brief Store gains in a struct to allow easier realtime buffer usage
+   * \brief Store gains in a struct to allow easier realtime box usage
    */
   struct Gains
   {
@@ -560,6 +560,8 @@ class Pid
    * \param d The derivative gain.
    * \param i_max Upper integral clamp.
    * \param i_min Lower integral clamp.
+   *
+   * \note This method is not RT safe
    */
   void get_gains(double & p, double & i, double & d, double & i_max, double & i_min);
 
@@ -585,6 +587,8 @@ class Pid
         the integral error to prevent windup; otherwise, constrains the
         integral contribution to the control output. i_max and
         i_min are applied in both scenarios.
+   *
+   * \note This method is not RT safe
    */
   [[deprecated("Use get_gains overload with AntiWindupStrategy argument.")]]
   void get_gains(
@@ -615,6 +619,8 @@ class Pid
    * \param antiwindup_strat Specifies the anti-windup strategy. Options: 'back_calculation',
         'conditional_integration', or 'none'. Note that the 'back_calculation' strategy use the
         tracking_time_constant parameter to tune the anti-windup behavior.
+   *
+   * \note This method is not RT safe
    */
   void get_gains(
     double & p, double & i, double & d, double & u_max, double & u_min,
@@ -623,15 +629,27 @@ class Pid
   /*!
    * \brief Get PID gains for the controller.
    * \return gains A struct of the PID gain values
+   *
+   * \note This method is not RT safe
    */
   Gains get_gains();
 
   /*!
    * \brief Get PID gains for the controller.
    * \return gains A struct of the PID gain values
+   *
+   * \note This method is not RT safe
    */
   [[deprecated("Use get_gains() instead")]] Gains getGains();
 
+  /*!
+   * \brief Get PID gains for the controller.
+   * \return gains A struct of the PID gain values
+   *
+   * \note This method can be called from the RT loop
+   */
+  Gains get_gains_rt() { return gains_; }
+
   /*!
    * \brief Set PID gains for the controller.
    * \param p The proportional gain.
@@ -646,6 +664,7 @@ class Pid
    * \return True if all parameters are successfully set, False otherwise.
    *
    * \note New gains are not applied if i_min > i_max
+   * \note This method is not RT safe
    */
   [[deprecated("Use set_gains with AntiWindupStrategy instead.")]]
   bool set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
@@ -681,6 +700,7 @@ class Pid
    * \return True if all parameters are successfully set, False otherwise.
    *
    * \note New gains are not applied if i_min_ > i_max_ or u_min > u_max
+   * \note This method is not RT safe
    */
   bool set_gains(
     double p, double i, double d, double u_max, double u_min,
@@ -692,6 +712,7 @@ class Pid
    * \return True if all parameters are successfully set, False otherwise.
    *
    * \note New gains are not applied if gains.i_min_ > gains.i_max_
+   * \note This method is not RT safe
    */
   bool set_gains(const Gains & gains);
 
@@ -885,8 +906,8 @@ class Pid
       return *this;
     }
 
-    // Copy the realtime buffer to then new PID class
-    gains_buffer_ = source.gains_buffer_;
+    // Copy the realtime box to then new PID class
+    gains_box_ = source.gains_box_;
 
     // Reset the state of this PID controller
     reset();
@@ -895,9 +916,17 @@ class Pid
   }
 
 protected:
-  // Store the PID gains in a realtime buffer to allow dynamic reconfigure to update it without
+  // local copy of the gains for the RT loop
+  Gains gains_{
+    0.0,
+    0.0,
+    0.0,
+    std::numeric_limits<double>::infinity(),
+    -std::numeric_limits<double>::infinity(),
+    AntiWindupStrategy()};
+  // Store the PID gains in a realtime box to allow dynamic reconfigure to update it without
   // blocking the realtime update loop
-  realtime_tools::RealtimeBuffer<Gains> gains_buffer_;
+  realtime_tools::RealtimeThreadSafeBox<Gains> gains_box_{gains_};
 
   double p_error_last_ = 0; /** Save state for derivative state calculation. */
   double p_error_ = 0;      /** Error. */

control_toolbox/include/control_toolbox/pid_ros.hpp

@@ -257,6 +257,8 @@ class PidROS
   /*!
    * \brief Get PID gains for the controller.
    * \return gains A struct of the PID gain values
+   *
+   * \note This method is not RT safe
    */
   Pid::Gains get_gains();
 
@@ -280,6 +282,7 @@ class PidROS
    * \return True if all parameters are successfully set, False otherwise.
    *
    * \note New gains are not applied if i_min > i_max
+   * \note This method is not RT safe
    */
   [[deprecated("Use set_gains with AntiWindupStrategy instead.")]]
   bool set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
@@ -298,6 +301,7 @@ class PidROS
    * \return True if all parameters are successfully set, False otherwise.
    *
    * \note New gains are not applied if u_min_ > u_max_.
+   * \note This method is not RT safe
    */
   bool set_gains(
     double p, double i, double d, double u_max, double u_min,
@@ -326,6 +330,7 @@ class PidROS
    * \return True if all parameters are successfully set, False otherwise.
    *
    * \note New gains are not applied if gains.i_min_ > gains.i_max_
+   * \note This method is not RT safe
    */
   bool set_gains(const Pid::Gains & gains);
 

control_toolbox/src/pid.cpp

@@ -54,7 +54,6 @@ constexpr double UMAX_INFINITY = std::numeric_limits<double>::infinity();
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
 Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwindup)
-: gains_buffer_()
 {
   if (i_min > i_max)
   {
@@ -77,7 +76,6 @@ Pid::Pid(double p, double i, double d, double i_max, double i_min, bool antiwind
 Pid::Pid(
   double p, double i, double d, double u_max, double u_min,
   const AntiWindupStrategy & antiwindup_strat)
-: gains_buffer_()
 {
   if (u_min > u_max)
   {
@@ -93,8 +91,8 @@ Pid::Pid(
 
 Pid::Pid(const Pid & source)
 {
-  // Copy the realtime buffer to the new PID class
-  gains_buffer_ = source.gains_buffer_;
+  // Copy the realtime box to the new PID class
+  gains_box_ = source.gains_box_;
 
   // Initialize saved i-term values
   clear_saved_iterm();
@@ -147,6 +145,9 @@ void Pid::reset(bool save_i_term)
   {
     clear_saved_iterm();
   }
+
+  // blocking, as reset() is not called in the RT thread
+  gains_ = gains_box_.get();
 }
 
 void Pid::clear_saved_iterm() { i_term_ = 0.0; }
@@ -177,8 +178,7 @@ void Pid::get_gains(
   double & p, double & i, double & d, double & u_max, double & u_min,
   AntiWindupStrategy & antiwindup_strat)
 {
-  Gains gains = *gains_buffer_.readFromRT();
-
+  Gains gains = get_gains();
   p = gains.p_gain_;
   i = gains.i_gain_;
   d = gains.d_gain_;
@@ -187,7 +187,11 @@ void Pid::get_gains(
   antiwindup_strat = gains.antiwindup_strat_;
 }
 
-Pid::Gains Pid::get_gains() { return *gains_buffer_.readFromRT(); }
+Pid::Gains Pid::get_gains()
+{
+  // blocking, as get_gains() is called from non-RT thread
+  return gains_box_.get();
+}
 
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
@@ -253,7 +257,8 @@ bool Pid::set_gains(const Gains & gains_in)
         gains.antiwindup_strat_.tracking_time_constant = gains.p_gain_ / gains.i_gain_;
       }
     }
-    gains_buffer_.writeFromNonRT(gains);
+    // blocking, as set_gains() is called from non-RT thread
+    gains_box_.set(gains);
     return true;
   }
   return false;
@@ -326,8 +331,12 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
   {
     throw std::invalid_argument("Pid is called with negative dt");
   }
-  // Get the gain parameters from the realtime buffer
-  Gains gains = *gains_buffer_.readFromRT();
+  // Get the gain parameters from the realtime box
+  auto gains_opt = gains_box_.try_get();
+  if (gains_opt.has_value())
+  {
+    gains_ = gains_opt.value();
+  }
 
   double p_term, d_term;
   p_error_ = error;      // This is error = target - state
@@ -341,57 +350,58 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
   }
 
   // Calculate proportional contribution to command
-  p_term = gains.p_gain_ * p_error_;
+  p_term = gains_.p_gain_ * p_error_;
 
   // Calculate derivative contribution to command
-  d_term = gains.d_gain_ * d_error_;
+  d_term = gains_.d_gain_ * d_error_;
 
-  if (gains.antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED)
+  if (gains_.antiwindup_strat_.type == AntiWindupStrategy::UNDEFINED)
   {
     throw std::runtime_error(
       "PID: Antiwindup strategy cannot be UNDEFINED. Please set a valid antiwindup strategy.");
   }
 
   // Calculate integral contribution to command
   const bool is_error_in_deadband_zone =
-    control_toolbox::is_zero(error, gains.antiwindup_strat_.error_deadband);
-  if (!is_error_in_deadband_zone && gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY)
+    control_toolbox::is_zero(error, gains_.antiwindup_strat_.error_deadband);
+  if (!is_error_in_deadband_zone && gains_.antiwindup_strat_.type == AntiWindupStrategy::LEGACY)
   {
-    if (gains.antiwindup_strat_.legacy_antiwindup)
+    if (gains_.antiwindup_strat_.legacy_antiwindup)
     {
       // Prevent i_term_ from climbing higher than permitted by i_max_/i_min_
-      i_term_ = std::clamp(i_term_ + gains.i_gain_ * dt_s * p_error_, gains.i_min_, gains.i_max_);
+      i_term_ =
+        std::clamp(i_term_ + gains_.i_gain_ * dt_s * p_error_, gains_.i_min_, gains_.i_max_);
     }
     else
     {
-      i_term_ += gains.i_gain_ * dt_s * p_error_;
+      i_term_ += gains_.i_gain_ * dt_s * p_error_;
     }
   }
 
   // Compute the command
   if (
-    !gains.antiwindup_strat_.legacy_antiwindup &&
-    gains.antiwindup_strat_.type == AntiWindupStrategy::LEGACY)
+    !gains_.antiwindup_strat_.legacy_antiwindup &&
+    gains_.antiwindup_strat_.type == AntiWindupStrategy::LEGACY)
   {
     // Limit i_term so that the limit is meaningful in the output
-    cmd_unsat_ = p_term + std::clamp(i_term_, gains.i_min_, gains.i_max_) + d_term;
+    cmd_unsat_ = p_term + std::clamp(i_term_, gains_.i_min_, gains_.i_max_) + d_term;
   }
   else
   {
     cmd_unsat_ = p_term + i_term_ + d_term;
   }
 
-  if (std::isfinite(gains.u_min_) || std::isfinite(gains.u_max_))
+  if (std::isfinite(gains_.u_min_) || std::isfinite(gains_.u_max_))
   {
-    if (gains.u_min_ > gains.u_max_)
+    if (gains_.u_min_ > gains_.u_max_)
     {
       throw std::runtime_error("Pid: Error while saturating the command : u_min > u_max");
     }
-    if (std::isnan(gains.u_min_) || std::isnan(gains.u_max_))
+    if (std::isnan(gains_.u_min_) || std::isnan(gains_.u_max_))
     {
       throw std::runtime_error("Pid: Error while saturating the command : u_min or u_max is NaN");
     }
-    cmd_ = std::clamp(cmd_unsat_, gains.u_min_, gains.u_max_);
+    cmd_ = std::clamp(cmd_unsat_, gains_.u_min_, gains_.u_max_);
   }
   else
   {
@@ -401,17 +411,17 @@ double Pid::compute_command(double error, double error_dot, const double & dt_s)
   if (!is_error_in_deadband_zone)
   {
     if (
-      gains.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION &&
-      !is_zero(gains.i_gain_))
+      gains_.antiwindup_strat_.type == AntiWindupStrategy::BACK_CALCULATION &&
+      !is_zero(gains_.i_gain_))
     {
-      i_term_ += dt_s * (gains.i_gain_ * error +
-                         1 / gains.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_));
+      i_term_ += dt_s * (gains_.i_gain_ * error +
+                         1 / gains_.antiwindup_strat_.tracking_time_constant * (cmd_ - cmd_unsat_));
     }
-    else if (gains.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION)
+    else if (gains_.antiwindup_strat_.type == AntiWindupStrategy::CONDITIONAL_INTEGRATION)
     {
       if (!(!is_zero(cmd_unsat_ - cmd_) && error * cmd_unsat_ > 0))
       {
-        i_term_ += dt_s * gains.i_gain_ * error;
+        i_term_ += dt_s * gains_.i_gain_ * error;
       }
     }
   }

control_toolbox/src/pid_ros.cpp

@@ -453,7 +453,7 @@ bool PidROS::set_gains(const Pid::Gains & gains)
 
 void PidROS::publish_pid_state(double cmd, double error, rclcpp::Duration dt)
 {
-  Pid::Gains gains = pid_.get_gains();
+  Pid::Gains gains = pid_.get_gains_rt();
 
   double p_error, i_term, d_error;
   get_current_pid_errors(p_error, i_term, d_error);
@@ -525,7 +525,6 @@ void PidROS::set_parameter_event_callback()
     rcl_interfaces::msg::SetParametersResult result;
     result.successful = true;
 
-    /// @note don't use getGains, it's rt
     Pid::Gains gains = pid_.get_gains();
     bool changed = false;
     // The saturation parameter is special, it can change the u_min and u_max parameters