feat(raw_vehicle_cmd_converter): validate map at first before run time (

#2152)

* feat(raw_vehicle_cmd_converter): validate map at first before run time

Signed-off-by: taikitanaka3 <[email protected]>

* chore: remove debug

* fix: clamp method and add more unit tests

Signed-off-by: taikitanaka3 <[email protected]>

* feat: add more realistic params

Signed-off-by: taikitanaka3 <[email protected]>

* chore: revert to simple map

Signed-off-by: tanaka3 <[email protected]>

* Revert "chore: revert to simple map"

This reverts commit 0350724.

Signed-off-by: taikitanaka3 <[email protected]>
Signed-off-by: tanaka3 <[email protected]>

vehicle/raw_vehicle_cmd_converter/data/default/steer_map.csv

@@ -1,4 +1,14 @@
-default,-10,0,10
--10,-10,-10,-10
-0,0,0,0
-10,10,10,10
+default,-0.6,-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4,0.5,0.6
+-12,-0.29,-0.3234236132,-0.3468991362,-0.37274847,-0.4288688461,-0.4696573,-0.4781014157,-0.49,-0.51,-0.52,-0.53,-0.54,-0.55
+-10,-0.2,-0.2451308686,-0.2667782734,-0.3090752469,-0.3575200568,-0.3874868999,-0.4041493831,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47
+-8,-0.1,-0.1586739777,-0.2020698818,-0.2317654357,-0.2778998646,-0.3029839842,-0.3188172953,-0.32,-0.33,-0.34,-0.35,-0.36,-0.37
+-6,-0.0404761584,-0.0806574159,-0.1208386734,-0.1559621241,-0.1888907059,-0.210790018,-0.2322336736,-0.24,-0.25,-0.26,-0.27,-0.28,-0.29
+-4,0.01,-0.01428569928,-0.04,-0.06805018099,-0.09718925222,-0.1189509092,-0.1361293637,-0.14,-0.153044463,-0.1688321844,-0.18,-0.2,-0.22
+-2,0.06,0.05,0.03,0.01,-0.006091655083,-0.03546033903,-0.05319488695,-0.06097627388,-0.07576251508,-0.09165178816,-0.1,-0.12,-0.14
+0,0.11,0.09,0.07,0.05,0.03,0.01,-0.0004106386541,-0.01,-0.03,-0.05,-0.07,-0.09,-0.11
+2,0.15,0.13,0.11,0.09,0.07812978496,0.06023545297,0.04386326928,0.02314813566,0.003779338416,-0.01390526686,-0.0324840879,-0.05106290894,-0.06964172998
+4,0.24,0.22,0.2,0.1839795042,0.1649754952,0.1455879759,0.1274837062,0.1020632549,0.0861964856,0.06971503353,0.04541294018,0.01308869356,-0.01923555307
+6,0.33,0.31,0.29,0.274963497,0.2447295892,0.2337353319,0.2126138313,0.1767700907,0.1578757866,0.1440584148,0.1136331403,0.0827443595,0.05185557872
+8,0.43,0.41,0.39,0.3714915121,0.3299578073,0.3156403746,0.2997845963,0.2500495071,0.2339668568,0.2153133621,0.1815127859,0.156071251,0.1306297162
+10,0.5,0.49,0.48,0.4629455165,0.4210353203,0.3977027236,0.3865034288,0.3250957262,0.312237913,0.2885123051,0.2474220915,0.2123900615,0.1773580315
+12,0.56,0.54,0.53,0.5151036525,0.5046070554,0.4897214196,0.4687756354,0.4036994672,0.3812674227,0.3496883008,0.32482655,0.2783538423,0.2318811345

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/csv_loader.hpp

@@ -32,6 +32,7 @@ class CSVLoader
 
   bool readCSV(Table & result, const char delim = ',');
   static bool validateData(const Table & table, const std::string & csv_path);
+  static bool validateMap(const Map & map, const bool is_row_decent, const bool is_col_decent);
   static Map getMap(const Table & table);
   static std::vector<double> getRowIndex(const Table & table);
   static std::vector<double> getColumnIndex(const Table & table);

vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp

@@ -38,19 +38,20 @@ bool AccelMap::readAccelMapFromCSV(const std::string & csv_path)
   vel_index_ = CSVLoader::getRowIndex(table);
   throttle_index_ = CSVLoader::getColumnIndex(table);
   accel_map_ = CSVLoader::getMap(table);
+  if (!CSVLoader::validateMap(accel_map_, false, true)) {
+    return false;
+  }
   return 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.push_back(interpolation::lerp(vel_index_, accelerations, clamped_vel));
   }
