refactor(raw_vehicle_cmd_converter): remove duplicates and add unit test

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp

@@ -29,12 +29,12 @@ namespace raw_vehicle_cmd_converter
 class AccelMap
 {
 public:
-  bool readAccelMapFromCSV(std::string csv_path);
-  bool getThrottle(double acc, double vel, double & throttle);
-  bool getAcceleration(double throttle, double vel, double & acc);
-  std::vector<double> getVelIdx() { return vel_index_; }
-  std::vector<double> getThrottleIdx() { return throttle_index_; }
-  std::vector<std::vector<double>> getAccelMap() { return accel_map_; }
+  bool readAccelMapFromCSV(const std::string & csv_path);
+  bool getThrottle(const double acc, double vel, double & throttle) const;
+  bool getAcceleration(double throttle, double vel, double & acc) const;
+  std::vector<double> getVelIdx() const { return vel_index_; }
+  std::vector<double> getThrottleIdx() const { return throttle_index_; }
+  std::vector<std::vector<double>> getAccelMap() const { return accel_map_; }
 
 private:
   rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("accel_map")};

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp

@@ -29,12 +29,12 @@ namespace raw_vehicle_cmd_converter
 class BrakeMap
 {
 public:
-  bool readBrakeMapFromCSV(std::string csv_path);
-  bool getBrake(double acc, double vel, double & brake);
-  bool getAcceleration(double brake, double vel, double & acc);
-  std::vector<double> getVelIdx() { return vel_index_; }
-  std::vector<double> getBrakeIdx() { return brake_index_; }
-  std::vector<std::vector<double>> getBrakeMap() { return brake_map_; }
+  bool readBrakeMapFromCSV(const std::string & csv_path);
+  bool getBrake(const double acc, double vel, double & brake);
+  bool getAcceleration(double brake, double vel, double & acc) const;
+  std::vector<double> getVelIdx() const { return vel_index_; }
+  std::vector<double> getBrakeIdx() const { return brake_index_; }
+  std::vector<std::vector<double>> getBrakeMap() const { return brake_map_; }
 
 private:
   rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("accel_map")};

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/csv_loader.hpp

