change back to enable_integration

Signed-off-by: Zhe Shen <lucidshenzhe@gmail.com>

Author: HansOersted

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp

@@ -30,14 +30,14 @@ class PIDController
    * @brief calculate the output of this PID
    * @param [in] error previous error
    * @param [in] dt time step [s]
-   * @param [in] erase_integral if true, will erase the calculated integral (P (proportional) and I
+   * @param [in] is_integrated if false, will erase the calculated integral (P (proportional) and I
    * (integral))
    * @param [out] pid_contributions values of the proportional, integral, and derivative components
    * @return PID output
    * @throw std::runtime_error if gains or limits have not been set
    */
   double calculate(
-    const double error, const double dt, const bool erase_integral,
+    const double error, const double dt, const bool is_integrated,
     std::vector<double> & pid_contributions);
   /**
    * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms

control/pid_longitudinal_controller/src/pid.cpp

@@ -32,7 +32,7 @@ PIDController::PIDController()
 }
 
 double PIDController::calculate(
-  const double error, const double dt, const bool erase_integral,
+  const double error, const double dt, const bool enable_integration,
   std::vector<double> & pid_contributions)
 {
   if (!m_is_gains_set || !m_is_limits_set) {
@@ -41,14 +41,15 @@ double PIDController::calculate(
 
   const auto & p = m_params;
 
-  m_virtual_displacement_error = erase_integral ? 0.0 : m_virtual_displacement_error + error * dt;
+  m_virtual_displacement_error =
+    enable_integration ? m_virtual_displacement_error + error * dt : 0.0;
 
   double ret_p = p.kp * m_virtual_displacement_error;
   ret_p = std::min(std::max(ret_p, p.min_ret_p), p.max_ret_p);
 
   m_virtual_displacement_error_integral =
-    erase_integral ? 0.0
-                   : m_virtual_displacement_error_integral + m_virtual_displacement_error * dt;
+    enable_integration ? m_virtual_displacement_error_integral + m_virtual_displacement_error * dt
+                       : 0.0;
 
   const double ret_i =
     std::min(std::max(p.ki * m_virtual_displacement_error_integral, p.min_ret_i), p.max_ret_i);

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -1095,13 +1095,11 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
     (vehicle_is_moving || (m_enable_integration_at_low_speed && vehicle_is_stuck)) &&
     is_under_control;
 
-  const bool erase_integral = !enable_integration;
-
   const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel);
 
   std::vector<double> pid_contributions(3);
   const double pid_acc =
-    m_pid_vel.calculate(error_vel_filtered, control_data.dt, erase_integral, pid_contributions);
+    m_pid_vel.calculate(error_vel_filtered, control_data.dt, enable_integration, pid_contributions);
 
   // Feedforward scaling:
   // This is for the coordinate conversion where feedforward is applied, from Time to Arclength.

control/pid_longitudinal_controller/test/test_pid.cpp

@@ -24,18 +24,18 @@ TEST(TestPID, calculate_pid_output)
   const double dt = 1.0;
   double target = 10.0;
   double current = 0.0;
-  bool erase_integral = true;
+  bool enable_integration = false;
 
   PIDController pid;
 
   // Cannot calculate before initializing gains and limits
-  EXPECT_THROW(pid.calculate(0.0, dt, erase_integral, contributions), std::runtime_error);
+  EXPECT_THROW(pid.calculate(0.0, dt, enable_integration, contributions), std::runtime_error);
 
   pid.setGains(1.0, 1.0, 1.0);
   pid.setLimits(10.0, 0.0, 10.0, 0.0, 10.0, 0.0, 10.0, 0.0);
   double error = target - current;
   while (current != target) {
-    current = pid.calculate(error, dt, erase_integral, contributions);
+    current = pid.calculate(error, dt, enable_integration, contributions);
     EXPECT_EQ(contributions[0], 0.0);  // integral is erased
     EXPECT_EQ(contributions[1], 0.0);  // double integral is erased
     EXPECT_EQ(contributions[2], error);
@@ -45,22 +45,22 @@ TEST(TestPID, calculate_pid_output)
 
   pid.setGains(100.0, 100.0, 100.0);
   pid.setLimits(10.0, -10.0, 10.0, -10.0, 10.0, -10.0, 10.0, -10.0);
-  erase_integral = false;
+  enable_integration = true;
 
   // High errors to force each component to its upper limit
-  EXPECT_EQ(pid.calculate(0.0, dt, erase_integral, contributions), 0.0);
+  EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0);
   for (double error = 100.0; error < 1000.0; error += 100.0) {
-    EXPECT_EQ(pid.calculate(error, dt, erase_integral, contributions), 10.0);
+    EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), 10.0);
     EXPECT_EQ(contributions[0], 10.0);  // integral is not erased
     EXPECT_EQ(contributions[1], 10.0);  // double integral is not erased
     EXPECT_EQ(contributions[2], 10.0);
   }
   pid.reset();
 
   // Low errors to force each component to its lower limit
-  EXPECT_EQ(pid.calculate(0.0, dt, erase_integral, contributions), 0.0);
+  EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0);
   for (double error = -100.0; error > -1000.0; error -= 100.0) {
-    EXPECT_EQ(pid.calculate(error, dt, erase_integral, contributions), -10.0);
+    EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), -10.0);
     EXPECT_EQ(contributions[0], -10.0);  // integral is not erased
     EXPECT_EQ(contributions[1], -10.0);  // double integral is not erased
     EXPECT_EQ(contributions[2], -10.0);