feat: add raw vehicle cmd converter package (autowarefoundation#79)

* release v0.4.0 * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "remove ROS1 packages temporarily" This reverts commit a2dc52036f5950188b5b4caedb2edcd36d3e5860. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * ROS2 Porting: raw_vehicle_cmd_converter (#9) * remove dependency to unused std_msgs package Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * rm COLCON_IGNORE Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * port raw_vehicle_cmd_converter to ROS2 Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * port raw_vehicle_cmd_converter.launch to ROS2 Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * fix typo Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com> * fix typo Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com> * remove include_directories form CMakeLists.txt Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * fix order of includes Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add missing comments Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com> * fix duration unit for RCLCPP_*_THROTTLE (autowarefoundation#75) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Rename h files to hpp (autowarefoundation#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * ROS2 Linting: raw_vehicle_cmd_converter (autowarefoundation#161) * Add linters * Process linter corrections * apply env_var to use_sim_time (autowarefoundation#222) * Ros2 v0.8.0 raw vehicle cmd converter (autowarefoundation#268) * restore raw_vehicle_cmd_converter files for 0.8.0 update Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * fix typos in vehicle modules (autowarefoundation#954) * add get func (autowarefoundation#1088) * Revert "restore raw_vehicle_cmd_converter files for 0.8.0 update" This reverts commit f243f26e20490757a332566aa92196553a682aa6. Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * update target branch for ci (autowarefoundation#309) * update target branch for ci Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * [as]: Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [latlon_muxer]: Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [map_based_prediction]: Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [raw_vehicle_cmd_converter]: Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [remote_cmd_converter]: Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * [turn_signal_decider]: Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> * Unify Apache-2.0 license name (autowarefoundation#1242) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * add plot_accel_brake_map.py (autowarefoundation#1249) (autowarefoundation#1296) * add plot_accel_brake_map.py (autowarefoundation#1249) * add plot_accel_brake_map.py * call plotter from rosrun * add permission Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * delete debug message * Add license Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * apply format * fix code Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: tomoya.kimura <tomoya.kimura@tier4.jp> * Fix lint errors (autowarefoundation#1378) * Fix lint errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix variable names Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * enable to designate directory of maps (autowarefoundation#1301) (autowarefoundation#1480) * enable to designate directory of maps * add error handling * use argparser Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Feature/vehicle interface improvements (autowarefoundation#1361) (autowarefoundation#1688) * Feature/vehicle interface improvements (autowarefoundation#1361) * add vehicle msg * add pacmod interface * add eps controller * use each control commands instead of vehicle command * fixed details * fixed speell check * const * fixed brake status * publish cmd when recieving ctrl cmd * fix shift cmd ptr * remove unused function and set proper license * fix names * fix typo for pacmod * remove unnecessary waiting * use flags, limit, multiarray * remove accel brake dependency * fix retrun value * replace eps to steer * cosmetic change for namespace * fix segfo and retval * Use Enum instead of int * remove unused var * add const * rename to calcFFMap * prev time steer calc * add sample csv * add new line Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Apply lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix build failure for remote cmd converter Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Fix lint Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * replace to vehicle cmd emergency (autowarefoundation#1710) (autowarefoundation#1717) * Fix subscriber topic type Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * Fix rclcpp::Time initialization Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * default debug value false in raw vehicle cmd converter (autowarefoundation#1755) * default debug value false in raw vehicle cmd converter * spell check * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * [raw vehicle cmd converter] apply autoware auto msgs (autowarefoundation#519) * remove ignore * replace to autoware auto control message * use localization odometry * apply steering report * appluy clang format * cosmetic change * fix spell check * update to readme * update launch * minor fix * precommit fix * update README.md Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * using AckermannControlCommand * fix format Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> * move cmd converters to control pkg (autowarefoundation#642) * auto/revert cmd converter (autowarefoundation#680) * Revert "move cmd converters to control pkg (autowarefoundation#642)" This reverts commit 19a10f9419aa25ef7a37e51c5df8f65b75cf8887. * fix topic * Auto/readme cmd converter (autowarefoundation#692) * fix format * add readme external cmd converter * fix lint * fiix sentence Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * fix format Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * fix sentence Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * better expression Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * fix door cmd topic (autowarefoundation#711) * fix door cmd topic * fix topic Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Keisuke Shima <keisuke.shima@tier4.jp> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
badai-nguyen · Dec 3, 2021 · 30b1080 · 30b1080
1 parent 5227548
commit 30b1080
Show file tree

Hide file tree

Showing 23 changed files with 1,655 additions and 0 deletions.
diff --git a/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/raw_vehicle_cmd_converter/CMakeLists.txt
@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(raw_vehicle_cmd_converter)
+
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+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
+)
+
+ament_auto_add_library(raw_vehicle_cmd_converter_node_component SHARED
+  src/node.cpp
+)
+
+target_link_libraries(raw_vehicle_cmd_converter_node_component actuation_map_converter)
+
+rclcpp_components_register_node(raw_vehicle_cmd_converter_node_component
+  PLUGIN "raw_vehicle_cmd_converter::RawVehicleCommandConverterNode"
+  EXECUTABLE raw_vehicle_cmd_converter_node
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  config
+  data
+  launch
+)
+
+install(PROGRAMS scripts/plot_accel_brake_map.py
+  DESTINATION lib/${PROJECT_NAME}
+)
diff --git a/vehicle/raw_vehicle_cmd_converter/README.md b/vehicle/raw_vehicle_cmd_converter/README.md
@@ -0,0 +1,31 @@
+# raw_vehicle_cmd_converter
+
+`raw_vehicle_command_converter` is a node that converts desired acceleration and velocity to mechanical input by using feed forward + feed back control (optional).
+
+## Input topics
+
+| Name                  | Type                                                     | Description                                                                                                        |
+| --------------------- | -------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ |
+| `~/input/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. |
+| `~/input/steering"`   | autoware_auto_vehicle_msgs::SteeringReport               | current status of steering used for steering feed back control                                                     |
+| `~/input/twist`       | navigation_msgs::Odometry                                | twist topic in odometry is used.                                                                                   |
+
+## Output topics
+
+| Name                     | Type                                                | Description                                             |
+| ------------------------ | --------------------------------------------------- | ------------------------------------------------------- |
+| `~/output/actuation_cmd` | autoware_vehicle_msgs::msg::ActuationCommandStamped | actuation command for vehicle to apply mechanical input |
+
+## Parameters
+
+| Parameter                  | Type   | Description                                                                     |
+| -------------------------- | ------ | ------------------------------------------------------------------------------- |
+| `update_rate`              | double | timer's update rate                                                             |
+| `th_max_message_delay_sec` | double | threshold time of input messages' maximum delay                                 |
+| `th_arrived_distance_m`    | double | threshold distance to check if vehicle has arrived at the trajectory's endpoint |
+| `th_stopped_time_sec`      | double | threshold time to check if vehicle is stopped                                   |
+| `th_stopped_velocity_mps`  | double | threshold velocity to check if vehicle is stopped                               |
+
+## Limitation
+
+The current feed back implementation is only applied to steering control.
diff --git a/vehicle/raw_vehicle_cmd_converter/config/converter.param.yaml b/vehicle/raw_vehicle_cmd_converter/config/converter.param.yaml
@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    use_steer_ff: true
+    use_steer_fb: true
+    is_debugging: false
+    steer_pid:
+      kp: 150.0
+      ki: 15.0
+      kd: 0.0
+      max: 8.0
+      min: -8.0
+      max_p: 8.0
+      min_p: -8.0
+      max_i: 8.0
+      min_i: -8.0
+      max_d: 0.0
+      min_d: 0.0
+      invalid_integration_decay: 0.97
diff --git a/vehicle/raw_vehicle_cmd_converter/data/default/accel_map.csv b/vehicle/raw_vehicle_cmd_converter/data/default/accel_map.csv
@@ -0,0 +1,7 @@
+default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89
+0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5
+0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28
+0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03
+0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58
+0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1
+0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61
diff --git a/vehicle/raw_vehicle_cmd_converter/data/default/brake_map.csv b/vehicle/raw_vehicle_cmd_converter/data/default/brake_map.csv
@@ -0,0 +1,10 @@
+default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89
+0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5
+0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51
+0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96
+0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633
+0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106
+0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116
+0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126
+0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136
+0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955
diff --git a/vehicle/raw_vehicle_cmd_converter/data/default/steer_map.csv b/vehicle/raw_vehicle_cmd_converter/data/default/steer_map.csv
@@ -0,0 +1,4 @@
+default,-10,0,10
+-10,-10,-10,-10
+0,0,0,0
+10,10,10,10
diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/accel_map.hpp
@@ -0,0 +1,50 @@
+// Copyright 2017-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_
+#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>
+
+#include <algorithm>
+#include <iostream>
+#include <string>
+#include <vector>
+
+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_; }
+
+private:
+  rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("accel_map")};
+  rclcpp::Clock clock_{RCL_ROS_TIME};
+  std::string vehicle_name_;
+  std::vector<double> vel_index_;
+  std::vector<double> throttle_index_;
+  std::vector<std::vector<double>> accel_map_;
+};
+}  // namespace raw_vehicle_cmd_converter
+
+#endif  // RAW_VEHICLE_CMD_CONVERTER__ACCEL_MAP_HPP_
diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/brake_map.hpp
@@ -0,0 +1,51 @@
+// Copyright 2017-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_
+#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>
+
+#include <algorithm>
+#include <iostream>
+#include <string>
+#include <vector>
+
+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_; }
+
+private:
+  rclcpp::Logger logger_{rclcpp::get_logger("raw_vehicle_cmd_converter").get_child("accel_map")};
+  rclcpp::Clock clock_{RCL_ROS_TIME};
+  std::string vehicle_name_;
+  std::vector<double> vel_index_;
+  std::vector<double> brake_index_;
+  std::vector<double> brake_index_rev_;
+  std::vector<std::vector<double>> brake_map_;
+};
+}  // namespace raw_vehicle_cmd_converter
+
+#endif  // RAW_VEHICLE_CMD_CONVERTER__BRAKE_MAP_HPP_
diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/csv_loader.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/csv_loader.hpp
@@ -0,0 +1,38 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_
+#define RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_
+
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+namespace raw_vehicle_cmd_converter
+{
+class CSVLoader
+{
+public:
+  explicit CSVLoader(std::string csv_path);
+
+  bool readCSV(std::vector<std::vector<std::string>> & result, const char delim = ',');
+
+private:
+  std::string csv_path_;
+};
+}  // namespace raw_vehicle_cmd_converter
+
+#endif  // RAW_VEHICLE_CMD_CONVERTER__CSV_LOADER_HPP_
diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/interpolate.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/interpolate.hpp
@@ -0,0 +1,33 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_
+#define RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_
+
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+namespace raw_vehicle_cmd_converter
+{
+class LinearInterpolate
+{
+public:
+  static bool interpolate(
+    const std::vector<double> & base_index, const std::vector<double> & base_value,
+    const double & return_index, double & return_value);
+};
+}  // namespace raw_vehicle_cmd_converter
+
+#endif  // RAW_VEHICLE_CMD_CONVERTER__INTERPOLATE_HPP_