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.h"
 
 #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.init_pid(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,25 @@ 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 init_pid(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 init_pid() instead")]] void initPid(
+    double p, double i, double d, double i_max, double i_min, bool antiwindup = false) {
+    init_pid(p, i, d, i_max, i_min, antiwindup);
+  }
 
   /*!
    * \brief Reset the state of this PID controller
@@ -211,7 +230,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 +254,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 getGains();
+  Gains get_gains();
+
+  /*!
+   * \brief Get PID gains for the controller.
+   * \return gains A struct of the PID gain values
+   */
+  [[deprecated("Use get_gains() instead")]] Gains getGains() {
+    return get_gains();
+  }
 
   /*!
    * \brief Set PID gains for the controller.
@@ -241,15 +296,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 setGains(const Gains & gains);
+  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_
+   */
+  [[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, double dt_s);
 
   /*!
    * \brief Set the PID error and compute the PID command with nonuniform time
@@ -261,7 +354,65 @@ 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, rcl_duration_value_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
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, rclcpp::Duration dt) {
+    return compute_command(error, dt.seconds());
+  }
+
+  /*!
+   * \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 computeCommand(double error, std::chrono::nanoseconds dt_ns) {
+    return compute_command(error, static_cast<double>(dt_ns.count())/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 in seconds
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, double error_dot, double dt_s);
 
   /*!
    * \brief Set the PID error and compute the PID command with nonuniform
@@ -274,30 +425,110 @@ 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, rcl_duration_value_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
+   *
+   * \returns PID command
+   */
+  [[nodiscard]] double compute_command(double error, double error_dot, rclcpp::Duration dt) {
+    return compute_command(error, error_dot, dt.seconds());
+  }
+
+  /*!
+   * \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, std::chrono::nanoseconds dt_ns) {
+    return compute_command(error, error_dot, static_cast<double>(dt_ns.count())/1.e9);
+  }
 
   /*!
    * \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 get_derivative_error();
 
   /*!
    * \brief Return derivative error
    */
-  double getDerivativeError();
+  [[deprecated("Use get_derivative_error() instead")]]
+  double getDerivativeError() { return get_derivative_error(); }
 
   /*!
    * \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);
+  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.
+   */
+  [[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