Support pass in a precomputed derivative error

include/control_toolbox/pid_ros.hpp

@@ -122,6 +122,19 @@ class CONTROL_TOOLBOX_PUBLIC PidROS
    */
   double computeCommand(double error, 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 in seconds
+   *
+   * \returns PID command
+   */
+  double computeCommand(double error, double error_dot, rclcpp::Duration dt);
+
   /*!
    * \brief Get PID gains for the controller.
    * \return gains A struct of the PID gain values

src/pid_ros.cpp

@@ -173,6 +173,14 @@ PidROS::computeCommand(double error, rclcpp::Duration dt)
   return cmd_;
 }
 
+double PidROS::computeCommand(double error, double error_dot, rclcpp::Duration dt)
+{
+  double cmd_ = pid_.computeCommand(error, error_dot, dt.nanoseconds());
+  publishPIDState(cmd_, error, dt);
+
+  return cmd_;
+}
+
 Pid::Gains
 PidROS::getGains()
 {