Rework PID class API

include/control_toolbox/pid.hpp

@@ -38,6 +38,7 @@
 #include <stdexcept>
 #include <string>
 
+#include "rclcpp/duration.hpp"
 #include "realtime_tools/realtime_buffer.hpp"
 
 #include "control_toolbox/visibility_control.hpp"
@@ -88,13 +89,13 @@ namespace control_toolbox
 
   \verbatim
   control_toolbox::Pid pid;
-  pid.initPid(6.0, 1.0, 2.0, 0.3, -0.3);
+  pid.initialize(6.0, 1.0, 2.0, 0.3, -0.3);
   double position_desi_ = 0.5;
   ...
   rclcpp::Time last_time = get_clock()->now();
   while (true) {
   rclcpp::Time time = get_clock()->now();
-  double effort = pid.computeCommand(position_desi_ - currentPosition(), (time - last_time).nanoseconds());
+  double effort = pid.computeCommand(position_desi_ - currentPosition(), (time - last_time));
   last_time = time;
   }
   \endverbatim
@@ -196,7 +197,26 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    *
    * \note New gains are not applied if i_min_ > i_max_
    */
-  void initPid(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+  void initialize(
+    double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+
+  /*!
+   * \brief Zeros out Pid values and initialize Pid-gains and integral term limits
+   *        Does not initialize the node's parameter interface for PID gains
+   *
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param i_max The max integral windup.
+   * \param i_min The min integral windup.
+   * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
+   *
+   * \note New gains are not applied if i_min_ > i_max_
+   */
+  [[deprecated("Use initialize() instead")]] void initPid(
+    double p, double i, double d, double i_max, double i_min, bool antiwindup = false) {
+    initialize(p, i, d, i_max, i_min, antiwindup);
+  }
 
   /*!
    * \brief Reset the state of this PID controller
@@ -211,7 +231,21 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    * \param i_max The max integral windup.
    * \param i_min The min integral windup.
    */
-  void getGains(double & p, double & i, double & d, double & i_max, double & i_min);
+  void get_gains(double & p, double & i, double & d, double & i_max, double & i_min);
+
+  /*!
+   * \brief Get PID gains for the controller.
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param i_max The max integral windup.
+   * \param i_min The min integral windup.
+   */
+  [[deprecated("Use get_gains() instead")]] void getGains(
+    double & p, double & i, double & d, double & i_max, double & i_min) {
+    get_gains(p, i, d, i_max, i_min);
+  }
+
   /*!
    * \brief Get PID gains for the controller.
    * \param p The proportional gain.
@@ -221,14 +255,36 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    * \param i_min The min integral windup.
    * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
    */
-  void getGains(
+  void get_gains(
     double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup);
 
+  /*!
+   * \brief Get PID gains for the controller.
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param i_max The max integral windup.
+   * \param i_min The min integral windup.
+   * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
+   */
+  [[deprecated("Use get_gains() instead")]] void getGains(
+    double & p, double & i, double & d, double & i_max, double & i_min, bool & antiwindup) {
+    get_gains(p, i, d, i_max, i_min, antiwindup);
+    }
+
+  /*!
+   * \brief Get PID gains for the controller.
+   * \return gains A struct of the PID gain values
+   */
+  Gains get_gains();
+
   /*!
    * \brief Get PID gains for the controller.
    * \return gains A struct of the PID gain values
    */
-  Gains getGains();
+  [[deprecated("Use get_gains() instead")]] Gains getGains() {
+    return get_gains();
+  }
 
   /*!
    * \brief Set PID gains for the controller.
@@ -241,15 +297,53 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    *
    * \note New gains are not applied if i_min > i_max
    */
-  void setGains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+  void set_gains(double p, double i, double d, double i_max, double i_min, bool antiwindup = false);
+
+  /*!
+   * \brief Set PID gains for the controller.
+   * \param p The proportional gain.
+   * \param i The integral gain.
+   * \param d The derivative gain.
+   * \param i_max The max integral windup.
+   * \param i_min The min integral windup.
+   * \param antiwindup If true, antiwindup is enabled and i_max/i_min are enforced
+   *
+   * \note New gains are not applied if i_min > i_max
+   */
+  [[deprecated("Use set_gains() instead")]] void setGains(
+    double p, double i, double d, double i_max, double i_min, bool antiwindup = false) {
+    set_gains(p, i, d, i_max, i_min, antiwindup);
+  }
+
+  /*!
+   * \brief Set PID gains for the controller.
+   * \param gains A struct of the PID gain values
+   *
+   * \note New gains are not applied if gains.i_min_ > gains.i_max_
+   */
+  void set_gains(const Gains & gains);
 
   /*!
    * \brief Set PID gains for the controller.
    * \param gains A struct of the PID gain values
    *
    * \note New gains are not applied if gains.i_min_ > gains.i_max_
    */
-  void setGains(const Gains & gains);
+  [[deprecated("Use set_gains() instead")]] void setGains(const Gains & gains) {
+    set_gains(gains);
+  }
+
+  /*!
+   * \brief Set the PID error and compute the PID command with nonuniform time
+   * step size. The derivative error is computed from the change in the error
+   * and the timestep \c dt.
+   *
+   * \param error  Error since last call (error = target - state)
+   * \param dt Change in time since last call in seconds
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, const double & dt_s);
 
   /*!
    * \brief Set the PID error and compute the PID command with nonuniform time
@@ -261,7 +355,59 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    *
    * \returns PID command
    */
-  [[nodiscard]] double computeCommand(double error, uint64_t dt);
+  [[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand(
+    double error, uint64_t dt_ns) {
+    return compute_command(error, static_cast<double>(dt_ns) / 1.e9);
+  }
+
+  /*!
+   * \brief Set the PID error and compute the PID command with nonuniform time
+   * step size. The derivative error is computed from the change in the error
+   * and the timestep \c dt.
+   *
+   * \param error  Error since last call (error = target - state)
+   * \param dt Change in time since last call, measured in nanoseconds.
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, const rcl_duration_value_t & dt_ns);
+
+  /*!
+   * \brief Set the PID error and compute the PID command with nonuniform time
+   * step size. The derivative error is computed from the change in the error
+   * and the timestep \c dt.
+   *
+   * \param error  Error since last call (error = target - state)
+   * \param dt Change in time since last call
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, const rclcpp::Duration & dt);
+
+  /*!
+   * \brief Set the PID error and compute the PID command with nonuniform time
+   * step size. The derivative error is computed from the change in the error
+   * and the timestep \c dt.
+   *
+   * \param error  Error since last call (error = target - state)
+   * \param dt Change in time since last call
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, const std::chrono::nanoseconds & dt_ns);
+
+  /*!
+   * \brief Set the PID error and compute the PID command with nonuniform
+   * time step size. This also allows the user to pass in a precomputed
+   * derivative error.
+   *
+   * \param error Error since last call (error = target - state)
+   * \param error_dot d(Error)/dt since last call
+   * \param dt Change in time since last call in seconds
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, double error_dot, const double & dt_s);
 
   /*!
    * \brief Set the PID error and compute the PID command with nonuniform
@@ -274,30 +420,104 @@ class CONTROL_TOOLBOX_PUBLIC Pid
    *
    * \returns PID command
    */
-  [[nodiscard]] double computeCommand(double error, double error_dot, uint64_t dt);
+  [[deprecated("Use compute_command() instead")]] [[nodiscard]] double computeCommand(
+    double error, double error_dot, uint64_t dt_ns) {
+    return compute_command(error, error_dot, static_cast<double>(dt_ns) / 1.e9);
+  }
+
+  /*!
+   * \brief Set the PID error and compute the PID command with nonuniform
+   * time step size. This also allows the user to pass in a precomputed
+   * derivative error.
+   *
+   * \param error Error since last call (error = target - state)
+   * \param error_dot d(Error)/dt since last call
+   * \param dt Change in time since last call, measured in nanoseconds.
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(
+    double error, double error_dot, const rcl_duration_value_t & dt_ns);
+
+  /*!
+   * \brief Set the PID error and compute the PID command with nonuniform
+   * time step size. This also allows the user to pass in a precomputed
+   * derivative error.
+   *
+   * \param error Error since last call (error = target - state)
+   * \param error_dot d(Error)/dt since last call
+   * \param dt Change in time since last call
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, double error_dot, const rclcpp::Duration & dt);
+
+  /*!
+   * \brief Set the PID error and compute the PID command with nonuniform
+   * time step size. This also allows the user to pass in a precomputed
+   * derivative error.
+   *
+   * \param error Error since last call (error = target - state)
+   * \param error_dot d(Error)/dt since last call
+   * \param dt Change in time since last call, measured in nanoseconds.
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(
+      double error, double error_dot, const std::chrono::nanoseconds & dt_ns);
 
   /*!
    * \brief Set current command for this PID controller
    */
-  void setCurrentCmd(double cmd);
+  void set_current_cmd(double cmd);
+
+  /*!
+   * \brief Set current command for this PID controller
+   */
+  [[deprecated("Use set_current_cmd() instead")]] void setCurrentCmd(double cmd) {
+    set_current_cmd(cmd);
+  }
 
   /*!
    * \brief Return current command for this PID controller
    */
-  double getCurrentCmd();
+  double get_current_cmd();
+
+  /*!
+   * \brief Return current command for this PID controller
+   */
+  [[deprecated("Use get_current_cmd() instead")]] double getCurrentCmd() {
+    return get_current_cmd();
+  }
 
   /*!
    * \brief Return derivative error
    */
-  double getDerivativeError();
+  [[deprecated("Use get_current_pid_errors() instead")]]
+  double getDerivativeError() {
+    double pe, ie, de;
+    get_current_pid_errors(pe, ie, de);
+    return de;
+  }
+
+  /*!
+   * \brief Return PID error terms for the controller.
+   * \param pe  The proportional error.
+   * \param ie  The integral error.
+   * \param de  The derivative error.
+   */
+  void get_current_pid_errors(double & pe, double & ie, double & de);
 
   /*!
    * \brief Return PID error terms for the controller.
    * \param pe  The proportional error.
    * \param ie  The integral error.
    * \param de  The derivative error.
    */
-  void getCurrentPIDErrors(double & pe, double & ie, double & de);
+  [[deprecated("Use get_current_pid_errors() instead")]] void getCurrentPIDErrors(
+    double & pe, double & ie, double & de) {
+    get_current_pid_errors(pe, ie, de);
+  }
 
   /*!
    * @brief Custom assignment operator
@@ -323,12 +543,13 @@ class CONTROL_TOOLBOX_PUBLIC Pid
   // blocking the realtime update loop
   realtime_tools::RealtimeBuffer<Gains> gains_buffer_;
 
-  double p_error_last_; /**< _Save position state for derivative state calculation. */
-  double p_error_;      /**< Position error. */
-  double i_error_;      /**< Integral of position error. */
-  double d_error_;      /**< Derivative of position error. */
-  double cmd_;          /**< Command to send. */
-  double error_dot_;    /**< Derivative error */
+  double p_error_last_; /** Save state for derivative state calculation. */
+  double p_error_;      /** Error. */
+  double i_error_;      /** Integral of error. */
+  double d_error_;      /** Derivative of error. */
+  double cmd_;          /** Command to send. */
+  // TODO(christophfroehlich) remove this -> breaks ABI
+  [[deprecated("Use d_error_")]] double error_dot_;    /** Derivative error */
 };
 
 }  // namespace control_toolbox