Skip to content

Commit e68ddf2

Browse files
kosuke55Takumi ItoTakaHoribepre-commit-ci[bot]
authored
feat(simple_plannign_simulator): add map acc model (#5688)
* (simple_planning_simulator):add delay converter model Signed-off-by: Takumi Ito <takumi.ito@tier4.jp> * refactoring rename and format read acc map path from config Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * update docs Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * remove noisy print Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * update map Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix pre-commit Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * update acc map Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * fix pre-commit and typo Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> typo typo * Update simulator/simple_planning_simulator/README.md Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update simulator/simple_planning_simulator/README.md Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update simulator/simple_planning_simulator/README.md Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * update error message Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * simplify map exmaple Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * use double Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * style(pre-commit): autofix * Update simulator/simple_planning_simulator/README.md Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * add csv loader im sim pacakges Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * revert raw vehicle cmd converter Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> --------- Signed-off-by: Takumi Ito <takumi.ito@tier4.jp> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Co-authored-by: Takumi Ito <takumi.ito@tier4.jp> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 703d91f commit e68ddf2

14 files changed

+3118
-20
lines changed

‎simulator/simple_planning_simulator/CMakeLists.txt

+2
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
1616
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
1717
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp
1818
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp
19+
src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp
20+
src/simple_planning_simulator/utils/csv_loader.cpp
1921
)
2022
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS})
2123

‎simulator/simple_planning_simulator/README.md

+46-15
Original file line numberDiff line numberDiff line change
@@ -62,26 +62,57 @@ The purpose of this simulator is for the integration test of planning and contro
6262
- `DELAY_STEER_VEL`
6363
- `DELAY_STEER_ACC`
6464
- `DELAY_STEER_ACC_GEARED`
65+
- `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.
6566

6667
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.
6768

6869
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).
6970

70-
| 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 |
71-
| :------------------------- | :----- | :--------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :------------ | :------ |
72-
| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | 0.1 | [s] |
73-
| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | 0.24 | [s] |
74-
| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | 0.25 | [s] |
75-
| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | 0.1 | [s] |
76-
| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | 0.27 | [s] |
77-
| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | 0.0 | [rad] |
78-
| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | 0.5 | [s] |
79-
| vel_lim | double | limit of velocity | x | x | x | o | o | o | 50.0 | [m/s] |
80-
| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | 7.0 | [m/ss] |
81-
| steer_lim | double | limit of steering angle | x | x | x | o | o | o | 1.0 | [rad] |
82-
| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | 5.0 | [rad/s] |
83-
| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | 1.0 | [-] |
84-
| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | 1.0 | [-] |
71+
| 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 |
72+
| :------------------------- | :----- | :---------------------------------------------------------------------------------------------------------- | :----- | :----- | :------- | :----- | :----- | :------- | :----------- | :------------ | :------ |
73+
| acc_time_delay | double | dead time for the acceleration input | x | x | x | x | o | o | o | 0.1 | [s] |
74+
| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | o | o | 0.24 | [s] |
75+
| vel_time_delay | double | dead time for the velocity input | x | x | x | o | x | x | x | 0.25 | [s] |
76+
| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | x | o | o | o | 0.1 | [s] |
77+
| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | o | o | 0.27 | [s] |
78+
| steer_dead_band | double | dead band for steering angle | x | x | x | o | o | o | x | 0.0 | [rad] |
79+
| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | x | o | x | x | x | 0.5 | [s] |
80+
| vel_lim | double | limit of velocity | x | x | x | o | o | o | o | 50.0 | [m/s] |
81+
| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | o | o | 7.0 | [m/ss] |
82+
| steer_lim | double | limit of steering angle | x | x | x | o | o | o | o | 1.0 | [rad] |
83+
| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | o | o | 5.0 | [rad/s] |
84+
| debug_acc_scaling_factor | double | scaling factor for accel command | x | x | x | x | o | o | x | 1.0 | [-] |
85+
| debug_steer_scaling_factor | double | scaling factor for steer command | x | x | x | x | o | o | x | 1.0 | [-] |
86+
| 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 | - | [-] |
87+
88+
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.
89+
90+
Example of `acceleration_map.csv`
91+
92+
```csv
93+
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
94+
-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
95+
-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
96+
-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
97+
-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
98+
-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
99+
-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
100+
-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
101+
-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
102+
-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
103+
-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
104+
-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
105+
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
106+
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
107+
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
108+
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
109+
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
110+
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
111+
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
112+
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
113+
```
114+
115+
![acceleration_map](./media/acceleration_map.svg)
85116

86117
<!-- deadzone_delta_steer | double | dead zone for the steering dynamics | x | x | x | o | o | 0.0 | [rad] | | -->
87118

‎simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -209,7 +209,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
209209
DELAY_STEER_ACC = 2,
210210
DELAY_STEER_ACC_GEARED = 3,
211211
IDEAL_STEER_VEL = 4,
212-
DELAY_STEER_VEL = 5
212+
DELAY_STEER_VEL = 5,
213+
DELAY_STEER_MAP_ACC_GEARED = 6
213214
} vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics
214215
std::shared_ptr<SimModelInterface> vehicle_model_ptr_; //!< @brief vehicle model pointer
215216

‎simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
// Copyright 2023 Autoware Foundation
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_
16+
#define SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_
17+
18+
#include <fstream>
19+
#include <iostream>
20+
#include <sstream>
21+
#include <string>
22+
#include <vector>
23+
24+
using Table = std::vector<std::vector<std::string>>;
25+
using Map = std::vector<std::vector<double>>;
26+
class CSVLoader
27+
{
28+
public:
29+
explicit CSVLoader(const std::string & csv_path);
30+
31+
bool readCSV(Table & result, const char delim = ',');
32+
static bool validateData(const Table & table, const std::string & csv_path);
33+
static bool validateMap(const Map & map, const bool is_col_decent);
34+
static Map getMap(const Table & table);
35+
static std::vector<double> getRowIndex(const Table & table);
36+
static std::vector<double> getColumnIndex(const Table & table);
37+
static double clampValue(
38+
const double val, const std::vector<double> & ranges, const std::string & name);
39+
40+
private:
41+
std::string csv_path_;
42+
};
43+
44+
#endif // SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_

‎simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp"
1919
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp"
20+
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp"
2021
#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp"
2122
#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp"
2223
#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp"

0 commit comments

Comments
 (0)