feat(raw-vehicle-cmd-converter): add test for pid (autowarefoundation…

…#2212)

* add-vehicle-pid-test

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

vehicle/raw_vehicle_cmd_converter/test/test_raw_vehicle_cmd_converter.cpp

@@ -16,6 +16,7 @@
 #include "gtest/gtest.h"
 #include "raw_vehicle_cmd_converter/accel_map.hpp"
 #include "raw_vehicle_cmd_converter/brake_map.hpp"
+#include "raw_vehicle_cmd_converter/pid.hpp"
 #include "raw_vehicle_cmd_converter/steer_map.hpp"
 
 #include <cmath>
@@ -49,6 +50,7 @@
 
 using raw_vehicle_cmd_converter::AccelMap;
 using raw_vehicle_cmd_converter::BrakeMap;
+using raw_vehicle_cmd_converter::PIDController;
 using raw_vehicle_cmd_converter::SteerMap;
 double epsilon = 1e-4;
 // may throw PackageNotFoundError exception for invalid package
@@ -70,6 +72,21 @@ bool loadSteerMapData(SteerMap & steer_map)
   return steer_map.readSteerMapFromCSV(map_path + "test_steer_map.csv");
 }
 
+PIDController createSteerPid(
+  double kp, double ki, double kd, double max_ret, double min_ret, double max_ret_p,
+  double min_ret_p, double max_ret_i, double min_ret_i, double max_ret_d, double min_ret_d,
+  double invalid_integration_decay)
+{
+  PIDController steer_pid;
+  steer_pid.setDecay(invalid_integration_decay);
+  steer_pid.setGains(kp, ki, kd);
+  steer_pid.setLimits(
+    max_ret, min_ret, max_ret_p, min_ret_p, max_ret_i, min_ret_i, max_ret_d, min_ret_d);
+  steer_pid.setInitialized();
+
+  return steer_pid;
+}
+
 TEST(ConverterTests, LoadExampleMap)
 {
   AccelMap accel_map;
@@ -212,3 +229,69 @@ TEST(ConverterTests, SteerMapCalculation)
   // case for interpolation
   EXPECT_DOUBLE_EQ(calcSteer(5.0, 5.0), 5.0);
 }
+
+TEST(PIDTests, calculateFB)
+{
+  PIDController steer_pid;
+
+  double dt = 0.1;
+  double vel = 5.0;
+  double target_value = 0.15;
+  double current_value;
+  std::vector<double> pid_contributions(3, 0.0);
+  std::vector<double> pid_errors(3, 0.0);
+  double fb_value;
+
+  std::vector<double> fb_values{8.0, 8.0, 8.0, 8.0, 8.0, 7.85, 6.4, 4.9, 3.4, 1.9};
+  std::vector<double> ret_p{8.0, 8.0, 8.0, 8.0, 8.0, 7.5, 6.0, 4.5, 3.0, 1.5};
+  std::vector<double> ret_i{0.225, 0.42, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5};
+  std::vector<double> ret_d{0.0, -0.15, -0.15, -0.15, -0.15, -0.15, -0.1, -0.1, -0.1, -0.1};
+
+  steer_pid = createSteerPid(150.0, 15.0, 1.0, 8.0, -8.0, 8.0, -8.0, 0.5, -0.5, 0.15, -0.15, 0.97);
+  current_value = 0;
+  for (int i = 0; i < 10; i++) {
+    fb_value =
+      steer_pid.calculateFB(target_value, dt, vel, current_value, pid_contributions, pid_errors);
+    EXPECT_NEAR(fb_value, fb_values.at(i), epsilon);
+    EXPECT_NEAR(pid_contributions.at(0), ret_p.at(i), epsilon);
+    EXPECT_NEAR(pid_contributions.at(1), ret_i.at(i), epsilon);
+    EXPECT_NEAR(pid_contributions.at(2), ret_d.at(i), epsilon);
+
+    if (i < 5) {
+      current_value += 0.02;
+    } else {
+      current_value += 0.01;
+    }
+  }
+
+  steer_pid.reset();
+  current_value = 0;
+  for (int i = 0; i < 10; i++) {
+    fb_value =
+      steer_pid.calculateFB(-target_value, dt, vel, current_value, pid_contributions, pid_errors);
+    EXPECT_NEAR(fb_value, -fb_values.at(i), epsilon);
+    EXPECT_NEAR(pid_contributions.at(0), -ret_p.at(i), epsilon);
+    EXPECT_NEAR(pid_contributions.at(1), -ret_i.at(i), epsilon);
+    EXPECT_NEAR(pid_contributions.at(2), -ret_d.at(i), epsilon);
+
+    if (i < 5) {
+      current_value -= 0.02;
+    } else {
+      current_value -= 0.01;
+    }
+  }
+
+  // invalid_integration_decay
+  steer_pid.reset();
+  current_value = 0;
+  vel = 5.0;
+  steer_pid.calculateFB(target_value, dt, vel, current_value, pid_contributions, pid_errors);
+  current_value = 0.02;
+  vel = 0.001;
+  fb_value =
+    steer_pid.calculateFB(target_value, dt, vel, current_value, pid_contributions, pid_errors);
+  EXPECT_NEAR(fb_value, 8.0, epsilon);
+  EXPECT_NEAR(pid_contributions.at(0), 8.0, epsilon);
+  EXPECT_NEAR(pid_contributions.at(1), 0.21825, epsilon);
+  EXPECT_NEAR(pid_contributions.at(2), -0.15, epsilon);
+}