refactor(raw_vehicle_cmd_converter): replace interpolate implementati…

…on with common library (#418)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

vehicle/raw_vehicle_cmd_converter/CMakeLists.txt

@@ -19,7 +19,6 @@ ament_auto_add_library(actuation_map_converter SHARED
   src/accel_map.cpp
   src/brake_map.cpp
   src/csv_loader.cpp
-  src/interpolate.cpp
   src/pid.cpp
   src/steer_converter.cpp
 )

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp

@@ -16,7 +16,6 @@
 #define RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_
 
 #include "raw_vehicle_cmd_converter/csv_loader.hpp"
-#include "raw_vehicle_cmd_converter/interpolate.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp

@@ -16,7 +16,6 @@
 #define RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_
 
 #include "raw_vehicle_cmd_converter/csv_loader.hpp"
-#include "raw_vehicle_cmd_converter/interpolate.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 

vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/steer_converter.hpp

@@ -16,7 +16,6 @@
 #define RAW_VEHICLE_CMD_CONVERTER__STEER_CONVERTER_HPP_
 
 #include "raw_vehicle_cmd_converter/csv_loader.hpp"
-#include "raw_vehicle_cmd_converter/interpolate.hpp"
 #include "raw_vehicle_cmd_converter/pid.hpp"
 
 #include <rclcpp/rclcpp.hpp>

vehicle/raw_vehicle_cmd_converter/package.xml

@@ -13,6 +13,7 @@
   <depend>autoware_auto_control_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>interpolation</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

vehicle/raw_vehicle_cmd_converter/src/accel_map.cpp

@@ -14,6 +14,8 @@
 
 #include "raw_vehicle_cmd_converter/accel_map.hpp"
 
+#include "interpolation/linear_interpolation.hpp"
+
 #include <algorithm>
 #include <chrono>
 #include <string>
@@ -62,7 +64,6 @@ bool AccelMap::readAccelMapFromCSV(std::string csv_path)
 
 bool AccelMap::getThrottle(double acc, double vel, double & throttle)
 {
-  LinearInterpolate linear_interp;
   std::vector<double> accs_interpolated;
 
   if (vel < vel_index_.front()) {
@@ -83,9 +84,7 @@ bool AccelMap::getThrottle(double acc, double vel, double & throttle)
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
   for (std::vector<double> accs : accel_map_) {
-    double acc_interpolated;
-    linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated);
-    accs_interpolated.push_back(acc_interpolated);
+    accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
   }
 
   // calculate throttle
@@ -97,14 +96,13 @@ bool AccelMap::getThrottle(double acc, double vel, double & throttle)
     throttle = throttle_index_.back();
     return true;
   }
-  linear_interp.interpolate(accs_interpolated, throttle_index_, acc, throttle);
+  throttle = interpolation::lerp(accs_interpolated, throttle_index_, acc);
 
   return true;
 }
 
 bool AccelMap::getAcceleration(double throttle, double vel, double & acc)
 {
-  LinearInterpolate linear_interp;
   std::vector<double> accs_interpolated;
 
   if (vel < vel_index_.front()) {
@@ -125,9 +123,7 @@ bool AccelMap::getAcceleration(double throttle, double vel, double & acc)
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
   for (std::vector<double> accs : accel_map_) {
-    double acc_interpolated;
-    linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated);
-    accs_interpolated.push_back(acc_interpolated);
+    accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
   }
 
   // calculate throttle
@@ -141,7 +137,7 @@ bool AccelMap::getAcceleration(double throttle, double vel, double & acc)
     throttle = std::min(std::max(throttle, min_throttle), max_throttle);
   }
 
-  linear_interp.interpolate(throttle_index_, accs_interpolated, throttle, acc);
+  acc = interpolation::lerp(throttle_index_, accs_interpolated, throttle);
 
   return true;
 }

vehicle/raw_vehicle_cmd_converter/src/brake_map.cpp

@@ -14,6 +14,8 @@
 
 #include "raw_vehicle_cmd_converter/brake_map.hpp"
 
+#include "interpolation/linear_interpolation.hpp"
+
 #include <algorithm>
 #include <string>
 #include <vector>