-
   // calculate throttle
   // When the desired acceleration is smaller than the throttle area, return false => brake sequence
   // When the desired acceleration is greater than the throttle area, return max throttle
@@ -61,7 +62,6 @@ bool AccelMap::getThrottle(const double acc, double vel, double & throttle) cons
     return true;
   }
   throttle = interpolation::lerp(interpolated_acc_vec, throttle_index_, acc);
-
   return true;
 }
 

vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp

@@ -36,6 +36,9 @@ bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path)
   brake_index_ = CSVLoader::getColumnIndex(table);
   brake_map_ = CSVLoader::getMap(table);
   brake_index_rev_ = brake_index_;
+  if (!CSVLoader::validateMap(brake_map_, false, false)) {
+    return false;
+  }
   std::reverse(std::begin(brake_index_rev_), std::end(brake_index_rev_));
 
   return true;

vehicle/raw_vehicle_cmd_converter/src/csv_loader.cpp

@@ -14,6 +14,8 @@
 
 #include "raw_vehicle_cmd_converter/csv_loader.hpp"
 
+#include <algorithm>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -49,14 +51,54 @@ bool CSVLoader::readCSV(Table & result, const char delim)
   return true;
 }
 
+bool CSVLoader::validateMap(const Map & map, const bool is_row_decent, const bool is_col_decent)
+{
+  std::pair<size_t, size_t> invalid_index_pair;
+  bool is_invalid = false;
+  // validate interpolation
+  for (size_t i = 1; i < map.size(); i++) {
+    const auto & vec = map.at(i);
+    const auto & prev_vec = map.at(i - 1);
+    // validate row data
+    for (size_t j = 1; j < vec.size(); j++) {
+      // validate col
+      if (vec.at(j) < prev_vec.at(j) && is_col_decent) {
+        invalid_index_pair = std::make_pair(i, j);
+        is_invalid = true;
+      }
+      if (vec.at(j) > prev_vec.at(j) && !is_col_decent) {
+        invalid_index_pair = std::make_pair(i, j);
+        is_invalid = true;
+      }
+      // validate row
+      if (vec.at(j) < vec.at(j - 1) && is_row_decent) {
+        invalid_index_pair = std::make_pair(i, j);
+        is_invalid = true;
+      }
+      if (vec.at(j) > vec.at(j - 1) && !is_row_decent) {
+        invalid_index_pair = std::make_pair(i, j);
+        is_invalid = true;
+      }
+    }
+  }
+  if (is_invalid) {
+    std::cerr << "index around (i,j) is invalid ( " << invalid_index_pair.first << ", "
+              << invalid_index_pair.second << " )" << std::endl;
+    return false;
+  }
+  return true;
+}
+
 bool CSVLoader::validateData(const Table & table, const std::string & csv_path)
 {
   if (table[0].size() < 2) {
     std::cerr << "Cannot read " << csv_path.c_str() << " CSV file should have at least 2 column"
               << std::endl;
     return false;
   }
+  // validate map size
   for (size_t i = 1; i < table.size(); i++) {
+    // validate row size
     if (table[0].size() != table[i].size()) {
       std::cerr << "Cannot read " << csv_path.c_str()
                 << ". Each row should have a same number of columns" << std::endl;
@@ -100,10 +142,10 @@ std::vector<double> CSVLoader::getColumnIndex(const Table & table)
 double CSVLoader::clampValue(
   const double val, const std::vector<double> & ranges, const std::string & name)
 {
-  const double max_value = ranges.back();
-  const double min_value = ranges.front();
+  const double max_value = *std::max_element(ranges.begin(), ranges.end());
+  const double min_value = *std::min_element(ranges.begin(), ranges.end());
   if (val < min_value || max_value < val) {
-    std::cerr << "Input" << name << ": " << val << " is out off range. use closest value."
+    std::cerr << "Input " << name << ": " << val << " is out of range. use closest value."
               << std::endl;
     return std::min(std::max(val, min_value), max_value);
   }

vehicle/raw_vehicle_cmd_converter/src/node.cpp

@@ -15,6 +15,7 @@
 #include "raw_vehicle_cmd_converter/node.hpp"
 
 #include <algorithm>
+#include <stdexcept>
 #include <string>
 #include <vector>
 
@@ -40,20 +41,19 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
   // for steering steer controller
   use_steer_ff_ = declare_parameter("use_steer_ff", true);
   use_steer_fb_ = declare_parameter("use_steer_fb", true);
-  ff_map_initialized_ = true;
   if (convert_accel_cmd_) {
     if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map)) {
-      ff_map_initialized_ = false;
+      throw std::invalid_argument("Accel map is invalid.");
     }
   }
   if (convert_brake_cmd_) {
     if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) {
-      ff_map_initialized_ = false;
+      throw std::invalid_argument("Brake map is invalid.");
     }
   }
   if (convert_steer_cmd_) {
     if (!steer_map_.readSteerMapFromCSV(csv_path_steer_map)) {
-      ff_map_initialized_ = false;
+      throw std::invalid_argument("Steer map is invalid.");
     }
     const auto kp_steer{declare_parameter("steer_pid.kp", 150.0)};
     const auto ki_steer{declare_parameter("steer_pid.ki", 15.0)};
@@ -88,10 +88,6 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode(
 
 void RawVehicleCommandConverterNode::publishActuationCmd()
 {
-  if (!ff_map_initialized_) {
-    RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "ff map is not initialized");
-    return;
-  }
   if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_) {
     RCLCPP_WARN_EXPRESSION(
       get_logger(), is_debugging_, "some pointers are null: %s, %s, %s",
@@ -148,25 +144,17 @@ double RawVehicleCommandConverterNode::calculateSteer(
   double dt = (current_time - prev_time_steer_calculation_).seconds();
   if (std::abs(dt) > 1.0) {
     RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "ignore old topic");
-    dt = 0.0;
+    dt = 0.1;  // set ordinaray delta time instead
   }
   prev_time_steer_calculation_ = current_time;
   // feed-forward
   if (use_steer_ff_) {
-    if (!ff_map_initialized_) {
-      RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "FF map is not initialized!");
-    } else {
-      steer_map_.getSteer(steer_rate, *current_steer_ptr_, ff_value);
-    }
+    steer_map_.getSteer(steer_rate, *current_steer_ptr_, ff_value);
   }
-  // feedback
+  // feed-back
   if (use_steer_fb_) {
-    if (!steer_pid_.getInitialized()) {
-      RCLCPP_WARN_EXPRESSION(get_logger(), is_debugging_, "FF map is not initialized!");
-    } else {
-      fb_value = steer_pid_.calculateFB(
-        steering, dt, vel, *current_steer_ptr_, pid_contributions, pid_errors);
-    }
+    fb_value =
+      steer_pid_.calculateFB(steering, dt, vel, *current_steer_ptr_, pid_contributions, pid_errors);
   }
   steering_output = ff_value + fb_value;
   // for steer debugging

vehicle/raw_vehicle_cmd_converter/src/steer_map.cpp

@@ -35,6 +35,9 @@ bool SteerMap::readSteerMapFromCSV(const std::string & csv_path)
   steer_index_ = CSVLoader::getRowIndex(table);
   output_index_ = CSVLoader::getColumnIndex(table);
   steer_map_ = CSVLoader::getMap(table);
+  if (!CSVLoader::validateMap(steer_map_, false, true)) {
+    return false;
+  }
   return true;
 }
 

vehicle/raw_vehicle_cmd_converter/test/map_data/test_accel_map.csv

@@ -1,4 +1,4 @@
-default,0.0,  10.0,  20.0
-0,      1.0,  11.0,  21.0
-0.5,    2.0,  22.0,  42.0
-1.0,    3.0,  33.0,  46.0
+default,0.0,  5.0,  10.0
+0.0,    0.0, -0.3, -0.5
+0.5,    1.0,  0.5,  0.0
+1.0,    3.0,  2.0,  1.5

vehicle/raw_vehicle_cmd_converter/test/map_data/test_brake_map.csv

@@ -1,4 +1,4 @@
-default, 0.0,   10.0,   20.0
-0,      -1.0,  -11.0,  -21.0
-0.5,    -2.0,  -22.0,  -42.0
-1.0,    -3.0,  -33.0,  -46.0
+default, 0.0,   5.0,   10.0
+0,       0.0,  -0.4,  -0.5
+0.5,    -1.5,  -2.0,  -2.0
+1.0,    -2.0,  -2.5,  -3.0

vehicle/raw_vehicle_cmd_converter/test/map_data/test_inconsistent_rows_map.csv

@@ -1,4 +1,4 @@
 default,0.0,  10.0,  20.0
-0,      1.0,  11.0,  21.0
-0.5,    2.0,  22.0,  42.0
-1.0,    3.0,  46.0
+0,      21.0,  11.0,  1.0
+0.5,    42.0,  22.0,  2.0
+1.0,    46.0,  33.0

vehicle/raw_vehicle_cmd_converter/test/map_data/test_not_interpolatable.csv

@@ -0,0 +1,4 @@
+default,0.0,  10.0,  20.0
+0,      1.0,  11.0,  21.0
+0.5,    0.9,  22.0,  42.0
+1.0,    3.0,  46.0,   42.0