@@ -24,15 +24,19 @@
 namespace raw_vehicle_cmd_converter
 {
 using Table = std::vector<std::vector<std::string>>;
+using Map = std::vector<std::vector<double>>;
 class CSVLoader
 {
 public:
-  explicit CSVLoader(std::string csv_path);
+  explicit CSVLoader(const std::string & csv_path);
 
   bool readCSV(Table & result, const char delim = ',');
   static bool validateData(const Table & table, const std::string & csv_path);
+  static Map getMap(const Table & table);
   static std::vector<double> getRowIndex(const Table & table);
   static std::vector<double> getColumnIndex(const Table & table);
+  static double clampValue(
+    const double val, const std::vector<double> & ranges, const std::string & name);
 
 private:
   std::string csv_path_;

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp

@@ -87,6 +87,9 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
   AckermannControlCommand::ConstSharedPtr control_cmd_ptr_;
   AccelMap accel_map_;
   BrakeMap brake_map_;
+  SteerMap steer_map_;
+  // TODO(tanaka): consider accel/brake pid too
+  PIDController steer_pid_;
   bool ff_map_initialized_;
   double max_accel_cmd_;
   double max_brake_cmd_;
@@ -99,7 +102,6 @@ class RawVehicleCommandConverterNode : public rclcpp::Node
   bool convert_accel_cmd_;  //!< @brief use accel or not
   bool convert_brake_cmd_;  //!< @brief use brake or not
   bool convert_steer_cmd_;  //!< @brief use steer or not
-  SteerConverter steer_controller_;
   rclcpp::Time prev_time_steer_calculation_{0, 0, RCL_ROS_TIME};
 
   double calculateAccelMap(

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/pid.hpp

@@ -24,6 +24,10 @@ namespace raw_vehicle_cmd_converter
 class PIDController
 {
 public:
+  double calculateFB(
+    const double target_value, const double dt, const double reset_trigger_value,
+    const double current_value, std::vector<double> & pid_contributions,
+    std::vector<double> & errors);
   double calculatePID(
     const double error, const double dt, const bool enable_integration,
     std::vector<double> & pid_contributions, std::vector<double> & errors, bool is_debugging);
@@ -33,6 +37,8 @@ class PIDController
     const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d);
   void reset();
   void setDecay(const double decay) { invalid_integration_decay_ = decay; }
+  void setInitialized() { is_initialized_ = true; }
+  bool getInitialized() { return is_initialized_; }
 
 private:
   // parameters
@@ -52,6 +58,7 @@ class PIDController
   double prev_error_{0.0};
   bool is_first_time_{true};
   double invalid_integration_decay_{0.0};
+  double is_initialized_{false};
 };
 }  // namespace raw_vehicle_cmd_converter
 

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_converter.hpp

@@ -25,40 +25,19 @@
 
 namespace raw_vehicle_cmd_converter
 {
-class SteerConverter
+class SteerMap
 {
 public:
-  bool setFFMap(const std::string & csv_path);
-  void setFBGains(const double kp, const double ki, const double kd);
-  bool setFBLimits(
-    const double max_ret, const double min_ret, const double max_ret_p, const double min_ret_p,
-    const double max_ret_i, const double min_ret_i, const double max_ret_d, const double min_ret_d);
-  double calcFFSteer(
-    const double target_steer_angle_velocity, const double current_steer_angle) const;
-  double calcFBSteer(
-    const double target_steer_angle, const double dt, const double current_velocity,
-    const double current_steer_angle, std::vector<double> & pid_contributions,
-    std::vector<double> & errors);
-  void setDecay(const double decay) { pid_.setDecay(decay); }
+  bool readSteerMapFromCSV(const std::string & csv_path);
+  void getSteer(double steer_rate, double steer, double & output) const;
 
 private:
-  double kp_, ki_, kd_;
   std::string vehicle_name_;
-  std::vector<double> vel_index_;
+  std::vector<double> steer_index_;
   std::vector<double> output_index_;
   std::vector<std::vector<double>> steer_map_;
-  PIDController pid_;
-  bool ff_map_initialized_{false};
-  bool fb_gains_initialized_{false};
-  bool fb_limits_initialized_{false};
-
   rclcpp::Logger logger_{
     rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("steer_converter")};
-
-  bool readSteerMapFromCSV(
-    const std::string & csv_path, std::string & vehicle_name, std::vector<double> & vel_index,
-    std::vector<double> & output_index, std::vector<std::vector<double>> & steer_map) const;
-  void calcFFMap(double steer_vel, double current_steer_val, double & output) const;
 };
 }  // namespace raw_vehicle_cmd_converter
 

vehicle/raw_vehicle_cmd_converter/package.xml

@@ -5,10 +5,13 @@
   <version>0.1.0</version>
   <description>The raw_vehicle_cmd_converter package</description>
   <maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
+  <maintainer email="taiki.tanaka@tier4.jp">Tanaka Taiki</maintainer>
+
   <license>Apache License 2.0</license>
 
   <author email="makoto.kurihara@tier4.jp">Makoto Kurihara</author>
   <author email="takamasa.horibe@tier4.jp">Takamasa Horibe</author>
+  <author email="taiki.tanaka@tier4.jp">Tanaka Taiki</author>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
 

vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp

@@ -25,7 +25,7 @@ using namespace std::literals::chrono_literals;
 
 namespace raw_vehicle_cmd_converter
 {
-bool AccelMap::readAccelMapFromCSV(std::string csv_path)
+bool AccelMap::readAccelMapFromCSV(const std::string & csv_path)
 {
   CSVLoader csv(csv_path);
   std::vector<std::vector<std::string>> table;
@@ -37,76 +37,49 @@ bool AccelMap::readAccelMapFromCSV(std::string csv_path)
   vehicle_name_ = table[0][0];
   vel_index_ = CSVLoader::getRowIndex(table);
   throttle_index_ = CSVLoader::getColumnIndex(table);
-
-  for (unsigned int i = 1; i < table.size(); i++) {
-    std::vector<double> accs;
-    for (unsigned int j = 1; j < table[i].size(); j++) {
-      accs.push_back(std::stod(table[i][j]));
-    }
-    accel_map_.push_back(accs);
-  }
-
+  accel_map_ = CSVLoader::getMap(table);
   return true;
 }
 
-bool AccelMap::getThrottle(double acc, double vel, double & throttle)
+bool AccelMap::getThrottle(const double acc, double vel, double & throttle) const
 {
-  std::vector<double> accs_interpolated;
-
-  if (vel < vel_index_.front() || vel_index_.back() < vel) {
-    RCLCPP_WARN_SKIPFIRST_THROTTLE(
-      logger_, clock_, 1000, "Exceeding the  min:%f  < current vel:%f < max:%f.",
-      vel_index_.front(), vel, vel_index_.back());
-    vel = std::min(std::max(vel, vel_index_.front()), vel_index_.back());
-  }
+  std::vector<double> accelerations_interpolated;
+  vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel");
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
-  for (std::vector<double> accs : accel_map_) {
-    accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
+  for (std::vector<double> accelerations : accel_map_) {
+    accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, 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
-  if (acc < accs_interpolated.front()) {
+  if (acc < accelerations_interpolated.front()) {
     return false;
-  } else if (accs_interpolated.back() < acc) {
+  } else if (accelerations_interpolated.back() < acc) {
     throttle = throttle_index_.back();
     return true;
   }
-  throttle = interpolation::lerp(accs_interpolated, throttle_index_, acc);
+  throttle = interpolation::lerp(accelerations_interpolated, throttle_index_, acc);
 
   return true;
 }
 
-bool AccelMap::getAcceleration(double throttle, double vel, double & acc)
+bool AccelMap::getAcceleration(double throttle, double vel, double & acc) const
 {
-  std::vector<double> accs_interpolated;
-
-  if (vel < vel_index_.front() || vel_index_.back() < vel) {
-    RCLCPP_WARN_SKIPFIRST_THROTTLE(
-      logger_, clock_, 1000, "Exceeding the  min:%f  < current vel:%f < max:%f.",
-      vel_index_.front(), vel, vel_index_.back());
-    vel = std::min(std::max(vel, vel_index_.front()), vel_index_.back());
-  }
+  std::vector<double> accelerations_interpolated;
+  vel = CSVLoader::clampValue(vel, vel_index_, "throttle: vel");
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
-  for (std::vector<double> accs : accel_map_) {
-    accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
+  for (std::vector<double> accelerations : accel_map_) {
+    accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, 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
-  const double max_throttle = throttle_index_.back();
-  const double min_throttle = throttle_index_.front();
-  if (throttle < min_throttle || max_throttle < throttle) {
-    RCLCPP_WARN_SKIPFIRST_THROTTLE(
-      logger_, clock_, 1000, "Input throttle: %f is out off range. use closest value.", throttle);
-    throttle = std::min(std::max(throttle, min_throttle), max_throttle);
-  }
-
-  acc = interpolation::lerp(throttle_index_, accs_interpolated, throttle);
+  // When the desired acceleration is smaller than the throttle area, return min acc
+  // When the desired acceleration is greater than the throttle area, return max acc
+  throttle = CSVLoader::clampValue(throttle, throttle_index_, "throttle: acc");
+  acc = interpolation::lerp(throttle_index_, accelerations_interpolated, throttle);
 
   return true;
 }

vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp

@@ -22,7 +22,7 @@
 
 namespace raw_vehicle_cmd_converter
 {
-bool BrakeMap::readBrakeMapFromCSV(std::string csv_path)
+bool BrakeMap::readBrakeMapFromCSV(const std::string & csv_path)
 {
   CSVLoader csv(csv_path);
   std::vector<std::vector<std::string>> table;
@@ -34,87 +34,60 @@ bool BrakeMap::readBrakeMapFromCSV(std::string csv_path)
   vehicle_name_ = table[0][0];
   vel_index_ = CSVLoader::getRowIndex(table);
   brake_index_ = CSVLoader::getColumnIndex(table);
-
-  for (unsigned int i = 1; i < table.size(); i++) {
-    std::vector<double> accs;
-    for (unsigned int j = 1; j < table[i].size(); j++) {
-      accs.push_back(std::stod(table[i][j]));
-    }
-    brake_map_.push_back(accs);
-  }
-
+  brake_map_ = CSVLoader::getMap(table);
   brake_index_rev_ = brake_index_;
   std::reverse(std::begin(brake_index_rev_), std::end(brake_index_rev_));
 
   return true;
 }
 
-bool BrakeMap::getBrake(double acc, double vel, double & brake)
+bool BrakeMap::getBrake(const double acc, double vel, double & brake)
 {
-  std::vector<double> accs_interpolated;
+  std::vector<double> accelerations_interpolated;
+  vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel");
 
-  if (vel < vel_index_.front() || vel_index_.back() < vel) {
-    RCLCPP_WARN_SKIPFIRST_THROTTLE(
-      logger_, clock_, 1000, "Exceeding the  min:%f  < current vel:%f < max:%f.",
-      vel_index_.front(), vel, vel_index_.back());
-    vel = std::min(std::max(vel, vel_index_.front()), vel_index_.back());
-  }
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
-  for (std::vector<double> accs : brake_map_) {
-    accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
+  for (std::vector<double> accelerations : brake_map_) {
+    accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel));
   }
 
   // calculate brake
   // When the desired acceleration is smaller than the brake area, return max brake on the map
   // When the desired acceleration is greater than the brake area, return min brake on the map
-  if (acc < accs_interpolated.back()) {
+  if (acc < accelerations_interpolated.back()) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(
       logger_, clock_, 1000,
       "Exceeding the acc range. Desired acc: %f < min acc on map: %f. return max "
       "value.",
-      acc, accs_interpolated.back());
+      acc, accelerations_interpolated.back());
     brake = brake_index_.back();
     return true;
-  } else if (accs_interpolated.front() < acc) {
+  } else if (accelerations_interpolated.front() < acc) {
     brake = brake_index_.front();
     return true;
   }
 
-  std::reverse(std::begin(accs_interpolated), std::end(accs_interpolated));
-  brake = interpolation::lerp(accs_interpolated, brake_index_rev_, acc);
+  std::reverse(std::begin(accelerations_interpolated), std::end(accelerations_interpolated));
+  brake = interpolation::lerp(accelerations_interpolated, brake_index_rev_, acc);
 
   return true;
 }
 
-bool BrakeMap::getAcceleration(double brake, double vel, double & acc)
+bool BrakeMap::getAcceleration(double brake, double vel, double & acc) const
 {
-  std::vector<double> accs_interpolated;
-
-  if (vel < vel_index_.front() || vel_index_.back() < vel) {
-    RCLCPP_WARN_SKIPFIRST_THROTTLE(
-      logger_, clock_, 1000,
-      "Exceeding the vel range. Current vel: %f < min or max < %f vel on map: %f.", vel,
-      vel_index_.front(), vel_index_.back());
-    vel = std::min(std::max(vel, vel_index_.front()), vel_index_.back());
-  }
+  std::vector<double> accelerations_interpolated;
+  vel = CSVLoader::clampValue(vel, vel_index_, "brake: vel");
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
-  for (std::vector<double> accs : brake_map_) {
-    accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
+  for (std::vector<double> accelerations : brake_map_) {
+    accelerations_interpolated.push_back(interpolation::lerp(vel_index_, accelerations, vel));
   }
 
   // calculate brake
-  // When the desired acceleration is smaller than the brake area, return max brake on the map
-  // When the desired acceleration is greater than the brake area, return min brake on the map
-  const double max_brake = brake_index_.back();
-  const double min_brake = brake_index_.front();
-  if (brake < min_brake || max_brake < brake) {
-    RCLCPP_WARN_SKIPFIRST_THROTTLE(
-      logger_, clock_, 1000, "Input brake: %f is out off range. use closest value.", brake);
-    brake = std::min(std::max(brake, min_brake), max_brake);
-  }
-
-  acc = interpolation::lerp(brake_index_, accs_interpolated, brake);
+  // When the desired acceleration is smaller than the brake area, return min acc
+  // When the desired acceleration is greater than the brake area, return min acc
+  brake = CSVLoader::clampValue(brake, brake_index_, "brake: acc");
+  acc = interpolation::lerp(brake_index_, accelerations_interpolated, brake);
 
   return true;
 }