test(raw_vehicle_cmd_converter): add tests (#8951)

* remove header file according to clangd warning Signed-off-by: Go Sakayori <[email protected]> * add test Signed-off-by: Go Sakayori <[email protected]> * fix Signed-off-by: Go Sakayori <[email protected]> * add test for get function Signed-off-by: Go Sakayori <[email protected]> * apply clang tidy Signed-off-by: Go Sakayori <[email protected]> * fix test content Signed-off-by: Go Sakayori <[email protected]> --------- Signed-off-by: Go Sakayori <[email protected]> Signed-off-by: Go Sakayori <[email protected]>
autowarefoundation · Sep 26, 2024 · ff669fd · ff669fd
1 parent 9ea73bb
commit ff669fd
Show file tree

Hide file tree

Showing 7 changed files with 46 additions and 24 deletions.
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp
@@ -16,13 +16,9 @@
 
 #include "autoware/interpolation/linear_interpolation.hpp"
 
-#include <algorithm>
-#include <chrono>
 #include <string>
 #include <vector>
 
-using namespace std::literals::chrono_literals;
-
 namespace autoware::raw_vehicle_cmd_converter
 {
 bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool validation)
@@ -38,18 +34,16 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool vali
   vel_index_ = CSVLoader::getRowIndex(table);
   throttle_index_ = CSVLoader::getColumnIndex(table);
   accel_map_ = CSVLoader::getMap(table);
-  if (validation && !CSVLoader::validateMap(accel_map_, true)) {
-    return false;
-  }
-  return true;
+  return !validation || CSVLoader::validateMap(accel_map_, true);
 }
 
 bool AccelMap::getThrottle(const double acc, double vel, double & throttle) const
 {
   std::vector<double> interpolated_acc_vec;
   const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel");
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
-  for (std::vector<double> accelerations : accel_map_) {
+  interpolated_acc_vec.reserve(accel_map_.size());
+  for (const std::vector<double> & accelerations : accel_map_) {
     interpolated_acc_vec.push_back(
       autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel));
   }
@@ -58,7 +52,8 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons
   // When the desired acceleration is greater than the throttle area, return max throttle
   if (acc < interpolated_acc_vec.front()) {
     return false;
-  } else if (interpolated_acc_vec.back() < acc) {
+  }
+  if (interpolated_acc_vec.back() < acc) {
     throttle = throttle_index_.back();
     return true;
   }
@@ -72,6 +67,7 @@ bool AccelMap::getAcceleration(const double throttle, const double vel, double &
   const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel");
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
+  interpolated_acc_vec.reserve(accel_map_.size());
   for (const auto & acc_vec : accel_map_) {
     interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel));
   }

diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp
@@ -50,7 +50,8 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake)
   const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel");
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
-  for (std::vector<double> accelerations : brake_map_) {
+  interpolated_acc_vec.reserve(brake_map_.size());
+  for (const std::vector<double> & accelerations : brake_map_) {
     interpolated_acc_vec.push_back(
       autoware::interpolation::lerp(vel_index_, accelerations, clamped_vel));
   }
@@ -66,7 +67,8 @@ bool BrakeMap::getBrake(const double acc, const double vel, double & brake)
       acc, interpolated_acc_vec.back());
     brake = brake_index_.back();
     return true;
-  } else if (interpolated_acc_vec.front() < acc) {
+  }
+  if (interpolated_acc_vec.front() < acc) {
     brake = brake_index_.front();
     return true;
   }
@@ -83,6 +85,7 @@ bool BrakeMap::getAcceleration(const double brake, const double vel, double & ac
   const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel");
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
+  interpolated_acc_vec.reserve(brake_map_.size());
   for (const auto & acc_vec : brake_map_) {
     interpolated_acc_vec.push_back(autoware::interpolation::lerp(vel_index_, acc_vec, clamped_vel));
   }

diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp
@@ -15,7 +15,6 @@
 #include "autoware_raw_vehicle_cmd_converter/csv_loader.hpp"
 
 #include <algorithm>
-#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -48,10 +47,7 @@ bool CSVLoader::readCSV(Table & result, const char delim)
       result.push_back(tokens);
     }
   }
-  if (!validateData(result, csv_path_)) {
-    return false;
-  }
-  return true;
+  return validateData(result, csv_path_);
 }
 
 bool CSVLoader::validateMap(const Map & map, const bool is_col_decent)

diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/pid.cpp
@@ -14,6 +14,8 @@
 
 #include "autoware_raw_vehicle_cmd_converter/pid.hpp"
 
+#include <math.h>
+
 #include <algorithm>
 #include <vector>
 
@@ -25,7 +27,7 @@ double PIDController::calculateFB(
   const double current_value, std::vector<double> & pid_contributions, std::vector<double> & errors)
 {
   const double error = target_value - current_value;
-  const bool enable_integration = (std::abs(reset_trigger_value) < 0.01) ? false : true;
+  const bool enable_integration = std::abs(reset_trigger_value) >= 0.01;
   return calculatePID(error, dt, enable_integration, pid_contributions, errors, false);
 }
 