@@ -63,7 +65,6 @@ bool BrakeMap::readBrakeMapFromCSV(std::string csv_path)
 
 bool BrakeMap::getBrake(double acc, double vel, double & brake)
 {
-  LinearInterpolate linear_interp;
   std::vector<double> accs_interpolated;
 
   if (vel < vel_index_.front()) {
@@ -84,9 +85,7 @@ bool BrakeMap::getBrake(double acc, double vel, double & brake)
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
   for (std::vector<double> accs : brake_map_) {
-    double acc_interpolated;
-    linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated);
-    accs_interpolated.push_back(acc_interpolated);
+    accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
   }
 
   // calculate brake
@@ -106,14 +105,13 @@ bool BrakeMap::getBrake(double acc, double vel, double & brake)
   }
 
   std::reverse(std::begin(accs_interpolated), std::end(accs_interpolated));
-  linear_interp.interpolate(accs_interpolated, brake_index_rev_, acc, brake);
+  brake = interpolation::lerp(accs_interpolated, brake_index_rev_, acc);
 
   return true;
 }
 
 bool BrakeMap::getAcceleration(double brake, double vel, double & acc)
 {
-  LinearInterpolate linear_interp;
   std::vector<double> accs_interpolated;
 
   if (vel < vel_index_.front()) {
@@ -134,9 +132,7 @@ bool BrakeMap::getAcceleration(double brake, double vel, double & acc)
 
   // (throttle, vel, acc) map => (throttle, acc) map by fixing vel
   for (std::vector<double> accs : brake_map_) {
-    double acc_interpolated;
-    linear_interp.interpolate(vel_index_, accs, vel, acc_interpolated);
-    accs_interpolated.push_back(acc_interpolated);
+    accs_interpolated.push_back(interpolation::lerp(vel_index_, accs, vel));
   }
 
   // calculate brake
@@ -150,7 +146,7 @@ bool BrakeMap::getAcceleration(double brake, double vel, double & acc)
     brake = std::min(std::max(brake, min_brake), max_brake);
   }
 
-  linear_interp.interpolate(brake_index_, accs_interpolated, brake, acc);
+  acc = interpolation::lerp(brake_index_, accs_interpolated, brake);
 
   return true;
 }

vehicle/raw_vehicle_cmd_converter/src/steer_converter.cpp

@@ -14,6 +14,8 @@
 
 #include "raw_vehicle_cmd_converter/steer_converter.hpp"
 
+#include "interpolation/linear_interpolation.hpp"
+
 #include <string>
 #include <vector>
 
@@ -134,7 +136,6 @@ void SteerConverter::calcFFMap(double steer_vel, double vehicle_vel, double & ou
 {
   rclcpp::Clock clock{RCL_ROS_TIME};
 
-  LinearInterpolate linear_interp;
   std::vector<double> steer_angle_velocities_interp;
 
   if (vehicle_vel < vel_index_.front()) {
@@ -154,16 +155,14 @@ void SteerConverter::calcFFMap(double steer_vel, double vehicle_vel, double & ou
   }
 
   for (std::vector<double> steer_angle_velocities : steer_map_) {
-    double steer_angle_vel_interp;
-    linear_interp.interpolate(
-      vel_index_, steer_angle_velocities, vehicle_vel, steer_angle_vel_interp);
-    steer_angle_velocities_interp.push_back(steer_angle_vel_interp);
+    steer_angle_velocities_interp.push_back(
+      interpolation::lerp(vel_index_, steer_angle_velocities, vehicle_vel));
   }
   if (steer_vel < steer_angle_velocities_interp.front()) {
     steer_vel = steer_angle_velocities_interp.front();
   } else if (steer_angle_velocities_interp.back() < steer_vel) {
     steer_vel = steer_angle_velocities_interp.back();
   }
-  linear_interp.interpolate(steer_angle_velocities_interp, output_index_, steer_vel, output);
+  output = interpolation::lerp(steer_angle_velocities_interp, output_index_, steer_vel);
 }
 }  // namespace raw_vehicle_cmd_converter