From e13643b1b0874552aa39b98092cda3b56ba9c1cd Mon Sep 17 00:00:00 2001
From: ChenJun <chen.junn@outlook.com>
Date: Sat, 9 Apr 2022 14:53:42 +0800
Subject: [PATCH] Support pass in a precomputed derivative error

---
 include/control_toolbox/pid_ros.hpp | 13 +++++++++++++
 src/pid_ros.cpp                     |  8 ++++++++
 2 files changed, 21 insertions(+)

diff --git a/include/control_toolbox/pid_ros.hpp b/include/control_toolbox/pid_ros.hpp
index 38f50b4a..1f273ba0 100644
--- a/include/control_toolbox/pid_ros.hpp
+++ b/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
diff --git a/src/pid_ros.cpp b/src/pid_ros.cpp
index 98bc1bd9..71f07106 100644
--- a/src/pid_ros.cpp
+++ b/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()
 {