@@ -48,7 +50,7 @@ double PIDController::calculatePID(
   }
   double ret_i = ki_ * error_integral_;
 
-  double error_differential;
+  double error_differential = 0.0;
   if (is_first_time_) {
     error_differential = 0;
     is_first_time_ = false;

diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp
@@ -35,16 +35,14 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path, const bool vali
   steer_index_ = CSVLoader::getRowIndex(table);
   output_index_ = CSVLoader::getColumnIndex(table);
   steer_map_ = CSVLoader::getMap(table);
-  if (validation && !CSVLoader::validateMap(steer_map_, true)) {
-    return false;
-  }
-  return true;
+  return !validation || CSVLoader::validateMap(steer_map_, true);
 }
 
 void SteerMap::getSteer(const double steer_rate, const double steer, double & output) const
 {
   const double clamped_steer = CSVLoader::clampValue(steer, steer_index_, "steer: steer");
   std::vector<double> steer_rate_interp = {};
+  steer_rate_interp.reserve(steer_map_.size());
   for (const auto & steer_rate_vec : steer_map_) {
     steer_rate_interp.push_back(
       autoware::interpolation::lerp(steer_index_, steer_rate_vec, clamped_steer));

diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/test/map_data/test_empty_map.csv b/vehicle/autoware_raw_vehicle_cmd_converter/test/map_data/test_empty_map.csv
diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/test/test_autoware_raw_vehicle_cmd_converter.cpp
@@ -21,7 +21,6 @@
 #include "gtest/gtest.h"
 
 #include <cmath>
-#include <stdexcept>
 
 /*
  * Throttle data: (vel, throttle -> acc)
@@ -124,6 +123,9 @@ TEST(ConverterTests, LoadValidPath)
   EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_1col_map.csv", true));
   EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_inconsistent_rows_map.csv", true));
   EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_not_interpolatable.csv", true));
+  EXPECT_FALSE(accel_map.readAccelMapFromCSV(map_path + "test_empty_map.csv", true));
+
+  EXPECT_FALSE(steer_map.readSteerMapFromCSV(map_path + "test_not_interpolatable.csv", true));
 }
 
 TEST(ConverterTests, AccelMapCalculation)
@@ -136,6 +138,16 @@ TEST(ConverterTests, AccelMapCalculation)
     return output;
   };
 
+  // for get function in acceleration
+  std::vector<double> map_column_idx = {0.0, 5.0, 10.0};
+  std::vector<double> map_raw_idx = {0.0, 0.5, 1.0};
+  std::vector<std::vector<double>> map_value = {
+    {0.0, -0.3, -0.5}, {1.0, 0.5, 0.0}, {3.0, 2.0, 1.5}};
+
+  EXPECT_EQ(accel_map.getVelIdx(), map_column_idx);
+  EXPECT_EQ(accel_map.getThrottleIdx(), map_raw_idx);
+  EXPECT_EQ(accel_map.getAccelMap(), map_value);
+
   // case for max vel nominal acc
   EXPECT_DOUBLE_EQ(calcThrottle(0.0, 20.0), 0.5);
 
@@ -148,6 +160,9 @@ TEST(ConverterTests, AccelMapCalculation)
   // case for interpolation
   EXPECT_DOUBLE_EQ(calcThrottle(2.0, 0.0), 0.75);
 
+  // case for max throttle
+  EXPECT_DOUBLE_EQ(calcThrottle(2.0, 10.0), 1.0);
+
   const auto calcAcceleration = [&](double throttle, double vel) {
     double output;
     accel_map.getAcceleration(throttle, vel, output);
@@ -177,6 +192,15 @@ TEST(ConverterTests, BrakeMapCalculation)
     return output;
   };
 
+  // for get function in brake
+  std::vector<double> map_column_idx = {0.0, 5.0, 10.0};
+  std::vector<double> map_raw_idx = {0.0, 0.5, 1.0};
+  std::vector<std::vector<double>> map_value = {
+    {0.0, -0.4, -0.5}, {-1.5, -2.0, -2.0}, {-2.0, -2.5, -3.0}};
+  EXPECT_EQ(brake_map.getVelIdx(), map_column_idx);
+  EXPECT_EQ(brake_map.getBrakeIdx(), map_raw_idx);
+  EXPECT_EQ(brake_map.getBrakeMap(), map_value);
+
   // case for min vel min acc
   EXPECT_DOUBLE_EQ(calcBrake(-2.5, 0.0), 1.0);
 
@@ -189,6 +213,9 @@ TEST(ConverterTests, BrakeMapCalculation)
   // case for interpolation
   EXPECT_DOUBLE_EQ(calcBrake(-2.25, 5.0), 0.75);
 
+  // case for min brake
+  EXPECT_DOUBLE_EQ(calcBrake(1.0, 5.0), 0.0);
+
   const auto calcAcceleration = [&](double brake, double vel) {
     double output;
     brake_map.getAcceleration(brake, vel, output);