fix(simple_planning_simulator, raw_vehicle_cmd_converter): swap row index and column index for csv loader (#8963)

swap row and column

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

Author: go-sakayori

simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp

@@ -41,8 +41,8 @@ class AccelerationMap
     }
 
     vehicle_name_ = table[0][0];
-    vel_index_ = CSVLoader::getRowIndex(table);
-    acc_index_ = CSVLoader::getColumnIndex(table);
+    vel_index_ = CSVLoader::getColumnIndex(table);
+    acc_index_ = CSVLoader::getRowIndex(table);
     acceleration_map_ = CSVLoader::getMap(table);
 
     std::cout << "[SimModelDelaySteerMapAccGeared]: success to read acceleration map from "

simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp

@@ -119,7 +119,7 @@ Map CSVLoader::getMap(const Table & table)
   return map;
 }
 
-std::vector<double> CSVLoader::getRowIndex(const Table & table)
+std::vector<double> CSVLoader::getColumnIndex(const Table & table)
 {
   // NOTE: table[0][i] represents the element in the 0-th row and i-th column
   // This means that we are getting the index of each column in the 0-th row
@@ -130,7 +130,7 @@ std::vector<double> CSVLoader::getRowIndex(const Table & table)
   return index;
 }
 
-std::vector<double> CSVLoader::getColumnIndex(const Table & table)
+std::vector<double> CSVLoader::getRowIndex(const Table & table)
 {
   // NOTE: table[i][0] represents the element in the i-th row and 0-th column
   // This means that we are getting the index of each row in the 0-th column

simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp

@@ -27,21 +27,19 @@ bool ActuationMap::readActuationMapFromCSV(const std::string & csv_path, const b
     return false;
   }
 
-  state_index_ = CSVLoader::getRowIndex(table);
-  actuation_index_ = CSVLoader::getColumnIndex(table);
+  state_index_ = CSVLoader::getColumnIndex(table);
+  actuation_index_ = CSVLoader::getRowIndex(table);
   actuation_map_ = CSVLoader::getMap(table);
-  if (validation && !CSVLoader::validateMap(actuation_map_, true)) {
-    return false;
-  }
-  return true;
+  return !validation || CSVLoader::validateMap(actuation_map_, true);
 }
 
 double ActuationMap::getControlCommand(const double actuation, const double state) const
 {
   std::vector<double> interpolated_control_vec{};
   const double clamped_state = CSVLoader::clampValue(state, state_index_);
 
-  for (std::vector<double> control_command_values : actuation_map_) {
+  interpolated_control_vec.reserve(actuation_map_.size());
+  for (const std::vector<double> & control_command_values : actuation_map_) {
     interpolated_control_vec.push_back(
       autoware::interpolation::lerp(state_index_, control_command_values, clamped_state));
   }
@@ -61,7 +59,8 @@ std::optional<double> AccelMap::getThrottle(const double acc, double vel) const
   const double clamped_vel = CSVLoader::clampValue(vel, vel_indices);
 
   // (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_indices, accelerations, clamped_vel));
   }
@@ -71,7 +70,8 @@ std::optional<double> AccelMap::getThrottle(const double acc, double vel) const
   // When the desired acceleration is greater than the throttle area, return max throttle
   if (acc < interpolated_acc_vec.front()) {
     return std::nullopt;
-  } else if (interpolated_acc_vec.back() < acc) {
+  }
+  if (interpolated_acc_vec.back() < acc) {
     return throttle_indices.back();
   }
 
@@ -88,7 +88,8 @@ double BrakeMap::getBrake(const double acc, double vel) const
   const double clamped_vel = CSVLoader::clampValue(vel, vel_indices);
 
   // (brake, vel, acc) map => (brake, 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_indices, accelerations, clamped_vel));
   }
@@ -98,7 +99,8 @@ double BrakeMap::getBrake(const double acc, double vel) const
   // When the desired acceleration is greater than the brake area, return min brake on the map
   if (acc < interpolated_acc_vec.back()) {
     return brake_indices.back();
-  } else if (interpolated_acc_vec.front() < acc) {
+  }
+  if (interpolated_acc_vec.front() < acc) {
     return brake_indices.front();
   }
 

vehicle/autoware_raw_vehicle_cmd_converter/src/accel_map.cpp

@@ -31,8 +31,8 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path, const bool vali
   }
 
   vehicle_name_ = table[0][0];
-  vel_index_ = CSVLoader::getRowIndex(table);
-  throttle_index_ = CSVLoader::getColumnIndex(table);
+  vel_index_ = CSVLoader::getColumnIndex(table);
+  throttle_index_ = CSVLoader::getRowIndex(table);
   accel_map_ = CSVLoader::getMap(table);
   return !validation || CSVLoader::validateMap(accel_map_, true);
 }

vehicle/autoware_raw_vehicle_cmd_converter/src/brake_map.cpp

@@ -32,8 +32,8 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path, const bool vali
   }
 
   vehicle_name_ = table[0][0];
-  vel_index_ = CSVLoader::getRowIndex(table);
-  brake_index_ = CSVLoader::getColumnIndex(table);
+  vel_index_ = CSVLoader::getColumnIndex(table);
+  brake_index_ = CSVLoader::getRowIndex(table);
   brake_map_ = CSVLoader::getMap(table);
   brake_index_rev_ = brake_index_;
   if (validation && !CSVLoader::validateMap(brake_map_, false)) {

vehicle/autoware_raw_vehicle_cmd_converter/src/csv_loader.cpp

@@ -115,7 +115,7 @@ Map CSVLoader::getMap(const Table & table)
   return map;
 }
 
-std::vector<double> CSVLoader::getRowIndex(const Table & table)
+std::vector<double> CSVLoader::getColumnIndex(const Table & table)
 {
   std::vector<double> index = {};
   for (size_t i = 1; i < table[0].size(); i++) {
@@ -124,7 +124,7 @@ std::vector<double> CSVLoader::getRowIndex(const Table & table)
   return index;
 }
 
-std::vector<double> CSVLoader::getColumnIndex(const Table & table)
+std::vector<double> CSVLoader::getRowIndex(const Table & table)
 {
   std::vector<double> index = {};
   for (size_t i = 1; i < table.size(); i++) {

vehicle/autoware_raw_vehicle_cmd_converter/src/steer_map.cpp

@@ -32,8 +32,8 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path, const bool vali
   }
 
   vehicle_name_ = table[0][0];
-  steer_index_ = CSVLoader::getRowIndex(table);
-  output_index_ = CSVLoader::getColumnIndex(table);
+  steer_index_ = CSVLoader::getColumnIndex(table);
+  output_index_ = CSVLoader::getRowIndex(table);
   steer_map_ = CSVLoader::getMap(table);
   return !validation || CSVLoader::validateMap(steer_map_, true);
 }