From e68ddf24b0b1ee36c199d32962b72e34a89f4129 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 29 Nov 2023 21:12:10 +0900 Subject: [PATCH] feat(simple_plannign_simulator): add map acc model (#5688) * (simple_planning_simulator):add delay converter model Signed-off-by: Takumi Ito * refactoring rename and format read acc map path from config Signed-off-by: kosuke55 * update docs Signed-off-by: kosuke55 * remove noisy print Signed-off-by: kosuke55 * update map Signed-off-by: kosuke55 * fix pre-commit Signed-off-by: kosuke55 * update acc map Signed-off-by: kosuke55 * fix pre-commit and typo Signed-off-by: kosuke55 typo typo * Update simulator/simple_planning_simulator/README.md Co-authored-by: Takamasa Horibe * Update simulator/simple_planning_simulator/README.md Co-authored-by: Takamasa Horibe * Update simulator/simple_planning_simulator/README.md Co-authored-by: Takamasa Horibe * Update simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp Co-authored-by: Takamasa Horibe * update error message Signed-off-by: kosuke55 * simplify map exmaple Signed-off-by: kosuke55 * use double Signed-off-by: kosuke55 * style(pre-commit): autofix * Update simulator/simple_planning_simulator/README.md Co-authored-by: Takamasa Horibe * add csv loader im sim pacakges Signed-off-by: kosuke55 * revert raw vehicle cmd converter Signed-off-by: kosuke55 * Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp Co-authored-by: Takamasa Horibe * Update simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp Co-authored-by: Takamasa Horibe * Update simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp Co-authored-by: Takamasa Horibe --------- Signed-off-by: Takumi Ito Signed-off-by: kosuke55 Co-authored-by: Takumi Ito Co-authored-by: Takamasa Horibe Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../simple_planning_simulator/CMakeLists.txt | 2 + simulator/simple_planning_simulator/README.md | 61 +- .../simple_planning_simulator_core.hpp | 3 +- .../utils/csv_loader.hpp | 44 + .../vehicle_model/sim_model.hpp | 1 + .../sim_model_delay_steer_map_acc_geared.hpp | 211 ++ .../simple_planning_simulator.launch.py | 14 +- .../media/acceleration_map.svg | 2448 +++++++++++++++++ .../simple_planning_simulator/package.xml | 2 + .../param/acceleration_map.csv | 4 + ...mple_planning_simulator_default.param.yaml | 1 + .../simple_planning_simulator_core.cpp | 23 +- .../utils/csv_loader.cpp | 150 + .../sim_model_delay_steer_map_acc_geared.cpp | 174 ++ 14 files changed, 3118 insertions(+), 20 deletions(-) create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp create mode 100644 simulator/simple_planning_simulator/media/acceleration_map.svg create mode 100644 simulator/simple_planning_simulator/param/acceleration_map.csv create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 6034697304059..87d0f7e5fef0b 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -16,6 +16,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp + src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp + src/simple_planning_simulator/utils/csv_loader.cpp ) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 1491f8ffea36e..b441f9f903d0d 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -62,26 +62,57 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_VEL` - `DELAY_STEER_ACC` - `DELAY_STEER_ACC_GEARED` +- `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). -| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | Default value | unit | -| :------------------------- | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] | -| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] | -| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] | -| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | 1.0 | [-] | -| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | 1.0 | [-] | +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_V | D_ST_A | D_ST_A_G | D_ST_M_ACC_G | Default value | unit | +| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :------------ | :------ | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | 0.24 | [s] | +| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | 0.25 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | 0.27 | [s] | +| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | x | 0.0 | [rad] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | 0.5 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | 5.0 | [rad/s] | +| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | 1.0 | [-] | +| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | 1.0 | [-] | +| acceleration_map_path | string | path to csv file for acceleration map which converts velocity and ideal acceleration to actual acceleration | x | x | x | x | x | x | o | - | [-] | + +The `acceleration_map` is used only for `DELAY_STEER_MAP_ACC_GEARED` and it shows the acceleration command on the vertical axis and the current velocity on the horizontal axis, with each cell representing the converted acceleration command that is actually used in the simulator's motion calculation. Values in between are linearly interpolated. + +Example of `acceleration_map.csv` + +```csv +default, 0.00, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89, 15.28, 16.67 +-4.0, -4.40, -4.36, -4.38, -4.12, -4.20, -3.94, -3.98, -3.80, -3.77, -3.76, -3.59, -3.50, -3.40 +-3.5, -4.00, -3.91, -3.85, -3.64, -3.68, -3.55, -3.42, -3.24, -3.25, -3.00, -3.04, -2.93, -2.80 +-3.0, -3.40, -3.37, -3.33, -3.00, -3.00, -2.90, -2.88, -2.65, -2.43, -2.44, -2.43, -2.39, -2.30 +-2.5, -2.80, -2.72, -2.72, -2.62, -2.41, -2.43, -2.26, -2.18, -2.11, -2.03, -1.96, -1.91, -1.85 +-2.0, -2.30, -2.24, -2.12, -2.02, -1.92, -1.81, -1.67, -1.58, -1.51, -1.49, -1.40, -1.35, -1.30 +-1.5, -1.70, -1.61, -1.47, -1.46, -1.40, -1.37, -1.29, -1.24, -1.10, -0.99, -0.83, -0.80, -0.78 +-1.0, -1.30, -1.28, -1.10, -1.09, -1.04, -1.02, -0.98, -0.89, -0.82, -0.61, -0.52, -0.54, -0.56 +-0.8, -0.96, -0.90, -0.82, -0.74, -0.70, -0.65, -0.63, -0.59, -0.55, -0.44, -0.39, -0.39, -0.35 +-0.6, -0.77, -0.71, -0.67, -0.65, -0.58, -0.52, -0.51, -0.50, -0.40, -0.33, -0.30, -0.31, -0.30 +-0.4, -0.45, -0.40, -0.45, -0.44, -0.38, -0.35, -0.31, -0.30, -0.26, -0.30, -0.29, -0.31, -0.25 +-0.2, -0.24, -0.24, -0.25, -0.22, -0.23, -0.25, -0.27, -0.29, -0.24, -0.22, -0.17, -0.18, -0.12 + 0.0, 0.00, 0.00, -0.05, -0.05, -0.05, -0.05, -0.08, -0.08, -0.08, -0.08, -0.10, -0.10, -0.10 + 0.2, 0.16, 0.12, 0.02, 0.02, 0.00, 0.00, -0.05, -0.05, -0.05, -0.05, -0.08, -0.08, -0.08 + 0.4, 0.38, 0.30, 0.22, 0.25, 0.24, 0.23, 0.20, 0.16, 0.16, 0.14, 0.10, 0.05, 0.05 + 0.6, 0.52, 0.52, 0.51, 0.49, 0.43, 0.40, 0.35, 0.33, 0.33, 0.33, 0.32, 0.34, 0.34 + 0.8, 0.82, 0.81, 0.78, 0.68, 0.63, 0.56, 0.53, 0.48, 0.43, 0.41, 0.37, 0.38, 0.40 + 1.0, 1.00, 1.08, 1.01, 0.88, 0.76, 0.69, 0.66, 0.58, 0.54, 0.49, 0.45, 0.40, 0.40 + 1.5, 1.52, 1.50, 1.38, 1.26, 1.14, 1.03, 0.91, 0.82, 0.67, 0.61, 0.51, 0.41, 0.41 + 2.0, 1.80, 1.80, 1.64, 1.43, 1.25, 1.11, 0.96, 0.81, 0.70, 0.59, 0.51, 0.42, 0.42 +``` + +![acceleration_map](./media/acceleration_map.svg) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index b0df04285ac9f..38f689c124439 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -209,7 +209,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node DELAY_STEER_ACC = 2, DELAY_STEER_ACC_GEARED = 3, IDEAL_STEER_VEL = 4, - DELAY_STEER_VEL = 5 + DELAY_STEER_VEL = 5, + DELAY_STEER_MAP_ACC_GEARED = 6 } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp new file mode 100644 index 0000000000000..fc71837541b83 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 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 SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ + +#include +#include +#include +#include +#include + +using Table = std::vector>; +using Map = std::vector>; +class CSVLoader +{ +public: + 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 bool validateMap(const Map & map, const bool is_col_decent); + static Map getMap(const Table & table); + static std::vector getRowIndex(const Table & table); + static std::vector getColumnIndex(const Table & table); + static double clampValue( + const double val, const std::vector & ranges, const std::string & name); + +private: + std::string csv_path_; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index aef83f10ac417..9fa4516c1a522 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -17,6 +17,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp new file mode 100644 index 0000000000000..2cc33f9b5f82b --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -0,0 +1,211 @@ +// Copyright 2023 The 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 SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" +#include "interpolation/linear_interpolation.hpp" +#include "simple_planning_simulator/utils/csv_loader.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include + +#include +#include +#include +#include +#include + +class AccelerationMap +{ +public: + bool readAccelerationMapFromCSV(const std::string & csv_path) + { + CSVLoader csv(csv_path); + std::vector> table; + if (!csv.readCSV(table)) { + std::cerr << "[SimModelDelaySteerMapAccGeared]: failed to read acceleration map from " + << csv_path << std::endl; + return false; + } + + vehicle_name_ = table[0][0]; + vel_index_ = CSVLoader::getRowIndex(table); + acc_index_ = CSVLoader::getColumnIndex(table); + acceleration_map_ = CSVLoader::getMap(table); + + std::cout << "[SimModelDelaySteerMapAccGeared]: success to read acceleration map from " + << csv_path << std::endl; + return true; + } + + double getAcceleration(const double acc_des, const double vel) const + { + std::vector interpolated_acc_vec; + const double clamped_vel = CSVLoader::clampValue(vel, vel_index_, "acc: vel"); + + // (throttle, vel, acc) map => (throttle, acc) map by fixing vel + for (const auto & acc_vec : acceleration_map_) { + interpolated_acc_vec.push_back(interpolation::lerp(vel_index_, acc_vec, clamped_vel)); + } + // calculate 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 + const double clamped_acc = CSVLoader::clampValue(acc_des, acc_index_, "acceleration: acc"); + const double acc = interpolation::lerp(acc_index_, interpolated_acc_vec, clamped_acc); + + return acc; + } + std::vector> acceleration_map_; + +private: + std::string vehicle_name_; + std::vector vel_index_; + std::vector acc_index_; +}; + +class SimModelDelaySteerMapAccGeared : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] steer_lim steering limit [rad] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] + * @param [in] wheelbase vehicle wheelbase length [m] + * @param [in] dt delta time information to set input buffer for delay + * @param [in] acc_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] path path to csv file for acceleration conversion map + */ + SimModelDelaySteerMapAccGeared( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double acc_delay, double acc_time_constant, double steer_delay, + double steer_time_constant, std::string path); + + /** + * @brief default destructor + */ + ~SimModelDelaySteerMapAccGeared() = default; + + AccelerationMap acc_map_; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U { + ACCX_DES = 0, + STEER_DES, + DRIVE_SHIFT, + }; + + const double vx_lim_; //!< @brief velocity limit [m/s] + const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const double steer_lim_; //!< @brief steering limit [rad] + const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_; //!< @brief vehicle wheelbase length [m] + + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + const double acc_time_constant_; //!< @brief time constant for accel dynamics + const double steer_delay_; //!< @brief time delay for steering command [s] + const double steer_time_constant_; //!< @brief time constant for steering dynamics + const std::string path_; //!< @brief conversion map path + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + double getVy() override; + + /** + * @brief get vehicle longitudinal acceleration + */ + double getAx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time delay steering model + * @param [in] state current model state + * @param [in] input input vector to model + */ + Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; + + /** + * @brief update state considering current gear + * @param [in] state current state + * @param [in] prev_state previous state + * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] dt delta time to update state + */ + void updateStateWithGear( + Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, + const double dt); +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 2f9c2cfe333f4..0070646a20d49 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -18,6 +18,7 @@ from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +import launch_ros.parameter_descriptions from launch_ros.substitutions import FindPackageShare import yaml @@ -35,8 +36,9 @@ def launch_setup(context, *args, **kwargs): vehicle_characteristics_param = yaml.safe_load(f)["/**"]["ros__parameters"] simulator_model_param_path = LaunchConfiguration("simulator_model_param_file").perform(context) - with open(simulator_model_param_path, "r") as f: - simulator_model_param = yaml.safe_load(f)["/**"]["ros__parameters"] + simulator_model_param = launch_ros.parameter_descriptions.ParameterFile( + param_file=simulator_model_param_path, allow_substs=True + ) simple_planning_simulator_node = Node( package="simple_planning_simulator", @@ -129,4 +131,12 @@ def add_launch_arg(name: str, default_value=None, description=None): "path to config file for simulator_model", ) + add_launch_arg( + "acceleration_param_file", + [ + FindPackageShare("simple_planning_simulator"), + "/param/acceleration_map.csv", + ], + ) + return launch.LaunchDescription(launch_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/simulator/simple_planning_simulator/media/acceleration_map.svg b/simulator/simple_planning_simulator/media/acceleration_map.svg new file mode 100644 index 0000000000000..55b0c685726a4 --- /dev/null +++ b/simulator/simple_planning_simulator/media/acceleration_map.svg @@ -0,0 +1,2448 @@ + + + + + + + + 2023-11-28T14:30:04.088460 + image/svg+xml + + + Matplotlib v3.5.1, https://matplotlib.org/ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 5652be138b18f..a2038486029dc 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -11,6 +11,8 @@ ament_cmake_auto autoware_cmake + autoware_cmake + autoware_auto_control_msgs autoware_auto_mapping_msgs autoware_auto_planning_msgs diff --git a/simulator/simple_planning_simulator/param/acceleration_map.csv b/simulator/simple_planning_simulator/param/acceleration_map.csv new file mode 100644 index 0000000000000..e365ead979912 --- /dev/null +++ b/simulator/simple_planning_simulator/param/acceleration_map.csv @@ -0,0 +1,4 @@ +default,0.00,20.0 +-4,-4,-4 +0,0,0 +4,4,4 diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml index dbd56bf9e9bff..bbc92a4ffeff8 100644 --- a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -17,5 +17,6 @@ steer_dead_band: 0.0 x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate + # acceleration_map_path: $(var vehicle_model_pkg)/config/acceleration_map.csv # only `DELAY_STEER_MAP_ACC_GEARED` needs this parameter # Note: vehicle characteristics parameters (e.g. wheelbase) are defined in a separate file. diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index f0b126cbc61be..9a0ea6a6c3659 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -261,6 +262,22 @@ void SimplePlanningSimulator::initialize_vehicle_model() vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, steer_dead_band, debug_acc_scaling_factor, debug_steer_scaling_factor); + } else if (vehicle_model_type_str == "DELAY_STEER_MAP_ACC_GEARED") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_MAP_ACC_GEARED; + const std::string acceleration_map_path = + declare_parameter("acceleration_map_path"); + if (!std::filesystem::exists(acceleration_map_path)) { + throw std::runtime_error( + "`acceleration_map_path` parameter is necessary for `DELAY_STEER_MAP_ACC_GEARED` simulator " + "model, but " + + acceleration_map_path + + " does not exist. Please confirm that the parameter is set correctly in " + "{simulator_model.param.yaml}."); + } + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant, + acceleration_map_path); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } @@ -464,7 +481,8 @@ void SimplePlanningSimulator::set_input( input << acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) { input << acc, steer; } vehicle_model_ptr_->setInput(input); @@ -560,7 +578,8 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & state << x, y, yaw, vx, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_MAP_ACC_GEARED) { state << x, y, yaw, vx, steer, accx; } vehicle_model_ptr_->setState(state); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp new file mode 100644 index 0000000000000..c9eb7a5a237fb --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp @@ -0,0 +1,150 @@ +// Copyright 2023 Tier IV, Inc. All rights reserved. +// +// 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. + +#include "simple_planning_simulator/utils/csv_loader.hpp" + +#include +#include +#include +#include + +CSVLoader::CSVLoader(const std::string & csv_path) +{ + csv_path_ = csv_path; +} + +bool CSVLoader::readCSV(Table & result, const char delim) +{ + std::ifstream ifs(csv_path_); + if (!ifs.is_open()) { + std::cerr << "Cannot open " << csv_path_.c_str() << std::endl; + return false; + } + + std::string buf; + while (std::getline(ifs, buf)) { + std::vector tokens; + + std::istringstream iss(buf); + std::string token; + while (std::getline(iss, token, delim)) { + tokens.push_back(token); + } + + if (tokens.size() != 0) { + result.push_back(tokens); + } + } + if (!validateData(result, csv_path_)) { + return false; + } + return true; +} + +bool CSVLoader::validateMap(const Map & map, const bool is_col_decent) +{ + std::pair 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 = 0; 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; + } + } + } + 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.empty()) { + std::cerr << "The table is empty." << std::endl; + return false; + } + 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; + return false; + } + } + return true; +} + +Map CSVLoader::getMap(const Table & table) +{ + Map map = {}; + for (size_t i = 1; i < table.size(); i++) { + std::vector accelerations; + for (size_t j = 1; j < table[i].size(); j++) { + accelerations.push_back(std::stod(table[i][j])); + } + map.push_back(accelerations); + } + return map; +} + +std::vector CSVLoader::getRowIndex(const Table & table) +{ + std::vector index = {}; + for (size_t i = 1; i < table[0].size(); i++) { + index.push_back(std::stod(table[0][i])); + } + return index; +} + +std::vector CSVLoader::getColumnIndex(const Table & table) +{ + std::vector index = {}; + for (size_t i = 1; i < table.size(); i++) { + index.push_back(std::stod(table[i][0])); + } + return index; +} + +double CSVLoader::clampValue( + const double val, const std::vector & ranges, const std::string & name) +{ + 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 of range. use closest value. Please update the conversion map" + << std::endl; + return std::min(std::max(val, min_value), max_value); + } + return val; +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp new file mode 100644 index 0000000000000..b04a10667adcf --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -0,0 +1,174 @@ +// Copyright 2023 The 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. + +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" + +#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" + +#include + +SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double acc_delay, double acc_time_constant, double steer_delay, + double steer_time_constant, std::string path) +: SimModelInterface(6 /* dim x */, 2 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + steer_lim_(steer_lim), + steer_rate_lim_(steer_rate_lim), + wheelbase_(wheelbase), + acc_delay_(acc_delay), + acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + path_(path) +{ + initializeInputQueue(dt); + acc_map_.readAccelerationMapFromCSV(path_); +} + +double SimModelDelaySteerMapAccGeared::getX() +{ + return state_(IDX::X); +} +double SimModelDelaySteerMapAccGeared::getY() +{ + return state_(IDX::Y); +} +double SimModelDelaySteerMapAccGeared::getYaw() +{ + return state_(IDX::YAW); +} +double SimModelDelaySteerMapAccGeared::getVx() +{ + return state_(IDX::VX); +} +double SimModelDelaySteerMapAccGeared::getVy() +{ + return 0.0; +} +double SimModelDelaySteerMapAccGeared::getAx() +{ + return state_(IDX::ACCX); +} +double SimModelDelaySteerMapAccGeared::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +double SimModelDelaySteerMapAccGeared::getSteer() +{ + return state_(IDX::STEER); +} +void SimModelDelaySteerMapAccGeared::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); + delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); + acc_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); + steer_input_queue_.pop_front(); + + const auto prev_state = state_; + updateRungeKutta(dt, delayed_input); + + // take velocity limit explicitly + state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); + + // consider gear + // update position and velocity first, and then acceleration is calculated naturally + updateStateWithGear(state_, prev_state, gear_, dt); +} + +void SimModelDelaySteerMapAccGeared::initializeInputQueue(const double & dt) +{ + size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); + acc_input_queue_.resize(acc_input_queue_size); + std::fill(acc_input_queue_.begin(), acc_input_queue_.end(), 0.0); + + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + steer_input_queue_.resize(steer_input_queue_size); + std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); +} + +Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vel = std::clamp(state(IDX::VX), -vx_lim_, vx_lim_); + const double acc = std::clamp(state(IDX::ACCX), -vx_rate_lim_, vx_rate_lim_); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + const double acc_des = std::clamp(input(IDX_U::ACCX_DES), -vx_rate_lim_, vx_rate_lim_); + const double steer_des = std::clamp(input(IDX_U::STEER_DES), -steer_lim_, steer_lim_); + double steer_rate = -(steer - steer_des) / steer_time_constant_; + steer_rate = std::clamp(steer_rate, -steer_rate_lim_, steer_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = acc; + d_state(IDX::STEER) = steer_rate; + const double converted_acc = + acc_map_.acceleration_map_.empty() ? acc_des : acc_map_.getAcceleration(acc_des, vel); + + d_state(IDX::ACCX) = -(acc - converted_acc) / acc_time_constant_; + + return d_state; +} + +void SimModelDelaySteerMapAccGeared::updateStateWithGear( + Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) +{ + using autoware_auto_vehicle_msgs::msg::GearCommand; + if ( + gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || + gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || + gear == GearCommand::DRIVE_7 || gear == GearCommand::DRIVE_8 || gear == GearCommand::DRIVE_9 || + gear == GearCommand::DRIVE_10 || gear == GearCommand::DRIVE_11 || + gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || + gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || + gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { + if (state(IDX::VX) < 0.0) { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + } + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { + if (state(IDX::VX) > 0.0) { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + } + } else if (gear == GearCommand::PARK) { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + } else { + state(IDX::VX) = 0.0; + state(IDX::X) = prev_state(IDX::X); + state(IDX::Y) = prev_state(IDX::Y); + state(IDX::YAW) = prev_state(IDX::YAW); + state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); + } +}