diff --git a/vehicle/accel_brake_map_calibrator/CMakeLists.txt b/vehicle/accel_brake_map_calibrator/CMakeLists.txt deleted file mode 100644 index f8aed0ab..00000000 --- a/vehicle/accel_brake_map_calibrator/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(accel_brake_map_calibrator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_executable(accel_brake_map_calibrator - src/accel_brake_map_calibrator_node.cpp - src/main.cpp -) -ament_target_dependencies(accel_brake_map_calibrator) - -install( - PROGRAMS - scripts/__init__.py - scripts/actuation_cmd_publisher.py - scripts/accel_tester.py - scripts/brake_tester.py - scripts/config.py - scripts/delay_estimator.py - scripts/plotter.py - scripts/view_statistics.py - scripts/calc_utils.py - scripts/csv_reader.py - scripts/log_analyzer.py - scripts/view_plot.py - scripts/new_accel_brake_map_server.py - test/plot_csv_client_node.py - test/sim_actuation_status_publisher.py - DESTINATION lib/${PROJECT_NAME} -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config - rviz) diff --git a/vehicle/accel_brake_map_calibrator/README.md b/vehicle/accel_brake_map_calibrator/README.md deleted file mode 100644 index c8813280..00000000 --- a/vehicle/accel_brake_map_calibrator/README.md +++ /dev/null @@ -1,260 +0,0 @@ -# accel_brake_map_calibrator - -The role of this node is to automatically calibrate `accel_map.csv` / `brake_map.csv` used in the `raw_vehicle_cmd_converter` node. - -The base map, which is lexus's one by default, is updated iteratively with the loaded driving data. - -## How to calibrate - -### Launch Calibrator - -After launching Autoware, run the `accel_brake_map_calibrator` by the following command and then perform autonomous driving. Note: You can collect data with manual driving if it is possible to use the same vehicle interface as during autonomous driving (e.g. using a joystick). - -```sh -ros2 launch accel_brake_map_calibrator accel_brake_map_calibrator.launch.xml rviz:=true -``` - -Or if you want to use rosbag files, run the following commands. - -```sh -ros2 launch accel_brake_map_calibrator accel_brake_map_calibrator.launch.xml rviz:=true use_sim_time:=true -ros2 bag play --clock -``` - -During the calibration with setting the parameter `progress_file_output` to true, the log file is output in [directory of *accel_brake_map_calibrator*]/config/ . You can also see accel and brake maps in [directory of *accel_brake_map_calibrator*]/config/accel_map.csv and [directory of *accel_brake_map_calibrator*]/config/brake_map.csv after calibration. - -### Calibration plugin - -The `rviz:=true` option displays the RViz with a calibration plugin as below. - -

- -

- -The current status (velocity and pedal) is shown in the plugin. The color on the current cell varies green/red depending on the current data is valid/invalid. The data that doesn't satisfy the following conditions are considered invalid and will not be used for estimation since aggressive data (e.g. when the pedal is moving fast) causes bad calibration accuracy. - -- The velocity and pedal conditions are within certain ranges from the index values. -- The steer value, pedal speed, pitch value, etc. are less than corresponding thresholds. -- The velocity is higher than a threshold. - -The detailed parameters are described in the parameter section. - -Note: You don't need to worry about whether the current state is red or green during calibration. Just keep getting data until all the cells turn red. - -The value of each cell in the map is gray at first, and it changes from blue to red as the number of valid data in the cell accumulates. It is preferable to continue the calibration until each cell of the map becomes close to red. In particular, the performance near the stop depends strongly on the velocity of 0 ~ 6m/s range and the pedal value of +0.2 ~ -0.4, range so it is desirable to focus on those areas. - -### Diagnostics - -The `accel brake map_calibrator` publishes diagnostics message depending on the calibration status. -Diagnostic type `WARN` indicates that the current accel/brake map is estimated to be inaccurate. In this situation, it is strongly recommended to perform a re-calibration of the accel/brake map. - -| Status | Diagnostics Type | Diagnostics message | Description | -| ----------------------- | ---------------- | ------------------------------------------ | --------------------------------------------------- | -| No calibration required | `OK` | "OK" | | -| Calibration Required | `WARN` | "Accel/brake map Calibration is required." | The accuracy of current accel/brake map may be low. | - -This diagnostics status can be also checked on the following ROS topic. - -```sh -ros2 topic echo /accel_brake_map_calibrator/output/update_suggest -``` - -When the diagnostics type is `WARN`, `True` is published on this topic and the update of the accel/brake map is suggested. - -### Evaluation of the accel / brake map accuracy - -The accuracy of map is evaluated by the **Root Mean Squared Error (RMSE)** between the observed acceleration and predicted acceleration. - -**TERMS:** - -- `Observed acceleration`: the current vehicle acceleration which is calculated as a derivative value of the wheel speed. - -- `Predicted acceleration`: the output of the original accel/brake map, which the Autoware is expecting. The value is calculated using the current pedal and velocity. - -You can check additional error information with the following topics. - -- `/accel_brake_map_calibrator/output/current_map_error` : The error of the original map set in the `csv_path_accel/brake_map` path. The original map is not accurate if this value is large. -- `/accel_brake_map_calibrator/output/updated_map_error` : The error of the map calibrated in this node. The calibration quality is low if this value is large. -- `/accel_brake_map_calibrator/output/map_error_ratio` : The error ratio between the original map and updated map (ratio = updated / current). If this value is less than 1, it is desirable to update the map. - -### How to visualize calibration data - -The process of calibration can be visualized as below. Since these scripts need the log output of the calibration, the `pedal_accel_graph_output` parameter must be set to true while the calibration is running for the visualization. - -#### Visualize plot of relation between acceleration and pedal - -The following command shows the plot of used data in the calibration. In each plot of velocity ranges, you can see the distribution of the relationship between pedal and acceleration, and raw data points with colors according to their pitch angles. - -```sh -ros2 run accel_brake_map_calibrator view_plot.py -``` - -![sample pic](media/log_sample.png) - -#### Visualize statistics about acceleration/velocity/pedal data - -The following command shows the statistics of the calibration: - -- mean value -- standard deviation -- number of data - -of all data in each map cell. - -```sh -ros2 run accel_brake_map_calibrator view_statistics.py -``` - -![sample pic2](media/statistics_sample.png) - -### How to save the calibrated accel / brake map anytime you want - -You can save accel and brake map anytime with the following command. - -```sh -ros2 service call /accel_brake_map_calibrator/update_map_dir tier4_vehicle_msgs/srv/UpdateAccelBrakeMap "path: ''" -``` - -You can also save accel and brake map in the default directory where Autoware reads accel_map.csv/brake_map.csv using the RViz plugin (AccelBrakeMapCalibratorButtonPanel) as following. - -1. Click _Panels_ tab, and select AccelBrakeMapCalibratorButtonPanel. - - ![add_panel](./media/add_panel.png) - -2. Select the panel, and the button will appear at the bottom of RViz. - - ![calibrator_button_panel](./media/calibrator_button_panel.png) - -3. Press the button, and the accel / brake map will be saved. - (The button cannot be pressed in certain situations, such as when the calibrator node is not running.) - - ![push_calibration_button](./media/push_calibration_button.png) - -## Parameters - -## System Parameters - -| Name | Type | Description | Default value | -| :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------------------------------------- | -| update_method | string | you can select map calibration method. "update_offset_each_cell" calculates offsets for each grid cells on the map. "update_offset_total" calculates the total offset of the map. | "update_offset_each_cell" | -| get_pitch_method | string | "tf": get pitch from tf, "none": unable to perform pitch validation and pitch compensation | "tf" | -| pedal_accel_graph_output | bool | if true, it will output a log of the pedal accel graph. | true | -| progress_file_output | bool | if true, it will output a log and csv file of the update process. | false | -| default_map_dir | str | directory of default map | [directory of *raw_vehicle_cmd_converter*]/data/default/ | -| calibrated_map_dir | str | directory of calibrated map | [directory of *accel_brake_map_calibrator*]/config/ | -| update_hz | double | hz for update | 10.0 | - -## Algorithm Parameters - -| Name | Type | Description | Default value | -| :----------------------- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| initial_covariance | double | Covariance of initial acceleration map (larger covariance makes the update speed faster) | 0.05 | -| velocity_min_threshold | double | Speeds smaller than this are not used for updating. | 0.1 | -| velocity_diff_threshold | double | When the velocity data is more than this threshold away from the grid reference speed (center value), the associated data is not used for updating. | 0.556 | -| max_steer_threshold | double | If the steer angle is greater than this value, the associated data is not used for updating. | 0.2 | -| max_pitch_threshold | double | If the pitch angle is greater than this value, the associated data is not used for updating. | 0.02 | -| max_jerk_threshold | double | If the ego jerk calculated from ego acceleration is greater than this value, the associated data is not used for updating. | 0.7 | -| pedal_velocity_thresh | double | If the pedal moving speed is greater than this value, the associated data is not used for updating. | 0.15 | -| pedal_diff_threshold | double | If the current pedal value is more then this threshold away from the previous value, the associated data is not used for updating. | 0.03 | -| max_accel | double | Maximum value of acceleration calculated from velocity source. | 5.0 | -| min_accel | double | Minimum value of acceleration calculated from velocity source. | -5.0 | -| pedal_to_accel_delay | double | The delay time between actuation_cmd to acceleration, considered in the update logic. | 0.3 | -| update_suggest_thresh | double | threshold of RMSE ratio that update suggest flag becomes true. ( RMSE ratio: [RMSE of new map] / [RMSE of original map] ) | 0.7 | -| max_data_count | int | For visualization. When the data num of each grid gets this value, the grid color gets red. | 100 | -| accel_brake_value_source | string | Whether to use actuation_status or actuation_command as accel/brake sources. value | status | - -## Test utility scripts - -### Constant accel/brake command test - -These scripts are useful to test for accel brake map calibration. These generate an `ActuationCmd` with a constant accel/brake value given interactively by a user through CLI. - -- accel_tester.py -- brake_tester.py -- actuation_cmd_publisher.py - -The `accel/brake_tester.py` receives a target accel/brake command from CLI. It sends a target value to `actuation_cmd_publisher.py` which generates the `ActuationCmd`. You can run these scripts by the following commands in the different terminals, and it will be as in the screenshot below. - -```bash -ros2 run accel_brake_map_calibrator accel_tester.py -ros2 run accel_brake_map_calibrator brake_tester.py -ros2 run accel_brake_map_calibrator actuation_cmd_publisher.py -``` - -![actuation_cmd_publisher_util](./media/actuation_cmd_publisher_util.png) - -## Calibration Method - -Two algorithms are selectable for the acceleration map update, [update_offset_four_cell_around](#update_offset_four_cell_around-1) and [update_offset_each_cell](#update_offset_each_cell). Please see the link for details. - -### Data Preprocessing - -Before calibration, missing or unusable data (e.g., too large handle angles) must first be eliminated. The following parameters are used to determine which data to remove. - -#### Parameters - -| Name | Description | Default Value | -| ---------------------- | ---------------------------- | ------------- | -| velocity_min_threshold | Exclude minimal velocity | 0.1 | -| max_steer_threshold | Exclude large steering angle | 0.2 | -| max_pitch_threshold | Exclude large pitch angle | 0.02 | -| max_jerk_threshold | Exclude large jerk | 0.7 | -| pedal_velocity_thresh | Exclude large pedaling speed | 0.15 | - -### update_offset_each_cell - -Update by Recursive Least Squares(RLS) method using data close enough to each grid. - -**Advantage** : Only data close enough to each grid is used for calibration, allowing accurate updates at each point. - -**Disadvantage** : Calibration is time-consuming due to a large amount of data to be excluded. - -#### Parameters - -Data selection is determined by the following thresholds. -| Name | Default Value | -| ----------------------- | ------------- | -| velocity_diff_threshold | 0.556 | -| pedal_diff_threshold | 0.03 | - -#### Update formula - -$$ -\begin{align} - \theta[n]=& - \theta[n-1]+\frac{p[n-1]x^{(n)}}{\lambda+p[n-1]{(x^{(n)})}^2}(y^{(n)}-\theta[n-1]x^{(n)})\\ - p[n]=&\frac{p[n-1]}{\lambda+p[n-1]{(x^{(n)})}^2} -\end{align} -$$ - -#### Variables - -| Variable name | Symbol | -| ------------------ | ----------- | -| covariance | $p[n-1]$ | -| map_offset | $\theta[n]$ | -| forgetting*factor* | $\lambda$ | -| phi | $x(=1)$ | -| measured_acc | $y$ | - -### update_offset_four_cell_around [1] - -Update the offsets by RLS in four grids around newly obtained data. By considering linear interpolation, the update takes into account appropriate weights. Therefore, there is no need to remove data by thresholding. - -**Advantage** : No data is wasted because updates are performed on the 4 grids around the data with appropriate weighting. -**Disadvantage** : Accuracy may be degraded due to extreme bias of the data. For example, if data $z(k)$ is biased near $Z_{RR}$ in Fig. 2, updating is performed at the four surrounding points ( $Z_{RR}$, $Z_{RL}$, $Z_{LR}$, and $Z_{LL}$), but accuracy at $Z_{LL}$ is not expected. - - -

- -

- -#### Implementation - -See eq.(7)-(10) in [1] for the updated formula. In addition, eq.(17),(18) from [1] are used for Anti-Windup. - -### References - - - -[1] [Gabrielle Lochrie, Michael Doljevic, Mario Nona, Yongsoon Yoon, Anti-Windup Recursive Least Squares Method for Adaptive Lookup Tables with Application to Automotive Powertrain Control Systems, IFAC-PapersOnLine, Volume 54, Issue 20, 2021, Pages 840-845](https://www.sciencedirect.com/science/article/pii/S240589632102320X) diff --git a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml b/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml deleted file mode 100644 index c7ee0a3a..00000000 --- a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - update_hz: 10.0 - initial_covariance: 0.05 - velocity_min_threshold: 0.1 - velocity_diff_threshold: 0.556 - pedal_diff_threshold: 0.03 - max_steer_threshold: 0.2 - max_pitch_threshold: 0.02 - max_jerk_threshold: 0.7 - pedal_velocity_thresh: 0.15 - pedal_to_accel_delay: 0.3 - max_accel: 5.0 - min_accel: -5.0 - max_data_count: 100 - precision: 3 - update_method: "update_offset_four_cell_around" # or "update_offset_each_cell", "update_offset_total", "update_offset_four_cell_around" - update_suggest_thresh: 0.7 # threshold of rmse rate that update suggest flag becomes true. ( rmse_rate: [rmse of update map] / [rmse of original map] ) - progress_file_output: false # flag to output log file - accel_brake_value_source: "status" # "status" or "command" diff --git a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml b/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml deleted file mode 100644 index 2f272494..00000000 --- a/vehicle/accel_brake_map_calibrator/config/accel_brake_map_calibrator.xml +++ /dev/null @@ -1,818 +0,0 @@ - - - - #ffffff - #000000 - false - false - - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/0 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/3 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - raw accel - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/4 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - accel - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/1 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - accel pedal - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/2 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - brake pedal - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/12 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - success to update - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/13 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - jerk - - - - true - - 30 - main info - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - true - - 30 - Untitled Plot - - - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/9 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - raw pitch - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/10 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - pitch - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/5 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - raw accel speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/6 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - accel speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/7 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - raw brake speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/8 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - brake speed - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - data/11 - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /accel_brake_map_calibrator/debug_values - std_msgs/Float32MultiArray - - - - #000000 - 0 - - - 1000 - 10 - 0 - - - 100 - steer - - - - true - - 30 - sub info - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - true - - 30 - Untitled Plot - - - - false -
- diff --git a/vehicle/accel_brake_map_calibrator/data/accel_map.csv b/vehicle/accel_brake_map_calibrator/data/accel_map.csv deleted file mode 100644 index 32e639ca..00000000 --- a/vehicle/accel_brake_map_calibrator/data/accel_map.csv +++ /dev/null @@ -1,7 +0,0 @@ -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/accel_brake_map_calibrator/data/accel_map_double_res.csv b/vehicle/accel_brake_map_calibrator/data/accel_map_double_res.csv deleted file mode 100644 index e5492886..00000000 --- a/vehicle/accel_brake_map_calibrator/data/accel_map_double_res.csv +++ /dev/null @@ -1,12 +0,0 @@ -default,0,0.695,1.39,2.085,2.78,3.475,4.17,4.865,5.56,6.255,6.95,7.645,8.34,9.035,9.73,10.425,11.12,11.815,12.51,13.205,13.9 -0,0.3,0.125,-0.05,-0.175,-0.3,-0.345,-0.39,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5 -0.05,0.45,0.3175,0.185,0.0775,-0.03,-0.0675,-0.105,-0.1225,-0.14,-0.16,-0.18,-0.215,-0.25,-0.275,-0.3,-0.315,-0.33,-0.345,-0.36,-0.375,-0.39 -0.1,0.6,0.51,0.42,0.33,0.24,0.21,0.18,0.15,0.12,0.085,0.05,-0.015,-0.08,-0.12,-0.16,-0.18,-0.2,-0.22,-0.24,-0.26,-0.28 -0.15,0.875,0.7875,0.7,0.605,0.51,0.45,0.39,0.345,0.3,0.2475,0.195,0.1425,0.09,0.055,0.02,-0.015,-0.05,-0.0725,-0.095,-0.125,-0.155 -0.2,1.15,1.065,0.98,0.88,0.78,0.69,0.6,0.54,0.48,0.41,0.34,0.3,0.26,0.23,0.2,0.15,0.1,0.075,0.05,0.01,-0.03 -0.25,1.45,1.37,1.29,1.195,1.1,1.025,0.95,0.88,0.81,0.74,0.67,0.625,0.58,0.54,0.5,0.455,0.41,0.3775,0.345,0.31,0.275 -0.3,1.75,1.675,1.6,1.51,1.42,1.36,1.3,1.22,1.14,1.07,1,0.95,0.9,0.85,0.8,0.76,0.72,0.68,0.64,0.61,0.58 -0.35,2.2,2.12,2.04,1.95,1.86,1.7875,1.715,1.63,1.545,1.46,1.375,1.3075,1.24,1.1825,1.125,1.0725,1.02,0.97,0.92,0.88,0.84 -0.4,2.65,2.565,2.48,2.39,2.3,2.215,2.13,2.04,1.95,1.85,1.75,1.665,1.58,1.515,1.45,1.385,1.32,1.26,1.2,1.15,1.1 -0.45,2.975,2.92,2.865,2.7875,2.71,2.6175,2.525,2.42,2.315,2.1825,2.05,1.9625,1.875,1.795,1.715,1.6575,1.6,1.5325,1.465,1.41,1.355 -0.5,3.3,3.275,3.25,3.185,3.12,3.02,2.92,2.8,2.68,2.515,2.35,2.26,2.17,2.075,1.98,1.93,1.88,1.805,1.73,1.67,1.61 diff --git a/vehicle/accel_brake_map_calibrator/data/brake_map.csv b/vehicle/accel_brake_map_calibrator/data/brake_map.csv deleted file mode 100644 index adf2c809..00000000 --- a/vehicle/accel_brake_map_calibrator/data/brake_map.csv +++ /dev/null @@ -1,10 +0,0 @@ -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/accel_brake_map_calibrator/data/brake_map_double_res.csv b/vehicle/accel_brake_map_calibrator/data/brake_map_double_res.csv deleted file mode 100644 index 4e9dfb16..00000000 --- a/vehicle/accel_brake_map_calibrator/data/brake_map_double_res.csv +++ /dev/null @@ -1,18 +0,0 @@ -default,0,0.695,1.39,2.085,2.78,3.475,4.17,4.865,5.56,6.25,6.94,7.635,8.33,9.025,9.72,10.415,11.11,11.805,12.5,13.195,13.89 -0,0.3,0.125,-0.05,-0.175,-0.3,-0.345,-0.39,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5 -0.05,0.295,0.12,-0.055,-0.18,-0.305,-0.35,-0.395,-0.4,-0.405,-0.41,-0.415,-0.42,-0.425,-0.435,-0.445,-0.455,-0.465,-0.475,-0.485,-0.495,-0.505 -0.1,0.29,0.115,-0.06,-0.185,-0.31,-0.355,-0.4,-0.405,-0.41,-0.415,-0.42,-0.425,-0.43,-0.44,-0.45,-0.46,-0.47,-0.48,-0.49,-0.5,-0.51 -0.15,-0.045,-0.1375,-0.23,-0.3725,-0.515,-0.5575,-0.6,-0.6075,-0.615,-0.625,-0.635,-0.6425,-0.65,-0.66,-0.67,-0.68,-0.69,-0.7025,-0.715,-0.725,-0.735 -0.2,-0.38,-0.39,-0.4,-0.56,-0.72,-0.76,-0.8,-0.81,-0.82,-0.835,-0.85,-0.86,-0.87,-0.88,-0.89,-0.9,-0.91,-0.925,-0.94,-0.95,-0.96 -0.25,-0.69,-0.705,-0.72,-0.91,-1.1,-1.1375,-1.175,-1.185,-1.195,-1.2075,-1.22,-1.23,-1.24,-1.25,-1.26,-1.26525,-1.2705,-1.27825,-1.286,-1.29125,-1.2965 -0.3,-1,-1.02,-1.04,-1.26,-1.48,-1.515,-1.55,-1.56,-1.57,-1.58,-1.59,-1.6,-1.61,-1.62,-1.63,-1.6305,-1.631,-1.6315,-1.632,-1.6325,-1.633 -0.35,-1.24,-1.255,-1.27,-1.4675,-1.665,-1.7325,-1.8,-1.8175,-1.835,-1.84025,-1.8455,-1.85075,-1.856,-1.86125,-1.8665,-1.867,-1.8675,-1.868,-1.8685,-1.869,-1.8695 -0.4,-1.48,-1.49,-1.5,-1.675,-1.85,-1.95,-2.05,-2.075,-2.1,-2.1005,-2.101,-2.1015,-2.102,-2.1025,-2.103,-2.1035,-2.104,-2.1045,-2.105,-2.1055,-2.106 -0.45,-1.485,-1.495,-1.505,-1.68,-1.855,-1.955,-2.055,-2.08,-2.105,-2.1055,-2.106,-2.1065,-2.107,-2.1075,-2.108,-2.1085,-2.109,-2.1095,-2.11,-2.1105,-2.111 -0.5,-1.49,-1.5,-1.51,-1.685,-1.86,-1.96,-2.06,-2.085,-2.11,-2.1105,-2.111,-2.1115,-2.112,-2.1125,-2.113,-2.1135,-2.114,-2.1145,-2.115,-2.1155,-2.116 -0.55,-1.495,-1.505,-1.515,-1.69,-1.865,-1.965,-2.065,-2.09,-2.115,-2.1155,-2.116,-2.1165,-2.117,-2.1175,-2.118,-2.1185,-2.119,-2.1195,-2.12,-2.1205,-2.121 -0.6,-1.5,-1.51,-1.52,-1.695,-1.87,-1.97,-2.07,-2.095,-2.12,-2.1205,-2.121,-2.1215,-2.122,-2.1225,-2.123,-2.1235,-2.124,-2.1245,-2.125,-2.1255,-2.126 -0.65,-1.505,-1.515,-1.525,-1.7,-1.875,-1.975,-2.075,-2.1,-2.125,-2.1255,-2.126,-2.1265,-2.127,-2.1275,-2.128,-2.1285,-2.129,-2.1295,-2.13,-2.1305,-2.131 -0.7,-1.51,-1.52,-1.53,-1.705,-1.88,-1.98,-2.08,-2.105,-2.13,-2.1305,-2.131,-2.1315,-2.132,-2.1325,-2.133,-2.1335,-2.134,-2.1345,-2.135,-2.1355,-2.136 -0.75,-1.845,-1.855,-1.865,-2.0775,-2.29,-2.365,-2.44,-2.4775,-2.515,-2.52775,-2.5405,-2.541,-2.5415,-2.542,-2.5425,-2.543,-2.5435,-2.544,-2.5445,-2.545,-2.5455 -0.8,-2.18,-2.19,-2.2,-2.45,-2.7,-2.75,-2.8,-2.85,-2.9,-2.925,-2.95,-2.9505,-2.951,-2.9515,-2.952,-2.9525,-2.953,-2.9535,-2.954,-2.9545,-2.955 diff --git a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp deleted file mode 100644 index 3580f054..00000000 --- a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ /dev/null @@ -1,381 +0,0 @@ -// -// Copyright 2020 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. -// - -#ifndef ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ -#define ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ - -#include "diagnostic_updater/diagnostic_updater.hpp" -#include "raw_vehicle_cmd_converter/accel_map.hpp" -#include "raw_vehicle_cmd_converter/brake_map.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2/utils.h" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" - -#include - -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32_multi_array.hpp" -#include "std_msgs/msg/multi_array_dimension.hpp" -#include "std_msgs/msg/string.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" -#include "tier4_external_api_msgs/msg/calibration_status.hpp" -#include "tier4_external_api_msgs/msg/calibration_status_array.hpp" -#include "tier4_external_api_msgs/srv/get_accel_brake_map_calibration_data.hpp" -#include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" -#include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" -#include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -#include -#include -#include -#include -#include -#include - -namespace accel_brake_map_calibrator -{ - -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::VelocityReport; -using geometry_msgs::msg::TwistStamped; -using nav_msgs::msg::OccupancyGrid; -using raw_vehicle_cmd_converter::AccelMap; -using raw_vehicle_cmd_converter::BrakeMap; -using std_msgs::msg::Float32MultiArray; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; -using tier4_external_api_msgs::msg::CalibrationStatus; -using tier4_vehicle_msgs::msg::ActuationCommandStamped; -using tier4_vehicle_msgs::msg::ActuationStatusStamped; -using visualization_msgs::msg::MarkerArray; - -using tier4_vehicle_msgs::srv::UpdateAccelBrakeMap; - -using Map = std::vector>; - -struct DataStamped -{ - DataStamped(const double _data, const rclcpp::Time & _data_time) - : data{_data}, data_time{_data_time} - { - } - double data; - rclcpp::Time data_time; -}; -using DataStampedPtr = std::shared_ptr; - -class AccelBrakeMapCalibrator : public rclcpp::Node -{ -private: - std::shared_ptr transform_listener_; - std::string csv_default_map_dir_; - rclcpp::Publisher::SharedPtr original_map_occ_pub_; - rclcpp::Publisher::SharedPtr update_map_occ_pub_; - rclcpp::Publisher::SharedPtr original_map_raw_pub_; - rclcpp::Publisher::SharedPtr update_map_raw_pub_; - rclcpp::Publisher::SharedPtr offset_covariance_pub_; - rclcpp::Publisher::SharedPtr debug_pub_; - rclcpp::Publisher::SharedPtr data_count_pub_; - rclcpp::Publisher::SharedPtr data_count_with_self_pose_pub_; - rclcpp::Publisher::SharedPtr data_ave_pub_; - rclcpp::Publisher::SharedPtr data_std_pub_; - rclcpp::Publisher::SharedPtr index_pub_; - rclcpp::Publisher::SharedPtr update_suggest_pub_; - rclcpp::Publisher::SharedPtr current_map_error_pub_; - rclcpp::Publisher::SharedPtr updated_map_error_pub_; - rclcpp::Publisher::SharedPtr map_error_ratio_pub_; - rclcpp::Publisher::SharedPtr calibration_status_pub_; - - rclcpp::Subscription::SharedPtr velocity_sub_; - rclcpp::Subscription::SharedPtr steer_sub_; - rclcpp::Subscription::SharedPtr actuation_status_sub_; - rclcpp::Subscription::SharedPtr actuation_cmd_sub_; - - // Service - rclcpp::Service::SharedPtr update_map_dir_server_; - - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::TimerBase::SharedPtr timer_output_csv_; - void initTimer(double period_s); - void initOutputCSVTimer(double period_s); - - TwistStamped::ConstSharedPtr twist_ptr_; - std::vector> twist_vec_; - std::vector accel_pedal_vec_; // for delayed pedal - std::vector brake_pedal_vec_; // for delayed pedal - SteeringReport::ConstSharedPtr steer_ptr_; - DataStampedPtr accel_pedal_ptr_; - DataStampedPtr brake_pedal_ptr_; - DataStampedPtr delayed_accel_pedal_ptr_; - DataStampedPtr delayed_brake_pedal_ptr_; - - // Diagnostic Updater - std::shared_ptr updater_ptr_; - bool is_default_map_ = true; - - int get_pitch_method_; - int update_method_; - int accel_brake_value_source_; - double acceleration_ = 0.0; - double acceleration_time_; - double pre_acceleration_ = 0.0; - double pre_acceleration_time_; - double jerk_ = 0.0; - double accel_pedal_speed_ = 0.0; - double brake_pedal_speed_ = 0.0; - double pitch_ = 0.0; - double update_hz_; - double velocity_min_threshold_; - double velocity_diff_threshold_; - double pedal_diff_threshold_; - double max_steer_threshold_; - double max_pitch_threshold_; - double max_jerk_threshold_; - double pedal_velocity_thresh_; - double max_accel_; - double min_accel_; - double pedal_to_accel_delay_; - int precision_; - std::string csv_calibrated_map_dir_; - std::string output_accel_file_; - std::string output_brake_file_; - - // for calculating differential of msg - const double dif_twist_time_ = 0.2; // 200ms - const double dif_pedal_time_ = 0.16; // 160ms - const std::size_t twist_vec_max_size_ = 100; - const std::size_t pedal_vec_max_size_ = 100; - const double timeout_sec_ = 0.1; - int max_data_count_; - const int max_data_save_num_ = 10000; - const double map_resolution_ = 0.1; - const double max_jerk_ = 5.0; - bool pedal_accel_graph_output_ = false; - bool progress_file_output_ = false; - - // Algorithm - AccelMap accel_map_; - BrakeMap brake_map_; - - // for evaluation - AccelMap new_accel_map_; - BrakeMap new_brake_map_; - std::vector part_original_accel_mse_que_; - std::vector full_original_accel_mse_que_; - // std::vector full_original_accel_esm_que_; - std::vector full_original_accel_l1_que_; - std::vector full_original_accel_sq_l1_que_; - std::vector new_accel_mse_que_; - std::size_t full_mse_que_size_ = 100000; - std::size_t part_mse_que_size_ = 3000; - double full_original_accel_rmse_ = 0.0; - double full_original_accel_error_l1norm_ = 0.0; - double part_original_accel_rmse_ = 0.0; - double new_accel_rmse_ = 0.0; - double update_suggest_thresh_; - - // Accel / Brake Map update - Map accel_map_value_; - Map brake_map_value_; - Map update_accel_map_value_; - Map update_brake_map_value_; - Map accel_offset_covariance_value_; - Map brake_offset_covariance_value_; - std::vector map_value_data_; - std::vector accel_vel_index_; - std::vector brake_vel_index_; - std::vector accel_pedal_index_; - std::vector brake_pedal_index_; - bool update_success_; - int update_success_count_ = 0; - int update_count_ = 0; - int lack_of_data_count_ = 0; - int failed_to_get_pitch_count_ = 0; - int too_large_pitch_count_ = 0; - int too_low_speed_count_ = 0; - int too_large_steer_count_ = 0; - int too_large_jerk_count_ = 0; - int invalid_acc_brake_count_ = 0; - int too_large_pedal_spd_count_ = 0; - int update_fail_count_ = 0; - - // for map update - double map_offset_ = 0.0; - double map_coef_ = 1.0; - double covariance_; - const double forgetting_factor_ = 0.999; - const double coef_update_skip_thresh_ = 0.1; - - // output log - std::ofstream output_log_; - - bool getCurrentPitchFromTF(double * pitch); - void timerCallback(); - void timerCallbackOutputCSV(); - void executeUpdate( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index); - bool updateFourCellAroundOffset( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index, const double measured_acc); - bool updateEachValOffset( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index, const double measured_acc, - const double map_acc); - void updateTotalMapOffset(const double measured_acc, const double map_acc); - void callbackActuation( - const std_msgs::msg::Header header, const double accel, const double brake); - void callbackActuationCommand(const ActuationCommandStamped::ConstSharedPtr msg); - void callbackActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg); - void callbackVelocity(const VelocityReport::ConstSharedPtr msg); - void callbackSteer(const SteeringReport::ConstSharedPtr msg); - bool callbackUpdateMapService( - const std::shared_ptr request_header, - UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res); - bool getAccFromMap(const double velocity, const double pedal); - double lowpass(const double original, const double current, const double gain = 0.8); - double getPedalSpeed( - const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, - const double prev_pedal_speed); - double getAccel( - const TwistStamped::ConstSharedPtr & prev_twist, - const TwistStamped::ConstSharedPtr & current_twist); - double getJerk(); - bool indexValueSearch( - const std::vector value_index, const double value, const double value_thresh, - int * searched_index); - int nearestValueSearch(const std::vector value_index, const double value); - int nearestPedalSearch(); - int nearestVelSearch(); - void takeConsistencyOfAccelMap(); - void takeConsistencyOfBrakeMap(); - bool updateAccelBrakeMap(); - void publishFloat32(const std::string publish_type, const double val); - void publishUpdateSuggestFlag(); - double getPitchCompensatedAcceleration(); - void executeEvaluation(); - double calculateEstimatedAcc( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map); - double calculateAccelSquaredError( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map); - double calculateAccelErrorL1Norm( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map); - std::vector getMapColumnFromUnifiedIndex( - const Map & accel_map_value, const Map & brake_map_value, const std::size_t index); - double getPedalValueFromUnifiedIndex(const std::size_t index); - int getUnifiedIndexFromAccelBrakeIndex(const bool accel_map, const std::size_t index); - void pushDataToQue( - const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, - std::queue * que); - template - void pushDataToVec(const T data, const std::size_t max_size, std::vector * vec); - template - T getNearestTimeDataFromVec( - const T base_data, const double back_time, const std::vector & vec); - DataStampedPtr getNearestTimeDataFromVec( - DataStampedPtr base_data, const double back_time, const std::vector & vec); - double getAverage(const std::vector & vec); - double getStandardDeviation(const std::vector & vec); - bool isTimeout(const builtin_interfaces::msg::Time & stamp, const double timeout_sec); - bool isTimeout(const DataStampedPtr & data_stamped, const double timeout_sec); - - /* for covariance calculation */ - // mean value on each cell (counting method depends on the update algorithm) - Eigen::MatrixXd accel_data_mean_mat_; - Eigen::MatrixXd brake_data_mean_mat_; - // calculated variance on each cell - Eigen::MatrixXd accel_data_covariance_mat_; - Eigen::MatrixXd brake_data_covariance_mat_; - // number of data on each cell (counting method depends on the update algorithm) - Eigen::MatrixXd accel_data_num_; - Eigen::MatrixXd brake_data_num_; - - OccupancyGrid getOccMsg( - const std::string frame_id, const double height, const double width, const double resolution, - const std::vector & map_value); - - /* Diag*/ - void checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat); - - /* Debug */ - void publishMap( - const Map accel_map_value, const Map brake_map_value, const std::string publish_type); - void publishOffsetCovMap(const Map accel_map_value, const Map brake_map_value); - void publishCountMap(); - void publishIndex(); - bool writeMapToCSV( - std::vector vel_index, std::vector pedal_index, Map value_map, - std::string filename); - void addIndexToCSV(std::ofstream * csv_file); - void addLogToCSV( - std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, - const double pitched_accel, const double accel_pedal, const double brake_pedal, - const double accel_pedal_speed, const double brake_pedal_speed, const double pitch, - const double steer, const double jerk, const double full_original_accel_mse, - const double part_original_accel_mse, const double new_accel_mse); - - mutable Float32MultiArrayStamped debug_values_; - enum DBGVAL { - CURRENT_SPEED = 0, - CURRENT_ACCEL_PEDAL = 1, - CURRENT_BRAKE_PEDAL = 2, - CURRENT_RAW_ACCEL = 3, - CURRENT_ACCEL = 4, - CURRENT_RAW_ACCEL_SPEED = 5, - CURRENT_ACCEL_SPEED = 6, - CURRENT_RAW_BRAKE_SPEED = 7, - CURRENT_BRAKE_SPEED = 8, - CURRENT_RAW_PITCH = 9, - CURRENT_PITCH = 10, - CURRENT_STEER = 11, - SUCCESS_TO_UPDATE = 12, - CURRENT_JERK = 13, - }; - static constexpr unsigned int num_debug_values_ = 14; - - enum GET_PITCH_METHOD { TF = 0, FILE = 1, NONE = 2 }; - - enum UPDATE_METHOD { - UPDATE_OFFSET_EACH_CELL = 0, - UPDATE_OFFSET_TOTAL = 1, - UPDATE_OFFSET_FOUR_CELL_AROUND = 2, - }; - - enum ACCEL_BRAKE_SOURCE { - STATUS = 0, - COMMAND = 1, - }; - - std::unique_ptr logger_configure_; - -public: - explicit AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options); -}; - -} // namespace accel_brake_map_calibrator - -#endif // ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ diff --git a/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml b/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml deleted file mode 100644 index c664158a..00000000 --- a/vehicle/accel_brake_map_calibrator/launch/accel_brake_map_calibrator.launch.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png b/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png deleted file mode 100644 index c29781b2..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/actuation_cmd_publisher_util.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/add_panel.png b/vehicle/accel_brake_map_calibrator/media/add_panel.png deleted file mode 100644 index f045042e..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/add_panel.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png b/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png deleted file mode 100644 index f21a2549..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/calib_rviz_image_sample.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png b/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png deleted file mode 100644 index 28db957c..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/calibrator_button_panel.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png b/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png deleted file mode 100644 index 8929f3d6..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/fourcell_RLS.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/log_sample.png b/vehicle/accel_brake_map_calibrator/media/log_sample.png deleted file mode 100644 index 13887a42..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/log_sample.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png b/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png deleted file mode 100644 index e57ed269..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/push_calibration_button.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/media/statistics_sample.png b/vehicle/accel_brake_map_calibrator/media/statistics_sample.png deleted file mode 100644 index c36ce9a5..00000000 Binary files a/vehicle/accel_brake_map_calibrator/media/statistics_sample.png and /dev/null differ diff --git a/vehicle/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/package.xml deleted file mode 100644 index 64e7d1fb..00000000 --- a/vehicle/accel_brake_map_calibrator/package.xml +++ /dev/null @@ -1,42 +0,0 @@ - - - - accel_brake_map_calibrator - 0.1.0 - The accel_brake_map_calibrator - Tomoya Kimura - Taiki Tanaka - Takeshi Miura - - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - autoware_auto_vehicle_msgs - diagnostic_updater - geometry_msgs - motion_utils - raw_vehicle_cmd_converter - rclcpp - std_msgs - std_srvs - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils - tier4_debug_msgs - tier4_external_api_msgs - tier4_vehicle_msgs - visualization_msgs - - python3-matplotlib - python3-pandas - python3-scipy - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz b/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz deleted file mode 100644 index ffcb63f4..00000000 --- a/vehicle/accel_brake_map_calibrator/rviz/occupancy.rviz +++ /dev/null @@ -1,141 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Map1 - - /Axes1 - - /MarkerArray1 - Splitter Ratio: 0.5581113696098328 - Tree Height: 1615 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 1 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Map - Topic: /accel_brake_map_calibrator/debug/data_count_self_pose_occ_map - Unreliable: false - Use Timestamp: false - Value: true - - Class: rviz_default_plugins/Axes - Enabled: false - Length: 0.10000000149011612 - Name: Axes - Radius: 0.009999999776482582 - Reference Frame: - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - occ_pedal_index: true - occ_vel_index: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /accel_brake_map_calibrator/debug/occ_index - Value: true - Enabled: true - Global Options: - Background Color: 238; 238; 236 - Default Light: true - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - - Class: rviz_default_plugins/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz_default_plugins/SetGoal - Topic: /move_base_simple/goal - - Class: rviz_plugins/PedestrianInitialPoseTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Theta std deviation: 0.0872664600610733 - Velocity: 1 - X std deviation: 0.009999999776482582 - Y std deviation: 0.009999999776482582 - Z position: 0 - Z std deviation: 0.009999999776482582 - - Class: rviz_plugins/CarInitialPoseTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Theta std deviation: 0.0872664600610733 - Velocity: 3 - X std deviation: 0.009999999776482582 - Y std deviation: 0.009999999776482582 - Z position: 0 - Z std deviation: 0.009999999776482582 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Value: true - Views: - Current: - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 1.8697248697280884 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.48724469542503357 - Y: 0.3405919671058655 - Z: 5.960464477539062e-7 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 4.710395336151123 - Saved: ~ -Window Geometry: - Displays: - collapsed: true - Height: 1862 - Hide Left Dock: true - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000005070000068cfc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000068c000000c900fffffffb0000000a00560069006500770073000000030c000003bd000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000005470000012b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d00610067006501000005d6000000f30000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000000a0049006d00610067006500000002ef000003da0000000000000000000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006fb0000005afc0100000002fb0000000800540069006d00650100000000000006fb000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006fb0000068c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Views: - collapsed: false - Width: 1787 - X: 1193 - Y: 165 diff --git a/vehicle/accel_brake_map_calibrator/scripts/__init__.py b/vehicle/accel_brake_map_calibrator/scripts/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py b/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py deleted file mode 100755 index f73718f7..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/accel_tester.py +++ /dev/null @@ -1,57 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2022 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. - -import rclpy -from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped - -MAX_ACCEL = 1.0 # [-] -MIN_ACCEL = 0.0 # [-] - - -class AccelTester(Node): - def __init__(self): - super().__init__("vehicle_accel_tester") - self.pub = self.create_publisher(Float32Stamped, "/vehicle/tester/accel", 1) - - def run(self): - while rclpy.ok(): - value = float(input(f"target accel [{MIN_ACCEL} ~ {MAX_ACCEL} -] > ")) - if value > MAX_ACCEL: - print("input value is larger than max accel!" + f"input: {value} max: {MAX_ACCEL}") - value = MAX_ACCEL - elif value < MIN_ACCEL: - print("input value is smaller than min accel!" + f"input: {value} min: {MIN_ACCEL}") - value = MIN_ACCEL - - msg = Float32Stamped(stamp=self.get_clock().now().to_msg(), data=value) - - self.pub.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - - accel_tester = AccelTester() - accel_tester.run() - - accel_tester.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py b/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py deleted file mode 100755 index 749c9e42..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py +++ /dev/null @@ -1,83 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2022 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. - -from autoware_auto_vehicle_msgs.msg import GearCommand -import rclpy -from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped -from tier4_vehicle_msgs.msg import ActuationCommandStamped - - -class ActuationCmdPublisher(Node): - def __init__(self): - super().__init__("actuation_cmd_publisher") - self.target_brake = 0.0 - self.target_accel = 0.0 - - self.sub_acc = self.create_subscription( - Float32Stamped, "/vehicle/tester/accel", self.on_accel, 1 - ) - self.sub_brk = self.create_subscription( - Float32Stamped, "/vehicle/tester/brake", self.on_brake, 1 - ) - self.pub_actuation_cmd = self.create_publisher( - ActuationCommandStamped, "/control/command/actuation_cmd", 1 - ) - self.pub_gear_cmd = self.create_publisher(GearCommand, "/control/command/gear_cmd", 1) - timer_period = 0.02 # seconds - self.timer = self.create_timer(timer_period, self.on_timer) - - def on_brake(self, msg): - self.target_brake = msg.data - print(f"Set target brake : {self.target_brake}") - - def on_accel(self, msg): - self.target_accel = msg.data - print(f"Set target accel : {self.target_accel}") - - def on_timer(self): - msg_actuation_cmd = ActuationCommandStamped() - msg_actuation_cmd.actuation.steer_cmd = 0.0 - msg_actuation_cmd.header.stamp = self.get_clock().now().to_msg() - msg_actuation_cmd.header.frame_id = "base_link" - msg_actuation_cmd.actuation.accel_cmd = self.target_accel - msg_actuation_cmd.actuation.brake_cmd = self.target_brake - self.pub_actuation_cmd.publish(msg_actuation_cmd) - - msg_gear_cmd = GearCommand() - msg_gear_cmd.stamp = self.get_clock().now().to_msg() - msg_gear_cmd.command = GearCommand.DRIVE - self.pub_gear_cmd.publish(msg_gear_cmd) - - print( - f"publish ActuationCommand with accel: {self.target_accel}, brake: {self.target_brake}" - ) - - -def main(args=None): - rclpy.init(args=args) - - actuation_cmd_publisher = ActuationCmdPublisher() - - rclpy.spin(actuation_cmd_publisher) - - actuation_cmd_publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py b/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py deleted file mode 100755 index 050f2323..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/brake_tester.py +++ /dev/null @@ -1,61 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2022 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. - - -import rclpy -from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped - -MAX_BRAKE = 1.0 # [-] -MIN_BRAKE = 0.0 # [-] - - -class BrakeTester(Node): - def __init__(self): - super().__init__("vehicle_brake_tester") - self.pub = self.create_publisher(Float32Stamped, "/vehicle/tester/brake", 1) - - def run(self): - while rclpy.ok(): - value = float( - input("target brake [" + str(MIN_BRAKE) + " ~ " + str(MAX_BRAKE) + "] > ") - ) - - if value > MAX_BRAKE: - print("input value is larger than max brake!" + f"input: {value} max: {MAX_BRAKE}") - value = MAX_BRAKE - elif value < MIN_BRAKE: - print("input value is smaller than min brake!" + f"input: {value} min: {MIN_BRAKE}") - value = MIN_BRAKE - - msg = Float32Stamped(stamp=self.get_clock().now().to_msg(), data=value) - - self.pub.publish(msg) - - -def main(args=None): - rclpy.init(args=args) - - brake_tester = BrakeTester() - brake_tester.run() - - brake_tester.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py b/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py deleted file mode 100755 index f4581699..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/calc_utils.py +++ /dev/null @@ -1,151 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import sys - -import numpy as np -from scipy import signal - -BIT = 1e-04 - - -def get_map_list(y_num, x_num): - data_list = [] - for yn in range(y_num): - child_data_list = [] - for xn in range(x_num): - child_data_list.append([]) - if xn == x_num - 1: - data_list.append(child_data_list) - return data_list - - -class CalcUtils: - @staticmethod - def average_filter(data, average_num): - if type(average_num) != int: - print( - "Error in average_filter(data, average_num):\ - Type of average_num must be int" - ) - sys.exit(1) - - if average_num % 2 == 0: - print( - "Error in average_filter(data, average_num):\ - average_num must be odd number" - ) - sys.exit(1) - - average_filter = np.ones(average_num) / float(average_num) - average_data = np.convolve(data, average_filter) - cut_num = (average_num - 1) / 2 - return average_data[cut_num : len(average_data) - cut_num] - - @staticmethod - # fp:pass Hz, #fs: block Hz, g_pass: pass dB, g_stop: stop DB - def lowpass_filter_scipy(x, sample_rate, fp, fs, g_pass, g_stop): - fn = sample_rate / 2 - wp = fp / fn - ws = fs / fn - N, Wn = signal.buttord(wp, ws, g_pass, g_stop) - b, a = signal.butter(N, Wn, "low") - y = signal.filtfilt(b, a, x) - return y - - @staticmethod - def create_2d_map( - x, - y, - data, - color_factor, - x_index_list, - x_thresh, - y_index_list, - y_thresh, - calibration_method="four_cell", - ): - if x.shape != y.shape or y.shape != data.shape: - print("Error: the shape of x, y, data must be same") - sys.exit() - data_size = len(x) - - x_num = len(x_index_list) - y_num = len(y_index_list) - data_list = get_map_list(y_num, x_num) - full_data_list = get_map_list(y_num, x_num) - - if calibration_method == "four_cell": - x_thresh = np.abs(x_index_list[1] - x_index_list[0]) / 2 - y_thresh = np.abs(y_index_list[1] - y_index_list[0]) / 2 - - for i in range(0, data_size): - x_index = None - y_index = None - for xi in range(0, x_num): - if np.abs(x_index_list[xi] - x[i]) < x_thresh: - x_index = xi - break - for yi in range(0, y_num): - if np.abs(y_index_list[yi] - y[i]) < y_thresh: - y_index = yi - break - - if x_index is not None and y_index is not None: - # append data - data_list[y_index][x_index].append(data[i]) - full_data_list[y_index][x_index].append([x[i], y[i], data[i], color_factor[i]]) - - return data_list, full_data_list - - @staticmethod - def extract_x_index_map(data_map, x_index): - y_num = len(data_map) - extracted_data = None - # x_num = len(data_map[0]) - for y in range(y_num): - data = np.array(data_map[y][x_index]) - if len(data) == 0: - continue - elif extracted_data is None: - extracted_data = data - else: - extracted_data = np.concatenate([extracted_data, data]) - return extracted_data - - @staticmethod - def create_stat_map(data_map, statistics_type="average"): - y_num = len(data_map) - x_num = len(data_map[0]) - count_map = np.zeros((y_num, x_num)) - average_map = np.zeros((y_num, x_num)) - stddev_map = np.zeros((y_num, x_num)) - for y in range(y_num): - for x in range(x_num): - data = np.array(data_map[y][x]) - count_map[y][x] = data.shape[0] - if count_map[y][x] == 0: - # print('Warn: data_map', y, x, 'is vacant list') - average_map[y][x] = 0.0 - stddev_map[y][x] = 0.0 - else: - if statistics_type == "average": - average_map[y][x] = np.average(data) - elif statistics_type == "median": - average_map[y][x] = np.median(data) - stddev_map[y][x] = np.std(data) - return count_map, average_map, stddev_map diff --git a/vehicle/accel_brake_map_calibrator/scripts/config.py b/vehicle/accel_brake_map_calibrator/scripts/config.py deleted file mode 100755 index c554c406..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/config.py +++ /dev/null @@ -1,44 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import numpy as np - -# config of file index -TS = "timestamp" -VEL = "velocity" -RAW_ACC = "accel" -PITCH_ACC = "pitch_comp_accel" -ACC = "final_accel" -A_PED = "accel_pedal" -B_PED = "brake_pedal" -A_PED_SPD = "accel_pedal_speed" -B_PED_SPD = "brake_pedal_speed" -PITCH = "pitch" -JERK = "jerk" -STEER = "steer" - -# config of accel / brake map -VEL_LIST = np.array([0, 5, 10, 15, 20, 25, 30, 35, 40, 45, 50]) # km -PEDAL_LIST = np.array( - [-0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4, 0.5] -) -VEL_MIN = VEL_LIST[0] -VEL_MAX = VEL_LIST[-1] -VEL_SPAN = (VEL_MAX - VEL_MIN) / (len(VEL_LIST) - 1) -PEDAL_MIN = PEDAL_LIST[0] -PEDAL_MAX = PEDAL_LIST[-1] -PEDAL_SPAN = (PEDAL_MAX - PEDAL_MIN) / (len(PEDAL_LIST) - 1) diff --git a/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py b/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py deleted file mode 100755 index c4494cdb..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/csv_reader.py +++ /dev/null @@ -1,132 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import glob - -import config as CF -import numpy as np -import pandas as pd - -# pre-defined -NUMERIC_LIMITS = 1e-02 - - -class CSVReader(object): - def __init__(self, csv, csv_type="dir"): - if csv_type == "dir": - csv_files = glob.glob(csv + "/*.csv") - csv_list = [] - for cf in csv_files: - csv_list.append(pd.read_csv(cf, engine="python")) - self.csv_data = pd.concat(csv_list, ignore_index=True) - else: - self.csv_data = pd.read_csv(csv, engine="python") - - def removeUnusedData( - self, - min_vel_thr, - max_steer_thr, - max_pitch_thr, - max_pedal_vel_thr, - max_jerk_thr, - remove_by_invalid_pedal=True, - remove_by_vel=True, - remove_by_steer=True, - remove_by_pitch=True, - remove_by_pedal_speed=True, - remove_by_jerk=True, - ): - # remove unused data - raw_size = len(self.csv_data) - - for i in range(0, raw_size)[::-1]: - vel = self.csv_data[CF.VEL][i] - steer = self.csv_data[CF.STEER][i] - accel_pedal = self.csv_data[CF.A_PED][i] - brake_pedal = self.csv_data[CF.B_PED][i] - accel_pedal_speed = self.csv_data[CF.A_PED_SPD][i] - brake_pedal_speed = self.csv_data[CF.B_PED_SPD][i] - jerk = self.csv_data[CF.JERK][i] - pitch = self.csv_data[CF.PITCH][i] - - # invalid pedal - if ( - remove_by_invalid_pedal - and accel_pedal > NUMERIC_LIMITS - and brake_pedal > NUMERIC_LIMITS - ): - self.csv_data = self.csv_data.drop(i) - continue - - # low velocity - if remove_by_vel and vel < min_vel_thr: - self.csv_data = self.csv_data.drop(i) - continue - - # high steer - if remove_by_steer and np.abs(steer) > max_steer_thr: - self.csv_data = self.csv_data.drop(i) - continue - - # high pitch - if remove_by_pitch and np.abs(pitch) > max_pitch_thr: - self.csv_data = self.csv_data.drop(i) - continue - - # high pedal speed - if ( - remove_by_pedal_speed - and np.abs(accel_pedal_speed) > max_pedal_vel_thr - or np.abs(brake_pedal_speed) > max_pedal_vel_thr - ): - self.csv_data = self.csv_data.drop(i) - continue - - # max_jerk_thr - if remove_by_jerk and np.abs(jerk) > max_jerk_thr: - self.csv_data = self.csv_data.drop(i) - continue - - return self.csv_data - - def getVelData(self): - vel_data = np.array(self.csv_data[CF.VEL]) - return vel_data - - def getPedalData(self): - pedal_data = np.array(self.csv_data[CF.A_PED]) - np.array(self.csv_data[CF.B_PED]) - return pedal_data - - def getAccData(self): - acc_data = np.array(self.csv_data[CF.ACC]) - return acc_data - - def getPitchData(self): - pitch_data = np.array(self.csv_data[CF.PITCH]) - return pitch_data - - def extractPedalRangeWithDelay(self, delay_step, pedal_value, pedal_diff_thr): - csv_data = self.csv_data.reset_index(drop=True) - - for i in range(delay_step, len(self.csv_data))[::-1]: - pedal = csv_data[CF.A_PED][i - delay_step] - csv_data[CF.B_PED][i - delay_step] - # extract data of pedal = pedal_value - if pedal > pedal_value + pedal_diff_thr or pedal < pedal_value - pedal_diff_thr: - csv_data = csv_data.drop(i) - continue - - return csv_data diff --git a/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py b/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py deleted file mode 100755 index eb2bce38..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/delay_estimator.py +++ /dev/null @@ -1,144 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -from ament_index_python.packages import get_package_share_directory -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -import numpy as np -from plotter import Plotter -import rclpy -from rclpy.node import Node - - -class DelayEstimator(Node): - def __init__(self): - # get parameter - super().__init__("delay_estimator") - package_path = get_package_share_directory("accel_brake_map_calibrator") - self.declare_parameter("log_file", package_path + "/config/log.csv") - log_file = self.get_parameter("log_file").get_parameter_value().string_value - self.declare_parameter("velocity_min_threshold", 0.1) - min_vel_thr = ( - self.get_parameter("velocity_min_threshold").get_parameter_value().double_value - ) - self.declare_parameter("velocity_diff_threshold", 0.556) - vel_diff_thr = ( - self.get_parameter("velocity_diff_threshold").get_parameter_value().double_value - ) - self.declare_parameter("pedal_diff_threshold", 0.03) - pedal_diff_thr = ( - self.get_parameter("pedal_diff_threshold").get_parameter_value().double_value - ) - self.declare_parameter("max_steer_threshold", 0.2) - max_steer_thr = self.get_parameter("max_steer_threshold").get_parameter_value().double_value - self.declare_parameter("max_pitch_threshold", 0.02) - max_pitch_thr = self.get_parameter("max_pitch_threshold").get_parameter_value().double_value - self.declare_parameter("max_jerk_threshold", 0.7) - max_jerk_thr = self.get_parameter("max_jerk_threshold").get_parameter_value().double_value - self.declare_parameter("pedal_velocity_thresh", 0.15) - max_pedal_vel_thr = ( - self.get_parameter("pedal_velocity_thresh").get_parameter_value().double_value - ) - self.declare_parameter("update_hz", 10.0) - update_hz = self.get_parameter("update_hz").get_parameter_value().double_value - - self.data_span = 1.0 / update_hz - - # read csv - self.cr = CSVReader(log_file, csv_type="file") - # remove unused_data - self.csv_data = self.cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr - ) - - # get statistics array - vel_data = self.cr.getVelData() - pedal_data = self.cr.getPedalData() - acc_data = self.cr.getAccData() - - # get color factor (pitch) array for plotting - color_data = self.cr.getPitchData() - - data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - vel_diff_thr, - CF.PEDAL_LIST, - pedal_diff_thr, - ) - - # plot all data - # value to use for statistics - PEDAL_VALUE = 0.1 - VEL_VALUE_LIST = np.array([10, 15, 20]) / 3.6 - plotter = Plotter(3, 2) - max_delay_step = 5 - for delay_step in range(max_delay_step + 1): - print("data processing... " + str(delay_step) + " / " + str(max_delay_step)) - csv_data = self.cr.extractPedalRangeWithDelay(delay_step, PEDAL_VALUE, pedal_diff_thr) - - # get correlation coefficient - # extract data of velocity is VEL_VALUE - coef_list = [] - for vel_value in VEL_VALUE_LIST: - ex_csv_data = csv_data[csv_data[CF.VEL] < vel_value + vel_diff_thr] - ex_csv_data = csv_data[csv_data[CF.VEL] > vel_value - vel_diff_thr] - pedal_speed = ex_csv_data[CF.A_PED_SPD] - ex_csv_data[CF.B_PED_SPD] - accel = ex_csv_data[CF.ACC] - coef = self.getCorCoef(pedal_speed, accel) - coef_list.append(coef) - - print("delay_step: ", delay_step) - print("coef: ", coef_list) - - ave_coef = np.average(coef_list) - self.plotPedalSpeedAndAccel(csv_data, plotter, delay_step + 1, delay_step, ave_coef) - plotter.show() - - def getCorCoef(self, a, b): - coef = np.corrcoef(np.array(a), np.array(b)) - return coef[0, 1] - - def plotPedalSpeedAndAccel(self, csv_data, plotter, subplot_num, delay_step, coef): - pedal_speed = csv_data[CF.A_PED_SPD] - csv_data[CF.B_PED_SPD] - accel = csv_data[CF.ACC] - velocity = csv_data[CF.VEL] * 3.6 - fig = plotter.subplot(subplot_num) - plotter.scatter_color(pedal_speed, accel, velocity, "hsv", label=None) - delay_time_ms = delay_step * self.data_span * 1000 - plotter.add_label( - "pedal-spd-acc (delay = " + str(delay_time_ms) + " ms), R = " + str(coef), - "pedal-speed", - "accel", - ) - plotter.set_lim(fig, [-0.4, 0.4], [-1.0, 1.0]) - - -def main(args=None): - rclpy.init(args=args) - node = DelayEstimator() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py b/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py deleted file mode 100755 index 71cdb502..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/log_analyzer.py +++ /dev/null @@ -1,303 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import argparse -import sys - -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -import numpy as np -from plotter import Plotter - -# parameter for statistics -SCATTER_GRAPH_PEDAL_LIM = [-0.55, 0.55] -SCATTER_GRAPH_ACCEL_LIM = [-1.8, 1.8] -CONSIDER_PEDAL_VAL_FOR_STAT = [0.0, 0.1, 0.2] - -parser = argparse.ArgumentParser() -parser.add_argument("--csv-file-dir", help="log file directory") -parser.add_argument("--min-vel-thr", type=float, default=0.1, help="thresh of minimum velocity") -parser.add_argument( - "--max-pedal-vel-thr", type=float, default=0.15, help="thresh of maximum pedal velocity" -) -parser.add_argument("--max-steer-thr", type=float, default=0.2, help="thresh of maximum steer") -parser.add_argument("--max-pitch-thr", type=float, default=0.02, help="thresh of maximum pitch") -parser.add_argument("--max-jerk-thr", type=float, default=0.7, help="thresh of maximum jerk") -parser.add_argument( - "--pedal-diff-thr", - type=float, - default=0.03, - help="thresh of max delta-pedal to add statistics map", -) -parser.add_argument( - "--vel-diff-thr", - type=float, - default=0.556, - help="thresh of max delta-velocity to add statistics map", -) -parser.add_argument( - "--view-velocity-1", type=int, default=5, help="velocity(kmh) to visualize by plot (1)" -) -parser.add_argument( - "--view-velocity-2", type=int, default=20, help="velocity(kmh) to visualize by plot (2)" -) -parser.add_argument( - "--view-velocity-3", type=int, default=30, help="velocity(kmh) to visualize by plot (3)" -) -parser.add_argument("--output-stat-csv", default="stat.csv", help="output csv file name") -parser.add_argument("--output-plot-file", default="result.png", help="output plot file name") -parser.add_argument("--no-plot", action="store_true") - -args = parser.parse_args() -# get path -csv_file_dir = args.csv_file_dir -min_vel_thr = args.min_vel_thr -max_pedal_vel_thr = args.max_pedal_vel_thr -max_steer_thr = args.max_steer_thr -max_pitch_thr = args.max_pitch_thr -max_jerk_thr = args.max_jerk_thr -pedal_diff_thr = args.pedal_diff_thr -vel_diff_thr = args.vel_diff_thr -view_velocity_list = [] -view_velocity_list.append(args.view_velocity_1) -view_velocity_list.append(args.view_velocity_2) -view_velocity_list.append(args.view_velocity_3) -output_stat_csv = args.output_stat_csv -output_plot_file = args.output_plot_file -remove_by_invalid_pedal = True -remove_by_vel = True -remove_by_steer = True -remove_by_pitch = True -remove_by_pedal_speed = True -remove_by_jerk = True -is_plot = not args.no_plot - -view_velocity_idx_list = [] -for vv in view_velocity_list: - if not (vv in CF.VEL_LIST): - print("invalid view_velocity. velocity list is: ", CF.VEL_LIST) - sys.exit() - view_velocity_idx_list.append(CF.VEL_LIST.tolist().index(vv)) - -# search index of pedal to use for statistics -stat_pedal_list = [] -for val in CONSIDER_PEDAL_VAL_FOR_STAT: - if not (val in CF.PEDAL_LIST): - print("invalid CONSIDER_PEDAL_VAL_FOR_STAT. pedal list is:", CF.PEDAL_LIST) - stat_pedal_list.append(CF.PEDAL_LIST.tolist().index(val)) - -# read file -cr = CSVReader(csv_file_dir) - -# remove unused_data -csv_data = cr.removeUnusedData( - min_vel_thr, - max_steer_thr, - max_pitch_thr, - max_pedal_vel_thr, - max_jerk_thr, - remove_by_invalid_pedal, - remove_by_vel, - remove_by_steer, - remove_by_pitch, - remove_by_pedal_speed, - remove_by_jerk, -) - - -# get statistics array -vel_data = cr.getVelData() -pedal_data = cr.getPedalData() -acc_data = cr.getAccData() - -# get color factor (pitch) array for plotting -color_data = cr.getPitchData() - -data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - vel_diff_thr, - CF.PEDAL_LIST, - pedal_diff_thr, -) - -count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) -velocity_map_list = [] -for v_idx in view_velocity_idx_list: - velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, v_idx)) - -# output statistics -f = open(output_stat_csv, "a") -count_list_over_v = [] -stddev_list_over_v = [] -for vi in view_velocity_idx_list: - count_list = [] - stddev_list = [] - for pi in stat_pedal_list: - count_list.append(count_map[pi][vi]) - if count_map[pi][vi] > 5: - stddev_list.append(stddev_map[pi][vi]) - else: - print("Warning: count is fewer than 5.") - count_sum = int(np.sum(count_list)) - stddev_ave = np.average(stddev_list) - count_list_over_v.append(count_sum) - stddev_list_over_v.append(stddev_ave) - f.write(str(count_sum) + ",") - f.write(str(stddev_ave) + ",") - print("velocity index:", vi) - print("\tcount: ", count_sum) - print("\tstddev: ", stddev_ave) -count_sum_over_v = int(np.sum(count_list_over_v)) -stddev_ave_over_v = np.average(stddev_list_over_v) -f.write(str(count_sum_over_v) + ",") -f.write(str(stddev_ave_over_v) + "\n") -f.close() -print("full data:") -print("\tcount: ", count_sum_over_v) -print("\tstddev: ", stddev_ave_over_v) - -# visualization -plotter = Plotter(2, 3) -plotter.subplot(1) -plotter.imshow( - average_map, CF.VEL_MIN, CF.VEL_MAX, CF.VEL_SPAN, CF.PEDAL_MIN, CF.PEDAL_MAX, CF.PEDAL_SPAN -) -plotter.add_label("average of accel", "velocity(kmh)", "throttle") - -plotter.subplot(2) -plotter.imshow( - stddev_map, CF.VEL_MIN, CF.VEL_MAX, CF.VEL_SPAN, CF.PEDAL_MIN, CF.PEDAL_MAX, CF.PEDAL_SPAN -) -plotter.add_label("std. dev. of accel", "velocity(kmh)", "throttle") - -plotter.subplot(3) -plotter.imshow( - count_map, - CF.VEL_MIN, - CF.VEL_MAX, - CF.VEL_SPAN, - CF.PEDAL_MIN, - CF.PEDAL_MAX, - CF.PEDAL_SPAN, - num_data_type="int", -) -plotter.add_label("number of accel data", "velocity(kmh)", "throttle") - -# view pedal-accel graph - - -def view_pedal_accel_graph(subplot_num, vel_list_idx): - # plot all data - fig = plotter.subplot(subplot_num) - plotter.scatter( - velocity_map_list[vel_list_idx][:, 1], - velocity_map_list[vel_list_idx][:, 2], - "blue", - label="all", - ) - - # plot average data - # remove average score of no data - average_map_v_avoid_0 = ( - average_map[:, view_velocity_idx_list[vel_list_idx]] - + np.where(average_map[:, view_velocity_idx_list[vel_list_idx]] == 0, True, False) * 1e03 - ) - - plotter.scatter(CF.PEDAL_LIST, average_map_v_avoid_0, "red", label="average") - - plotter.set_lim(fig, SCATTER_GRAPH_PEDAL_LIM, SCATTER_GRAPH_ACCEL_LIM) - plotter.add_label( - str(view_velocity_list[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" - ) - - -view_pedal_accel_graph(4, 0) -view_pedal_accel_graph(5, 1) -view_pedal_accel_graph(6, 2) - -if is_plot: - plotter.show() -plotter.save(output_plot_file) - - -# pedal-pitch plot -""" -cr = CSVReader(csv_file_dir) -csv_data = cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, - max_pedal_vel_thr, max_jerk_thr, - remove_by_invalid_pedal, - remove_by_vel=True, - remove_by_steer=False, - remove_by_pitch=False, - remove_by_pedal_speed=False, - remove_by_jerk=False) -pitch = csv_data[CF.PITCH] -pedal = csv_data[CF.A_PED] - csv_data[CF.B_PED] -velocity = csv_data[CF.VEL] * 3.6 # color -plotter.scatter_color(pedal, pitch, velocity, (0.0, 35.0), "hsv", label=None) -plotter.add_label("pedal-pitch relation", "pedal", "pitch") -plotter.show() -""" - -""" -# pedal-speed-accel plot -cr = CSVReader(csv_file_dir) -csv_data = cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, - max_pedal_vel_thr, max_jerk_thr, - remove_by_invalid_pedal, - remove_by_vel=True, - remove_by_steer=True, - remove_by_pitch=True, - remove_by_pedal_speed=False, - remove_by_jerk=False) - -csv_data = csv_data.reset_index(drop=True) -delay = 3 # accel delay (*100ms) -for i in range(delay, len(csv_data))[::-1]: - pedal = csv_data[CF.A_PED][i-delay] - csv_data[CF.B_PED][i-delay] - # extract data of pedal = 0.1 - tgt_ped = 0.1 - if pedal > tgt_ped + pedal_diff_thr or \ - pedal < tgt_ped - pedal_diff_thr: - csv_data = csv_data.drop(i) - continue - - velocity = csv_data[CF.VEL][i] - if velocity > 12.0 / 3.6 or \ - velocity < 8.0 / 3.6: - csv_data = csv_data.drop(i) - continue - -pedal_speed = csv_data[CF.A_PED_SPD] - csv_data[CF.B_PED_SPD] -accel = csv_data[CF.ACC] -velocity = csv_data[CF.VEL] * 3.6 -plotter = Plotter(1, 1) -fig = plotter.subplot(1) -plotter.scatter_color(pedal_speed, accel, velocity, - (0.0, 35.0), "hsv", label=None) -plotter.add_label("pedal-speed-accel relation (only pedal = 0.1)", - "pedal-speed", "accel") -plotter.set_lim(fig, [-0.4, 0.4], [-0.5, 1.0]) -plotter.show() -""" diff --git a/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py deleted file mode 100755 index a2d82a46..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ /dev/null @@ -1,367 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2022 Tier IV, Inc. -# -# 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. - -import math -from pathlib import Path - -from ament_index_python.packages import get_package_share_directory -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -import matplotlib.pyplot as plt -import numpy as np -from plotter import Plotter -import rclpy -from rclpy.node import Node -from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData -import yaml - - -class DrawGraph(Node): - calibrated_map_dir = "" - - def __init__(self): - super().__init__("plot_server") - - self.srv = self.create_service( - CalibData, "/accel_brake_map_calibrator/get_data_service", self.get_data_callback - ) - - default_map_path = get_package_share_directory("raw_vehicle_cmd_converter") - self.declare_parameter( - "/accel_brake_map_calibrator/csv_default_map_dir", default_map_path + "/data/default/" - ) - self.default_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") - .get_parameter_value() - .string_value - ) - - package_path = get_package_share_directory("accel_brake_map_calibrator") - self.declare_parameter( - "/accel_brake_map_calibrator/csv_calibrated_map_dir", package_path + "/config/" - ) - self.calibrated_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") - .get_parameter_value() - .string_value - ) - - self.declare_parameter("calibration_method", "each_cell") - self.calibration_method = ( - self.get_parameter("calibration_method").get_parameter_value().string_value - ) - if self.calibration_method is None: - self.calibration_method = "each_cell" - elif not ( - (self.calibration_method == "each_cell") | (self.calibration_method == "four_cell") - ): - print("invalid method.") - self.calibration_method = "each_cell" - - self.log_file = package_path + "/config/log.csv" - - config_file = package_path + "/config/accel_brake_map_calibrator.param.yaml" - if Path(config_file).exists(): - self.get_logger().info("config file exists") - with open(config_file) as yml: - data = yaml.safe_load(yml) - self.min_vel_thr = data["/**"]["ros__parameters"]["velocity_min_threshold"] - self.vel_diff_thr = data["/**"]["ros__parameters"]["velocity_diff_threshold"] - self.pedal_diff_thr = data["/**"]["ros__parameters"]["pedal_diff_threshold"] - self.max_steer_thr = data["/**"]["ros__parameters"]["max_steer_threshold"] - self.max_pitch_thr = data["/**"]["ros__parameters"]["max_pitch_threshold"] - self.max_jerk_thr = data["/**"]["ros__parameters"]["max_jerk_threshold"] - else: - self.get_logger().warning("config file is not found in {}".format(config_file)) - self.min_vel_thr = 0.1 - self.vel_diff_thr = 0.556 - self.pedal_diff_thr = 0.03 - self.max_steer_thr = 0.2 - self.max_pitch_thr = 0.02 - self.max_jerk_thr = 0.7 - - self.max_pedal_vel_thr = 0.7 - - # debug - self.get_logger().info("default map dir: {}".format(self.default_map_dir)) - self.get_logger().info("calibrated map dir: {}".format(self.calibrated_map_dir)) - self.get_logger().info("calibrated method: {}".format(self.calibration_method)) - self.get_logger().info("log file :{}".format(self.log_file)) - self.get_logger().info("min_vel_thr : {}".format(self.min_vel_thr)) - self.get_logger().info("vel_diff_thr : {}".format(self.vel_diff_thr)) - self.get_logger().info("pedal_diff_thr : {}".format(self.pedal_diff_thr)) - self.get_logger().info("max_steer_thr : {}".format(self.max_steer_thr)) - self.get_logger().info("max_pitch_thr : {}".format(self.max_pitch_thr)) - self.get_logger().info("max_jerk_thr : {}".format(self.max_jerk_thr)) - self.get_logger().info("max_pedal_vel_thr : {}".format(self.max_pedal_vel_thr)) - - def get_data_callback(self, request, response): - # read csv - # If log file doesn't exist, return empty data - if not Path(self.log_file).exists(): - response.graph_image = [] - self.get_logger().info("svg data is empty") - - response.accel_map = "" - self.get_logger().info("accel map is empty") - - response.brake_map = "" - self.get_logger().info("brake map is empty") - - return response - - self.cr = CSVReader(self.log_file, csv_type="file") - - # remove unused_data - self.csv_data = self.cr.removeUnusedData( - self.min_vel_thr, - self.max_steer_thr, - self.max_pitch_thr, - self.max_pedal_vel_thr, - self.max_jerk_thr, - ) - - # get statistics array - vel_data = self.cr.getVelData() - pedal_data = self.cr.getPedalData() - acc_data = self.cr.getAccData() - - # get color factor (pitch) array for plotting - color_data = self.cr.getPitchData() - - data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - self.vel_diff_thr, - CF.PEDAL_LIST, - self.pedal_diff_thr, - self.calibration_method, - ) - - count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) - velocity_map_list = [] - for i in range(len(CF.VEL_LIST)): - velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, i)) - - default_pedal_list, default_acc_list = self.load_map(self.default_map_dir) - if len(default_pedal_list) == 0 or len(default_acc_list) == 0: - self.get_logger().warning( - "No default map file was found in {}".format(self.default_map_dir) - ) - - response.graph_image = [] - self.get_logger().info("svg data is empty") - - response.accel_map = "" - self.get_logger().info("accel map is empty") - - response.brake_map = "" - self.get_logger().info("brake map is empty") - - return response - - calibrated_pedal_list, calibrated_acc_list = self.load_map(self.calibrated_map_dir) - if len(calibrated_pedal_list) == 0 or len(calibrated_acc_list) == 0: - self.get_logger().warning( - "No calibrated map file was found in {}".format(self.calibrated_map_dir) - ) - - # visualize point from data - plot_width = 3 - plot_height = int(math.ceil(len(CF.VEL_LIST) / float(plot_width))) - plotter = Plotter(plot_height, plot_width) - for i in range(len(CF.VEL_LIST)): - self.view_pedal_accel_graph( - plotter, - i, - velocity_map_list, - i, - count_map, - average_map, - stddev_map, - default_pedal_list, - default_acc_list, - calibrated_pedal_list, - calibrated_acc_list, - ) - plt.savefig("plot.svg") - self.get_logger().info("svg saved") - - # pack response data - text = Path("plot.svg").read_text() - if text == "": - response.graph_image = [] - self.get_logger().info("svg data is empty") - else: - byte = text.encode() - for b in byte: - response.graph_image.append(b) - self.get_logger().info("svg data is packed") - - accel_map_name = Path(self.calibrated_map_dir + "accel_map.csv") - if accel_map_name.exists(): - with open(self.calibrated_map_dir + "accel_map.csv", "r") as calibrated_accel_map: - for accel_data in calibrated_accel_map: - response.accel_map += accel_data - self.get_logger().info("accel map is packed") - else: - response.accel_map = "" - self.get_logger().info("accel map is empty") - - brake_map_name = Path(self.calibrated_map_dir + "brake_map.csv") - if brake_map_name.exists(): - with open(self.calibrated_map_dir + "brake_map.csv", "r") as calibrated_brake_map: - for brake_data in calibrated_brake_map: - response.brake_map += brake_data - self.get_logger().info("brake map is packed") - else: - response.brake_map = "" - self.get_logger().info("brake map is empty") - - return response - - def plotter_function(self): - return self.plotter - - def view_pedal_accel_graph( - self, - plotter, - subplot_num, - velocity_map_list, - vel_list_idx, - count_map, - average_map, - stddev_map, - default_pedal_list, - default_acc_list, - calibrated_pedal_list, - calibrated_acc_list, - ): - fig = plotter.subplot_more(subplot_num) - - # calibrated map - if len(calibrated_pedal_list) != 0 and len(calibrated_acc_list) != 0: - plotter.plot( - calibrated_pedal_list[vel_list_idx], - calibrated_acc_list[vel_list_idx], - color="blue", - label="calibrated", - ) - - # default map - if len(default_pedal_list) != 0 and len(default_acc_list) != 0: - plotter.plot( - default_pedal_list[vel_list_idx], - default_acc_list[vel_list_idx], - color="orange", - label="default", - linestyle="dashed", - ) - - # plot all data - pedal_list = [0 for i in range(len(CF.PEDAL_LIST))] - if velocity_map_list[vel_list_idx] is not None: - plotter.scatter_color( - velocity_map_list[vel_list_idx][:, 1], - velocity_map_list[vel_list_idx][:, 2], - color=velocity_map_list[vel_list_idx][:, 3], - label="all", - ) - - for pedal in velocity_map_list[vel_list_idx][:, 1]: - min_pedal = 10 - for pedal_idx, ref_pedal in enumerate(CF.PEDAL_LIST): - if min_pedal > abs(pedal - ref_pedal): - min_pedal = abs(pedal - ref_pedal) - min_pedal_idx = pedal_idx - pedal_list[min_pedal_idx] += 1 - - # plot average data - plotter.scatter(CF.PEDAL_LIST, average_map[:, vel_list_idx], "red", label="average") - - # add label of standard deviation - plotter.scatter([], [], "black", label="std dev") - - # plot average text - for i in range(len(CF.PEDAL_LIST)): - if count_map[i, vel_list_idx] == 0: - continue - x = CF.PEDAL_LIST[i] - y = average_map[i, vel_list_idx] - y2 = stddev_map[i, vel_list_idx] - # plot average - plotter.plot_text(x, y + 1, y, color="red") - - # plot standard deviation - plotter.plot_text(x, y - 1, y2, color="black") - - # plot the number of all data - plotter.plot_text( - x, y - 2, "{}\npts".format(pedal_list[i]), num_data_type="str", color="green" - ) - - pedal_lim = [CF.PEDAL_LIST[0] - 0.05, CF.PEDAL_LIST[-1] + 0.05] - accel_lim = [-5.0, 5.0] - - plotter.set_lim(fig, pedal_lim, accel_lim) - plotter.add_label( - str(CF.VEL_LIST[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" - ) - - def load_map(self, csv_dir): - try: - accel_pedal_list = [] - accel_acc_list = [] - with open(csv_dir + "accel_map.csv") as f: - for l_idx, l in enumerate(f.readlines()): - w = l.split(",") - w[-1] = w[-1][:-1] - if l_idx != 0: - accel_pedal_list.append([float(w[0]) for e in w[1:]]) - accel_acc_list.append([float(e) for e in w[1:]]) - - brake_pedal_list = [] - brake_acc_list = [] - with open(csv_dir + "brake_map.csv") as f: - for l_idx, l in enumerate(f.readlines()): - w = l.split(",") - w[-1] = w[-1][:-1] - if l_idx != 0: - brake_pedal_list.append([-float(w[0]) for e in w[1:]]) - brake_acc_list.append([float(e) for e in w[1:]]) - - return np.hstack( - [np.fliplr(np.array(accel_pedal_list).T), np.array(brake_pedal_list).T.tolist()] - ), np.hstack([np.fliplr(np.array(accel_acc_list).T), np.array(brake_acc_list).T]) - except OSError as e: - print(e) - return [], [] - - -def main(args=None): - rclpy.init(args=None) - node = DrawGraph() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/scripts/plotter.py b/vehicle/accel_brake_map_calibrator/scripts/plotter.py deleted file mode 100755 index 73e4cf54..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/plotter.py +++ /dev/null @@ -1,123 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import matplotlib.pyplot as plt -import numpy as np - - -class Plotter(object): - def __init__(self, total_height, total_width, plot=True): - self.total_height = total_height - self.total_width = total_width - self.plot_flag = True - self.fig = plt.figure(dpi=100, figsize=(24, 18)) - plt.subplots_adjust(left=0.04, right=0.98, bottom=0.05, top=0.95, wspace=0.1, hspace=0.4) - - def subplot(self, plot_num): - fig = plt.subplot(self.total_height, self.total_width, plot_num) - return fig - - def subplot_more(self, plot_num): - width = plot_num % self.total_width - height = int((plot_num - width) / self.total_width) - fig = plt.subplot2grid((self.total_height, self.total_width), (height, width)) - return fig - - def plot_1d(self, data, color, label): - plt.plot(data, color, label=label) - - def plot(self, x_data, y_data, color, label=None, linestyle="solid"): - plt.plot(x_data, y_data, color, label=label, linestyle=linestyle, zorder=-1) - - def scatter(self, x, y, color, label=None, colorbar=False): - plt.scatter(x, y, c=color, label=label, s=3) - - def scatter_color(self, x, y, color, cmap=None, label=None): - sc = plt.scatter(x, y, c=color, label=label, cmap=cmap, s=3) - plt.colorbar(sc) - - def plot_text(self, x, y, val, num_data_type="float", color="black"): - if num_data_type == "float": - plt.text( - x, - y, - "{0:.2f}".format(float(val)), - horizontalalignment="center", - verticalalignment="center", - color=color, - clip_on=True, - fontsize=8, - ) - elif num_data_type == "int": - plt.text( - x, - y, - "{0:d}".format(int(val)), - horizontalalignment="center", - verticalalignment="center", - color=color, - clip_on=True, - fontsize=8, - ) - elif num_data_type == "str": - plt.text( - x, - y, - val, - horizontalalignment="center", - verticalalignment="center", - color=color, - clip_on=True, - fontsize=8, - ) - - def imshow(self, data, left, right, ws, bottom, top, hs, num_data_type="float"): - wm = (right - left) / data.shape[1] - hm = (top - bottom) / data.shape[0] - hwm = wm / 2.0 - hhm = hm / 2.0 - hws = ws / 2.0 - hhs = hs / 2.0 - width_range = np.linspace(bottom - hhs + hhm, top + hhs - hhm, data.shape[0]) - height_range = np.linspace(left - hws + hwm, right + hws - hwm, data.shape[1]) - plt.imshow( - data, - extent=(left - hws, right + hws, bottom - hhs, top + hhs), - aspect="auto", - origin="lower", - ) - ys, xs = np.meshgrid(width_range, height_range, indexing="ij") - for x, y, val in zip(xs.flatten(), ys.flatten(), data.flatten()): - self.plot_text(x, y, val, num_data_type) - plt.colorbar() - - def set_lim(self, fig, xlim, ylim): - fig.set_xlim(xlim) - fig.set_ylim(ylim) - - def add_label(self, title, xlabel, ylabel): - plt.title(title) - plt.xlabel(xlabel) - plt.ylabel(ylabel) - plt.legend(loc="upper left", fontsize=6, labelspacing=0) - - def show(self): - if self.plot_flag: - plt.show() - - def save(self, file_name): - self.fig.savefig(file_name) diff --git a/vehicle/accel_brake_map_calibrator/scripts/view_plot.py b/vehicle/accel_brake_map_calibrator/scripts/view_plot.py deleted file mode 100755 index 4d713cf4..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/view_plot.py +++ /dev/null @@ -1,316 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -import argparse -import math - -from ament_index_python.packages import get_package_share_directory -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -import numpy as np -from plotter import Plotter -import rclpy -from rclpy.node import Node - - -class ViewPlot(Node): - def __init__(self, args): - super().__init__("plot_viewer") - default_map_dir = args.default_map_dir - calibrated_map_dir = args.calibrated_map_dir - calibration_method = args.method - scatter_only = args.scatter_only - log_file = args.log_file - min_vel_thr = args.min_vel_thr - vel_diff_thr = args.vel_diff_thr - pedal_diff_thr = args.pedal_diff_thr - max_steer_thr = args.max_steer_thr - max_pitch_thr = args.max_pitch_thr - max_jerk_thr = args.max_jerk_thr - max_pedal_vel_thr = args.max_pedal_vel_thr - - if default_map_dir is None: - package_path = get_package_share_directory("raw_vehicle_cmd_converter") - self.declare_parameter( - "/accel_brake_map_calibrator/csv_default_map_dir", package_path + "/data/default/" - ) - default_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_default_map_dir") - .get_parameter_value() - .string_value - ) - - if calibrated_map_dir is None: - package_path = get_package_share_directory("accel_brake_map_calibrator") - self.declare_parameter( - "/accel_brake_map_calibrator/csv_calibrated_map_dir", package_path + "/config/" - ) - calibrated_map_dir = ( - self.get_parameter("/accel_brake_map_calibrator/csv_calibrated_map_dir") - .get_parameter_value() - .string_value - ) - - if calibration_method is None: - calibration_method = "each_cell" - elif not ((calibration_method == "each_cell") | (calibration_method == "four_cell")): - print("invalid method.") - calibration_method = "each_cell" - - print("default map dir: {}".format(default_map_dir)) - print("calibrated map dir: {}".format(calibrated_map_dir)) - print("calibration method: {}".format(calibration_method)) - - # read csv - self.cr = CSVReader(log_file, csv_type="file") - - # remove unused_data - self.csv_data = self.cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr - ) - - # get statistics array - vel_data = self.cr.getVelData() - pedal_data = self.cr.getPedalData() - acc_data = self.cr.getAccData() - - # get color factor (pitch) array for plotting - color_data = self.cr.getPitchData() - - data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - vel_diff_thr, - CF.PEDAL_LIST, - pedal_diff_thr, - calibration_method, - ) - - count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) - velocity_map_list = [] - for i in range(len(CF.VEL_LIST)): - velocity_map_list.append(CalcUtils.extract_x_index_map(full_data, i)) - - default_pedal_list, default_acc_list = self.load_map(default_map_dir) - if len(default_pedal_list) == 0 or len(default_acc_list) == 0: - self.get_logger().warning("No default map file was found in {}".format(default_map_dir)) - - calibrated_pedal_list, calibrated_acc_list = self.load_map(calibrated_map_dir) - if len(calibrated_pedal_list) == 0 or len(calibrated_acc_list) == 0: - self.get_logger().warning( - "No calibrated map file was found in {}".format(calibrated_map_dir) - ) - - # visualize point from data - plot_width = 3 - plot_height = int(math.ceil(len(CF.VEL_LIST) / float(plot_width))) - plotter = Plotter(plot_height, plot_width) - for i in range(len(CF.VEL_LIST)): - self.view_pedal_accel_graph( - plotter, - i, - velocity_map_list, - i, - count_map, - average_map, - stddev_map, - default_pedal_list, - default_acc_list, - calibrated_pedal_list, - calibrated_acc_list, - scatter_only, - ) - plotter.show() - - def plotter_function(self): - return self.plotter - - def view_pedal_accel_graph( - self, - plotter, - subplot_num, - velocity_map_list, - vel_list_idx, - count_map, - average_map, - stddev_map, - default_pedal_list, - default_acc_list, - calibrated_pedal_list, - calibrated_acc_list, - scatter_only, - ): - fig = plotter.subplot_more(subplot_num) - - if not scatter_only: - # calibrated map - if len(calibrated_pedal_list) != 0 and len(calibrated_acc_list) != 0: - plotter.plot( - calibrated_pedal_list[vel_list_idx], - calibrated_acc_list[vel_list_idx], - color="blue", - label="calibrated", - ) - - # default map - if len(default_pedal_list) != 0 and len(default_acc_list) != 0: - plotter.plot( - default_pedal_list[vel_list_idx], - default_acc_list[vel_list_idx], - color="orange", - label="default", - linestyle="dashed", - ) - - # plot all data - pedal_list = [0 for i in range(len(CF.PEDAL_LIST))] - if velocity_map_list[vel_list_idx] is not None: - plotter.scatter_color( - velocity_map_list[vel_list_idx][:, 1], - velocity_map_list[vel_list_idx][:, 2], - color=velocity_map_list[vel_list_idx][:, 3], - label="all", - ) - - for pedal in velocity_map_list[vel_list_idx][:, 1]: - min_pedal = 10 - for pedal_idx, ref_pedal in enumerate(CF.PEDAL_LIST): - if min_pedal > abs(pedal - ref_pedal): - min_pedal = abs(pedal - ref_pedal) - min_pedal_idx = pedal_idx - pedal_list[min_pedal_idx] += 1 - - # plot average data - plotter.scatter(CF.PEDAL_LIST, average_map[:, vel_list_idx], "red", label="average") - - # add label of standard deviation - plotter.scatter([], [], "black", label="std dev") - - # plot average text - for i in range(len(CF.PEDAL_LIST)): - if count_map[i, vel_list_idx] == 0: - continue - x = CF.PEDAL_LIST[i] - y = average_map[i, vel_list_idx] - y2 = stddev_map[i, vel_list_idx] - # plot average - plotter.plot_text(x, y + 1, y, color="red") - - # plot standard deviation - plotter.plot_text(x, y - 1, y2, color="black") - - # plot the number of all data - plotter.plot_text( - x, y - 2, "{}\npts".format(pedal_list[i]), num_data_type="str", color="green" - ) - - pedal_lim = [CF.PEDAL_LIST[0] - 0.05, CF.PEDAL_LIST[-1] + 0.05] - accel_lim = [-5.0, 5.0] - - plotter.set_lim(fig, pedal_lim, accel_lim) - plotter.add_label( - str(CF.VEL_LIST[vel_list_idx]) + "kmh; pedal-accel relation", "pedal", "accel" - ) - - def load_map(self, csv_dir): - try: - accel_pedal_list = [] - accel_acc_list = [] - with open(csv_dir + "accel_map.csv") as f: - for l_idx, l in enumerate(f.readlines()): - w = l.split(",") - w[-1] = w[-1][:-1] - if l_idx != 0: - accel_pedal_list.append([float(w[0]) for e in w[1:]]) - accel_acc_list.append([float(e) for e in w[1:]]) - - brake_pedal_list = [] - brake_acc_list = [] - with open(csv_dir + "brake_map.csv") as f: - for l_idx, l in enumerate(f.readlines()): - w = l.split(",") - w[-1] = w[-1][:-1] - if l_idx != 0: - brake_pedal_list.append([-float(w[0]) for e in w[1:]]) - brake_acc_list.append([float(e) for e in w[1:]]) - - return np.hstack( - [np.fliplr(np.array(accel_pedal_list).T), np.array(brake_pedal_list).T.tolist()] - ), np.hstack([np.fliplr(np.array(accel_acc_list).T), np.array(brake_acc_list).T]) - except OSError as e: - print(e) - return [], [] - - -def main(args=None): - rclpy.init(args=None) - node = ViewPlot(args) - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - package_path = get_package_share_directory("accel_brake_map_calibrator") - parser = argparse.ArgumentParser() - parser.add_argument( - "-d", "--default-map-dir", default=None, type=str, help="directory of default map" - ) - parser.add_argument( - "-c", "--calibrated-map-dir", default=None, type=str, help="directory of calibrated map" - ) - parser.add_argument( - "-m", - "--method", - default=None, - type=str, - help="calibration method : each_cell or four_cell", - ) - parser.add_argument("-s", "--scatter-only", action="store_true", help="show only scatters") - parser.add_argument( - "-l", - "--log-file", - default=package_path + "/config/log.csv", - type=str, - help="path of log.csv", - ) - parser.add_argument( - "--min-vel-thr", default=0.1, type=float, help="valid min velocity threshold" - ) - parser.add_argument( - "--vel-diff-thr", default=0.556, type=float, help="valid velocity diff threshold" - ) - parser.add_argument( - "--pedal-diff-thr", default=0.03, type=float, help="valid pedal diff threshold" - ) - parser.add_argument( - "--max-steer-thr", default=0.2, type=float, help="valid max steer threshold" - ) - parser.add_argument( - "--max-pitch-thr", default=0.02, type=float, help="valid max pitch threshold" - ) - parser.add_argument("--max-jerk-thr", default=0.7, type=float, help="valid max jerk threshold") - parser.add_argument( - "--max-pedal-vel-thr", default=0.7, type=float, help="valid max pedal velocity threshold" - ) - - args = parser.parse_args() - main(args) diff --git a/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py b/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py deleted file mode 100755 index 83cfb869..00000000 --- a/vehicle/accel_brake_map_calibrator/scripts/view_statistics.py +++ /dev/null @@ -1,135 +0,0 @@ -#! /usr/bin/env python3 -# -*- coding: utf-8 -*- - -# Copyright 2021 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. - -from ament_index_python.packages import get_package_share_directory -from calc_utils import CalcUtils -import config as CF -from csv_reader import CSVReader -from plotter import Plotter -import rclpy -from rclpy.node import Node - - -class ViewPlot(Node): - def __init__(self): - super().__init__("statistics_viewer") - # get parameter - package_path = get_package_share_directory("accel_brake_map_calibrator") - self.declare_parameter("log_file", package_path + "/config/log.csv") - log_file = self.get_parameter("log_file").get_parameter_value().string_value - self.declare_parameter("velocity_min_threshold", 0.1) - min_vel_thr = ( - self.get_parameter("velocity_min_threshold").get_parameter_value().double_value - ) - self.declare_parameter("velocity_diff_threshold", 0.556) - vel_diff_thr = ( - self.get_parameter("velocity_diff_threshold").get_parameter_value().double_value - ) - self.declare_parameter("pedal_diff_threshold", 0.03) - pedal_diff_thr = ( - self.get_parameter("pedal_diff_threshold").get_parameter_value().double_value - ) - self.declare_parameter("max_steer_threshold", 0.2) - max_steer_thr = self.get_parameter("max_steer_threshold").get_parameter_value().double_value - self.declare_parameter("max_pitch_threshold", 0.02) - max_pitch_thr = self.get_parameter("max_pitch_threshold").get_parameter_value().double_value - self.declare_parameter("max_jerk_threshold", 0.7) - max_jerk_thr = self.get_parameter("max_jerk_threshold").get_parameter_value().double_value - self.declare_parameter("pedal_velocity_thresh", 0.15) - max_pedal_vel_thr = ( - self.get_parameter("pedal_velocity_thresh").get_parameter_value().double_value - ) - - # read csv - self.cr = CSVReader(log_file, csv_type="file") - # remove unused_data - self.csv_data = self.cr.removeUnusedData( - min_vel_thr, max_steer_thr, max_pitch_thr, max_pedal_vel_thr, max_jerk_thr - ) - - # get statistics array - vel_data = self.cr.getVelData() - pedal_data = self.cr.getPedalData() - acc_data = self.cr.getAccData() - - # get color factor (pitch) array for plotting - color_data = self.cr.getPitchData() - - data, full_data = CalcUtils.create_2d_map( - vel_data, - pedal_data, - acc_data, - color_data, - CF.VEL_LIST / 3.6, - vel_diff_thr, - CF.PEDAL_LIST, - pedal_diff_thr, - ) - - count_map, average_map, stddev_map = CalcUtils.create_stat_map(data) - - # visualization - plotter = Plotter(1, 3) - plotter.subplot(1) - plotter.imshow( - average_map, - CF.VEL_MIN, - CF.VEL_MAX, - CF.VEL_SPAN, - CF.PEDAL_MIN, - CF.PEDAL_MAX, - CF.PEDAL_SPAN, - ) - plotter.add_label("average of accel", "velocity(kmh)", "throttle") - - plotter.subplot(2) - plotter.imshow( - stddev_map, - CF.VEL_MIN, - CF.VEL_MAX, - CF.VEL_SPAN, - CF.PEDAL_MIN, - CF.PEDAL_MAX, - CF.PEDAL_SPAN, - ) - plotter.add_label("std. dev. of accel", "velocity(kmh)", "throttle") - - plotter.subplot(3) - plotter.imshow( - count_map, - CF.VEL_MIN, - CF.VEL_MAX, - CF.VEL_SPAN, - CF.PEDAL_MIN, - CF.PEDAL_MAX, - CF.PEDAL_SPAN, - num_data_type="int", - ) - plotter.add_label("number of accel data", "velocity(kmh)", "throttle") - plotter.show() - - -def main(args=None): - rclpy.init(args=args) - node = ViewPlot() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp deleted file mode 100644 index ecfc312d..00000000 --- a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ /dev/null @@ -1,1692 +0,0 @@ -// -// Copyright 2020 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 "accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp" - -#include "rclcpp/logging.hpp" - -#include -#include -#include -#include -#include -#include -#include - -namespace accel_brake_map_calibrator -{ - -AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & node_options) -: Node("accel_brake_map_calibrator", node_options) -{ - transform_listener_ = std::make_shared(this); - // get parameter - update_hz_ = declare_parameter("update_hz", 10.0); - covariance_ = declare_parameter("initial_covariance", 0.05); - velocity_min_threshold_ = declare_parameter("velocity_min_threshold", 0.1); - velocity_diff_threshold_ = declare_parameter("velocity_diff_threshold", 0.556); - pedal_diff_threshold_ = declare_parameter("pedal_diff_threshold", 0.03); - max_steer_threshold_ = declare_parameter("max_steer_threshold", 0.2); - max_pitch_threshold_ = declare_parameter("max_pitch_threshold", 0.02); - max_jerk_threshold_ = declare_parameter("max_jerk_threshold", 0.7); - pedal_velocity_thresh_ = declare_parameter("pedal_velocity_thresh", 0.15); - max_accel_ = declare_parameter("max_accel", 5.0); - min_accel_ = declare_parameter("min_accel", -5.0); - pedal_to_accel_delay_ = declare_parameter("pedal_to_accel_delay", 0.3); - max_data_count_ = declare_parameter("max_data_count", 200); - pedal_accel_graph_output_ = declare_parameter("pedal_accel_graph_output", false); - progress_file_output_ = declare_parameter("progress_file_output", false); - precision_ = declare_parameter("precision", 3); - const auto get_pitch_method_str = declare_parameter("get_pitch_method", std::string("tf")); - if (get_pitch_method_str == std::string("tf")) { - get_pitch_method_ = GET_PITCH_METHOD::TF; - } else if (get_pitch_method_str == std::string("none")) { - get_pitch_method_ = GET_PITCH_METHOD::NONE; - } else { - RCLCPP_ERROR_STREAM(get_logger(), "update_method_ is wrong. (available method: tf, file, none"); - return; - } - const auto accel_brake_value_source_str = - declare_parameter("accel_brake_value_source", std::string("status")); - if (accel_brake_value_source_str == std::string("status")) { - accel_brake_value_source_ = ACCEL_BRAKE_SOURCE::STATUS; - } else if (accel_brake_value_source_str == std::string("command")) { - accel_brake_value_source_ = ACCEL_BRAKE_SOURCE::COMMAND; - } else { - RCLCPP_ERROR_STREAM( - get_logger(), "accel_brake_value_source is wrong. (available source: status, command"); - return; - } - - update_suggest_thresh_ = declare_parameter("update_suggest_thresh", 0.7); - csv_calibrated_map_dir_ = declare_parameter("csv_calibrated_map_dir", std::string("")); - output_accel_file_ = csv_calibrated_map_dir_ + "/accel_map.csv"; - output_brake_file_ = csv_calibrated_map_dir_ + "/brake_map.csv"; - const std::string update_method_str = - declare_parameter("update_method", std::string("update_offset_each_cell")); - if (update_method_str == std::string("update_offset_each_cell")) { - update_method_ = UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL; - } else if (update_method_str == std::string("update_offset_total")) { - update_method_ = UPDATE_METHOD::UPDATE_OFFSET_TOTAL; - } else if (update_method_str == std::string("update_offset_four_cell_around")) { - update_method_ = UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND; - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "update_method is wrong. (available method: update_offset_each_cell, update_offset_total"); - return; - } - // initializer - - // QoS setup - static constexpr std::size_t queue_size = 1; - rclcpp::QoS durable_qos(queue_size); - - // Publisher for checkUpdateSuggest - calibration_status_pub_ = create_publisher( - "/accel_brake_map_calibrator/output/calibration_status", durable_qos); - - /* Diagnostic Updater */ - updater_ptr_ = std::make_shared(this, 1.0 / update_hz_); - updater_ptr_->setHardwareID("accel_brake_map_calibrator"); - updater_ptr_->add( - "accel_brake_map_calibrator", this, &AccelBrakeMapCalibrator::checkUpdateSuggest); - - { - csv_default_map_dir_ = declare_parameter("csv_default_map_dir", std::string("")); - - std::string csv_path_accel_map = csv_default_map_dir_ + "/accel_map.csv"; - std::string csv_path_brake_map = csv_default_map_dir_ + "/brake_map.csv"; - if ( - !accel_map_.readAccelMapFromCSV(csv_path_accel_map) || - !new_accel_map_.readAccelMapFromCSV(csv_path_accel_map)) { - is_default_map_ = false; - RCLCPP_ERROR_STREAM( - get_logger(), - "Cannot read accelmap. csv path = " << csv_path_accel_map.c_str() << ". stop calculation."); - return; - } - if ( - !brake_map_.readBrakeMapFromCSV(csv_path_brake_map) || - !new_brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) { - is_default_map_ = false; - RCLCPP_ERROR_STREAM( - get_logger(), - "Cannot read brakemap. csv path = " << csv_path_brake_map.c_str() << ". stop calculation."); - return; - } - } - - std::string output_log_file = declare_parameter("output_log_file", std::string("")); - output_log_.open(output_log_file); - addIndexToCSV(&output_log_); - - debug_values_.data.resize(num_debug_values_); - - // input map info - accel_map_value_ = accel_map_.getAccelMap(); - brake_map_value_ = brake_map_.getBrakeMap(); - accel_vel_index_ = accel_map_.getVelIdx(); - brake_vel_index_ = brake_map_.getVelIdx(); - accel_pedal_index_ = accel_map_.getThrottleIdx(); - brake_pedal_index_ = brake_map_.getBrakeIdx(); - update_accel_map_value_.resize((accel_map_value_.size())); - update_brake_map_value_.resize((brake_map_value_.size())); - for (auto & m : update_accel_map_value_) { - m.resize(accel_map_value_.at(0).size()); - } - for (auto & m : update_brake_map_value_) { - m.resize(brake_map_value_.at(0).size()); - } - accel_offset_covariance_value_.resize((accel_map_value_.size())); - brake_offset_covariance_value_.resize((brake_map_value_.size())); - for (auto & m : accel_offset_covariance_value_) { - m.resize(accel_map_value_.at(0).size(), covariance_); - } - for (auto & m : brake_offset_covariance_value_) { - m.resize(brake_map_value_.at(0).size(), covariance_); - } - map_value_data_.resize(accel_map_value_.size() + brake_map_value_.size() - 1); - for (auto & m : map_value_data_) { - m.resize(accel_map_value_.at(0).size()); - } - - std::copy(accel_map_value_.begin(), accel_map_value_.end(), update_accel_map_value_.begin()); - std::copy(brake_map_value_.begin(), brake_map_value_.end(), update_brake_map_value_.begin()); - - // initialize matrix for covariance calculation - { - const auto genConstMat = [](const Map & map, const auto val) { - return Eigen::MatrixXd::Constant(map.size(), map.at(0).size(), val); - }; - accel_data_mean_mat_ = genConstMat(accel_map_value_, map_offset_); - brake_data_mean_mat_ = genConstMat(brake_map_value_, map_offset_); - accel_data_covariance_mat_ = genConstMat(accel_map_value_, covariance_); - brake_data_covariance_mat_ = genConstMat(brake_map_value_, covariance_); - accel_data_num_ = genConstMat(accel_map_value_, 1); - brake_data_num_ = genConstMat(brake_map_value_, 1); - } - - // publisher - update_suggest_pub_ = - create_publisher("~/output/update_suggest", durable_qos); - original_map_occ_pub_ = create_publisher("~/debug/original_occ_map", durable_qos); - update_map_occ_pub_ = create_publisher("~/debug/update_occ_map", durable_qos); - data_ave_pub_ = create_publisher("~/debug/data_average_occ_map", durable_qos); - data_std_pub_ = create_publisher("~/debug/data_std_dev_occ_map", durable_qos); - data_count_pub_ = create_publisher("~/debug/data_count_occ_map", durable_qos); - data_count_with_self_pose_pub_ = - create_publisher("~/debug/data_count_self_pose_occ_map", durable_qos); - index_pub_ = create_publisher("~/debug/occ_index", durable_qos); - original_map_raw_pub_ = - create_publisher("~/debug/original_raw_map", durable_qos); - update_map_raw_pub_ = create_publisher("~/output/update_raw_map", durable_qos); - debug_pub_ = create_publisher("~/output/debug_values", durable_qos); - current_map_error_pub_ = - create_publisher("~/output/current_map_error", durable_qos); - updated_map_error_pub_ = - create_publisher("~/output/updated_map_error", durable_qos); - map_error_ratio_pub_ = create_publisher("~/output/map_error_ratio", durable_qos); - offset_covariance_pub_ = - create_publisher("~/debug/offset_covariance", durable_qos); - - // subscriber - using std::placeholders::_1; - using std::placeholders::_2; - using std::placeholders::_3; - - velocity_sub_ = create_subscription( - "~/input/velocity", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackVelocity, this, _1)); - steer_sub_ = create_subscription( - "~/input/steer", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackSteer, this, _1)); - if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { - actuation_status_sub_ = create_subscription( - "~/input/actuation_status", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackActuationStatus, this, _1)); - } - if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) { - actuation_cmd_sub_ = create_subscription( - "~/input/actuation_cmd", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackActuationCommand, this, _1)); - } - - // Service - update_map_dir_server_ = create_service( - "~/input/update_map_dir", - std::bind(&AccelBrakeMapCalibrator::callbackUpdateMapService, this, _1, _2, _3)); - - // timer - initTimer(1.0 / update_hz_); - initOutputCSVTimer(30.0); - - logger_configure_ = std::make_unique(this); -} - -void AccelBrakeMapCalibrator::initOutputCSVTimer(double period_s) -{ - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_output_csv_ = rclcpp::create_timer( - this, get_clock(), period_ns, - std::bind(&AccelBrakeMapCalibrator::timerCallbackOutputCSV, this)); -} - -void AccelBrakeMapCalibrator::initTimer(double period_s) -{ - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(period_s)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&AccelBrakeMapCalibrator::timerCallback, this)); -} - -bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) -{ - if (get_pitch_method_ == GET_PITCH_METHOD::NONE) { - // do not get pitch from tf - *pitch = 0.0; - return true; - } - - // get tf - const auto transform = transform_listener_->getTransform( - "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); - if (!transform) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, "cannot get map to base_link transform. "); - return false; - } - double roll, raw_pitch, yaw; - tf2::getEulerYPR(transform->transform.rotation, roll, raw_pitch, yaw); - debug_values_.data.at(CURRENT_RAW_PITCH) = raw_pitch; - *pitch = lowpass(*pitch, raw_pitch, 0.2); - debug_values_.data.at(CURRENT_PITCH) = *pitch; - return true; -} - -void AccelBrakeMapCalibrator::timerCallback() -{ - update_count_++; - - RCLCPP_DEBUG_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, - "map updating... count: " << update_success_count_ << " / " << update_count_ << "\n\t" - << "lack_of_data_count: " << lack_of_data_count_ << "\n\t" - << " failed_to_get_pitch_count: " << failed_to_get_pitch_count_ - << "\n\t" - << "too_large_pitch_count: " << too_large_pitch_count_ << "\n\t" - << " too_low_speed_count: " << too_low_speed_count_ << "\n\t" - << "too_large_steer_count: " << too_large_steer_count_ << "\n\t" - << "too_large_jerk_count: " << too_large_jerk_count_ << "\n\t" - << "invalid_acc_brake_count: " << invalid_acc_brake_count_ << "\n\t" - << "too_large_pedal_spd_count: " << too_large_pedal_spd_count_ - << "\n\t" - << "update_fail_count_: " << update_fail_count_ << "\n"); - - /* valid check */ - - // data check - if ( - !twist_ptr_ || !steer_ptr_ || !accel_pedal_ptr_ || !brake_pedal_ptr_ || - !delayed_accel_pedal_ptr_ || !delayed_brake_pedal_ptr_) { - // lack of data - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, "lack of topics (twist, steer, accel, brake)"); - lack_of_data_count_++; - return; - } - - // data check 2 - if ( - isTimeout(twist_ptr_->header.stamp, timeout_sec_) || - isTimeout(steer_ptr_->stamp, timeout_sec_) || isTimeout(accel_pedal_ptr_, timeout_sec_) || - isTimeout(brake_pedal_ptr_, timeout_sec_)) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, "timeout of topics (twist, steer, accel, brake)"); - lack_of_data_count_++; - return; - } - - // data check 3 - if (!getCurrentPitchFromTF(&pitch_)) { - // cannot get pitch - RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot get pitch"); - failed_to_get_pitch_count_++; - return; - } - - /* write data to log */ - if (pedal_accel_graph_output_) { - addLogToCSV( - &output_log_, rclcpp::Time(twist_ptr_->header.stamp).seconds(), twist_ptr_->twist.linear.x, - acceleration_, getPitchCompensatedAcceleration(), delayed_accel_pedal_ptr_->data, - delayed_brake_pedal_ptr_->data, accel_pedal_speed_, brake_pedal_speed_, pitch_, - steer_ptr_->steering_tire_angle, jerk_, full_original_accel_rmse_, part_original_accel_rmse_, - new_accel_rmse_); - } - - /* publish map & debug_values*/ - publishMap(accel_map_value_, brake_map_value_, "original"); - publishMap(update_accel_map_value_, update_brake_map_value_, "update"); - publishOffsetCovMap(accel_offset_covariance_value_, brake_offset_covariance_value_); - publishCountMap(); - publishIndex(); - publishUpdateSuggestFlag(); - debug_pub_->publish(debug_values_); - publishFloat32("current_map_error", part_original_accel_rmse_); - publishFloat32("updated_map_error", new_accel_rmse_); - publishFloat32( - "map_error_ratio", - part_original_accel_rmse_ != 0.0 ? new_accel_rmse_ / part_original_accel_rmse_ : 1.0); - - // -- processing start -- - - /* initialize */ - debug_values_.data.at(SUCCESS_TO_UPDATE) = false; - update_success_ = false; - - // twist check - if (twist_ptr_->twist.linear.x < velocity_min_threshold_) { - // too low speed ( or backward velocity) - too_low_speed_count_++; - return; - } - - // accel / brake map evaluation (do not evaluate when the car stops) - executeEvaluation(); - - // pitch check - if (std::fabs(pitch_) > max_pitch_threshold_) { - // too large pitch - too_large_pitch_count_++; - return; - } - - // steer check - if (std::fabs(steer_ptr_->steering_tire_angle) > max_steer_threshold_) { - // too large steer - too_large_steer_count_++; - return; - } - - // jerk check - if (std::fabs(jerk_) > max_jerk_threshold_) { - // too large jerk - too_large_jerk_count_++; - return; - } - - // pedal check - if ( - delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon() && - delayed_brake_pedal_ptr_->data > std::numeric_limits::epsilon()) { - // both (accel/brake) output - RCLCPP_DEBUG_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, - "invalid pedal value (Both of accel output and brake output area not zero. )"); - invalid_acc_brake_count_++; - return; - } - - // pedal speed check - if ( - std::fabs(accel_pedal_speed_) > pedal_velocity_thresh_ || - std::fabs(brake_pedal_speed_) > pedal_velocity_thresh_) { - // too large pedal speed - too_large_pedal_spd_count_++; - return; - } - - /* update map */ - if (updateAccelBrakeMap()) { - update_success_count_++; - debug_values_.data.at(SUCCESS_TO_UPDATE) = true; - update_success_ = true; - } else { - update_fail_count_++; - } -} - -void AccelBrakeMapCalibrator::timerCallbackOutputCSV() -{ - // write accel/ brake map to file - const auto ros_time = std::to_string(this->now().seconds()); - writeMapToCSV(accel_vel_index_, accel_pedal_index_, update_accel_map_value_, output_accel_file_); - writeMapToCSV(brake_vel_index_, brake_pedal_index_, update_brake_map_value_, output_brake_file_); - if (progress_file_output_) { - writeMapToCSV( - accel_vel_index_, accel_pedal_index_, update_accel_map_value_, - output_accel_file_ + "_" + ros_time); - writeMapToCSV( - brake_vel_index_, brake_pedal_index_, update_brake_map_value_, - output_brake_file_ + "_" + ros_time); - writeMapToCSV( - accel_vel_index_, accel_pedal_index_, accel_map_value_, output_accel_file_ + "_original"); - writeMapToCSV( - brake_vel_index_, brake_pedal_index_, brake_map_value_, output_brake_file_ + "_original"); - } - - // update newest accel / brake map - // check file existence - std::ifstream af(output_accel_file_); - std::ifstream bf(output_brake_file_); - if (!af.is_open() || !bf.is_open()) { - RCLCPP_WARN(get_logger(), "Accel map or brake map does not exist"); - return; - } - - new_accel_map_ = AccelMap(); - if (!new_accel_map_.readAccelMapFromCSV(output_accel_file_)) { - RCLCPP_WARN(get_logger(), "Cannot read accelmap. csv path = %s. ", output_accel_file_.c_str()); - } - new_brake_map_ = BrakeMap(); - if (!new_brake_map_.readBrakeMapFromCSV(output_brake_file_)) { - RCLCPP_WARN(get_logger(), "Cannot read brakemap. csv path = %s. ", output_brake_file_.c_str()); - } -} - -void AccelBrakeMapCalibrator::callbackVelocity(const VelocityReport::ConstSharedPtr msg) -{ - // convert velocity-report to twist-stamped - auto twist_msg = std::make_shared(); - twist_msg->header = msg->header; - twist_msg->twist.linear.x = msg->longitudinal_velocity; - twist_msg->twist.linear.y = msg->lateral_velocity; - twist_msg->twist.angular.z = msg->heading_rate; - - if (!twist_vec_.empty()) { - const auto past_msg = getNearestTimeDataFromVec(twist_msg, dif_twist_time_, twist_vec_); - const double raw_acceleration = getAccel(past_msg, twist_msg); - acceleration_ = lowpass(acceleration_, raw_acceleration, 0.25); - acceleration_time_ = rclcpp::Time(msg->header.stamp).seconds(); - debug_values_.data.at(CURRENT_RAW_ACCEL) = raw_acceleration; - debug_values_.data.at(CURRENT_ACCEL) = acceleration_; - - // calculate jerk - if ( - this->now().seconds() - pre_acceleration_time_ > timeout_sec_ || - (acceleration_time_ - pre_acceleration_time_) <= std::numeric_limits::epsilon()) { - RCLCPP_DEBUG_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot calculate jerk"); - // does not update jerk - } else { - const double raw_jerk = getJerk(); - // to avoid to get delayed jerk, set high gain - jerk_ = lowpass(jerk_, raw_jerk, 0.5); - } - debug_values_.data.at(CURRENT_JERK) = jerk_; - pre_acceleration_ = acceleration_; - pre_acceleration_time_ = acceleration_time_; - } - - debug_values_.data.at(CURRENT_SPEED) = twist_msg->twist.linear.x; - twist_ptr_ = twist_msg; - pushDataToVec(twist_msg, twist_vec_max_size_, &twist_vec_); -} - -void AccelBrakeMapCalibrator::callbackSteer(const SteeringReport::ConstSharedPtr msg) -{ - debug_values_.data.at(CURRENT_STEER) = msg->steering_tire_angle; - steer_ptr_ = msg; -} - -void AccelBrakeMapCalibrator::callbackActuation( - const std_msgs::msg::Header header, const double accel, const double brake) -{ - // get accel data - accel_pedal_ptr_ = std::make_shared(accel, rclcpp::Time(header.stamp)); - if (!accel_pedal_vec_.empty()) { - const auto past_accel_ptr = - getNearestTimeDataFromVec(accel_pedal_ptr_, dif_pedal_time_, accel_pedal_vec_); - const double raw_accel_pedal_speed = - getPedalSpeed(past_accel_ptr, accel_pedal_ptr_, accel_pedal_speed_); - accel_pedal_speed_ = lowpass(accel_pedal_speed_, raw_accel_pedal_speed, 0.5); - debug_values_.data.at(CURRENT_RAW_ACCEL_SPEED) = raw_accel_pedal_speed; - debug_values_.data.at(CURRENT_ACCEL_SPEED) = accel_pedal_speed_; - } - debug_values_.data.at(CURRENT_ACCEL_PEDAL) = accel_pedal_ptr_->data; - pushDataToVec(accel_pedal_ptr_, pedal_vec_max_size_, &accel_pedal_vec_); - delayed_accel_pedal_ptr_ = - getNearestTimeDataFromVec(accel_pedal_ptr_, pedal_to_accel_delay_, accel_pedal_vec_); - - // get brake data - brake_pedal_ptr_ = std::make_shared(brake, rclcpp::Time(header.stamp)); - if (!brake_pedal_vec_.empty()) { - const auto past_brake_ptr = - getNearestTimeDataFromVec(brake_pedal_ptr_, dif_pedal_time_, brake_pedal_vec_); - const double raw_brake_pedal_speed = - getPedalSpeed(past_brake_ptr, brake_pedal_ptr_, brake_pedal_speed_); - brake_pedal_speed_ = lowpass(brake_pedal_speed_, raw_brake_pedal_speed, 0.5); - debug_values_.data.at(CURRENT_RAW_BRAKE_SPEED) = raw_brake_pedal_speed; - debug_values_.data.at(CURRENT_BRAKE_SPEED) = brake_pedal_speed_; - } - debug_values_.data.at(CURRENT_BRAKE_PEDAL) = brake_pedal_ptr_->data; - pushDataToVec(brake_pedal_ptr_, pedal_vec_max_size_, &brake_pedal_vec_); - delayed_brake_pedal_ptr_ = - getNearestTimeDataFromVec(brake_pedal_ptr_, pedal_to_accel_delay_, brake_pedal_vec_); -} - -void AccelBrakeMapCalibrator::callbackActuationCommand( - const ActuationCommandStamped::ConstSharedPtr msg) -{ - const auto header = msg->header; - const auto accel = msg->actuation.accel_cmd; - const auto brake = msg->actuation.brake_cmd; - callbackActuation(header, accel, brake); -} - -void AccelBrakeMapCalibrator::callbackActuationStatus( - const ActuationStatusStamped::ConstSharedPtr msg) -{ - const auto header = msg->header; - const auto accel = msg->status.accel_status; - const auto brake = msg->status.brake_status; - callbackActuation(header, accel, brake); -} - -bool AccelBrakeMapCalibrator::callbackUpdateMapService( - [[maybe_unused]] const std::shared_ptr request_header, - UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res) -{ - // if req.path is input, use this as the directory to set updated maps - std::string update_map_dir = req->path.empty() ? csv_default_map_dir_ : req->path; - - RCLCPP_INFO_STREAM(get_logger(), "update accel/brake map. directory: " << update_map_dir); - const auto accel_map_file = update_map_dir + "/accel_map.csv"; - const auto brake_map_file = update_map_dir + "/brake_map.csv"; - if ( - writeMapToCSV(accel_vel_index_, accel_pedal_index_, update_accel_map_value_, accel_map_file) && - writeMapToCSV(brake_vel_index_, brake_pedal_index_, update_brake_map_value_, brake_map_file)) { - res->success = true; - res->message = - "Data has been successfully saved on " + update_map_dir + "/(accel/brake)_map.csv"; - } else { - res->success = false; - res->message = "Failed to save data. Maybe invalid path?"; - } - return true; -} - -double AccelBrakeMapCalibrator::lowpass( - const double original, const double current, const double gain) -{ - return current * gain + original * (1.0 - gain); -} - -double AccelBrakeMapCalibrator::getPedalSpeed( - const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, - const double prev_pedal_speed) -{ - const double dt = (current_pedal->data_time - prev_pedal->data_time).seconds(); - if (dt < 1e-03) { - // invalid pedal info. return prev pedal speed - return prev_pedal_speed; - } - - const double d_pedal = current_pedal->data - prev_pedal->data; - return d_pedal / dt; -} - -double AccelBrakeMapCalibrator::getAccel( - const TwistStamped::ConstSharedPtr & prev_twist, - const TwistStamped::ConstSharedPtr & current_twist) -{ - const double dt = - (rclcpp::Time(current_twist->header.stamp) - rclcpp::Time(prev_twist->header.stamp)).seconds(); - if (dt < 1e-03) { - // invalid twist. return prev acceleration - return acceleration_; - } - const double dv = current_twist->twist.linear.x - prev_twist->twist.linear.x; - return std::min(std::max(min_accel_, dv / dt), max_accel_); -} - -double AccelBrakeMapCalibrator::getJerk() -{ - const double jerk = - (acceleration_ - pre_acceleration_) / (acceleration_time_ - pre_acceleration_time_); - return std::min(std::max(-max_jerk_, jerk), max_jerk_); -} - -bool AccelBrakeMapCalibrator::indexValueSearch( - const std::vector value_index, const double value, const double value_thresh, - int * searched_index) -{ - if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - /* return lower left index. - +-------+ +:grid - | * | *:given data - | | o:index to return - o-------+ - */ - if (value < 0) { - std::cerr << "error : invalid value." << std::endl; - return false; - } - double old_diff_value = -999; // TODO(Hirano) - for (std::size_t i = 0; i < value_index.size(); i++) { - const double diff_value = value_index.at(i) - value; - if (diff_value <= 0 && old_diff_value < diff_value) { - *searched_index = i; - old_diff_value = diff_value; - } else { - return true; - } - } - } else { - for (std::size_t i = 0; i < value_index.size(); i++) { - const double diff_value = std::fabs(value_index.at(i) - value); - if (diff_value <= value_thresh) { - *searched_index = i; - return true; - } - } - } - return false; -} - -int AccelBrakeMapCalibrator::nearestValueSearch( - const std::vector value_index, const double value) -{ - double max_dist = std::numeric_limits::max(); - int nearest_idx = 0; - - for (std::size_t i = 0; i < value_index.size(); i++) { - const double dist = std::fabs(value - value_index.at(i)); - if (max_dist > dist) { - nearest_idx = i; - max_dist = dist; - } - } - return nearest_idx; -} - -int AccelBrakeMapCalibrator::nearestPedalSearch() -{ - bool accel_mode; - - if (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()) { - accel_mode = true; - } else { - accel_mode = false; - } - - int nearest_idx; - if (accel_mode) { - nearest_idx = nearestValueSearch(accel_pedal_index_, delayed_accel_pedal_ptr_->data); - } else { - nearest_idx = nearestValueSearch(brake_pedal_index_, delayed_brake_pedal_ptr_->data); - } - - return getUnifiedIndexFromAccelBrakeIndex(accel_mode, nearest_idx); -} - -int AccelBrakeMapCalibrator::nearestVelSearch() -{ - const double current_vel = twist_ptr_->twist.linear.x; - return nearestValueSearch(accel_vel_index_, current_vel); -} - -void AccelBrakeMapCalibrator::takeConsistencyOfAccelMap() -{ - const double bit = std::pow(1e-01, precision_); - for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_accel_map_value_.at(0).size() - 1; vel_idx++) { - const double current_acc = update_accel_map_value_.at(ped_idx).at(vel_idx); - const double next_ped_acc = update_accel_map_value_.at(ped_idx + 1).at(vel_idx); - const double next_vel_acc = update_accel_map_value_.at(ped_idx).at(vel_idx + 1); - - if (current_acc <= next_vel_acc) { - // the higher the velocity, the lower the acceleration - update_accel_map_value_.at(ped_idx).at(vel_idx + 1) = current_acc - bit; - } - - if (current_acc >= next_ped_acc) { - // the higher the accel pedal, the higher the acceleration - update_accel_map_value_.at(ped_idx + 1).at(vel_idx) = current_acc + bit; - } - } - } -} - -void AccelBrakeMapCalibrator::takeConsistencyOfBrakeMap() -{ - const double bit = std::pow(1e-01, precision_); - for (std::size_t ped_idx = 0; ped_idx < update_brake_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_brake_map_value_.at(0).size() - 1; vel_idx++) { - const double current_acc = update_brake_map_value_.at(ped_idx).at(vel_idx); - const double next_ped_acc = update_brake_map_value_.at(ped_idx + 1).at(vel_idx); - const double next_vel_acc = update_brake_map_value_.at(ped_idx).at(vel_idx + 1); - - if (current_acc <= next_vel_acc) { - // the higher the velocity, the lower the acceleration - update_brake_map_value_.at(ped_idx).at(vel_idx + 1) = current_acc - bit; - } - - if (current_acc <= next_ped_acc) { - // the higher the brake pedal, the lower the acceleration - update_brake_map_value_.at(ped_idx + 1).at(vel_idx) = current_acc - bit; - } - } - } -} - -bool AccelBrakeMapCalibrator::updateAccelBrakeMap() -{ - // get pedal index - bool accel_mode = false; - int accel_pedal_index = 0; - int brake_pedal_index = 0; - int accel_vel_index = 0; - int brake_vel_index = 0; - - if (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()) { - accel_mode = true; - } else { - accel_mode = false; - } - - if ( - accel_mode && !indexValueSearch( - accel_pedal_index_, delayed_accel_pedal_ptr_->data, pedal_diff_threshold_, - &accel_pedal_index)) { - // not match accel pedal output to pedal value in index - return false; - } - - if ( - !accel_mode && !indexValueSearch( - brake_pedal_index_, delayed_brake_pedal_ptr_->data, pedal_diff_threshold_, - &brake_pedal_index)) { - // not match accel pedal output to pedal value in index - return false; - } - - if ( - accel_mode && - !indexValueSearch( - accel_vel_index_, twist_ptr_->twist.linear.x, velocity_diff_threshold_, &accel_vel_index)) { - // not match current velocity to velocity value in index - return false; - } - - if ( - !accel_mode && - !indexValueSearch( - brake_vel_index_, twist_ptr_->twist.linear.x, velocity_diff_threshold_, &brake_vel_index)) { - // not match current velocity to velocity value in index - return false; - } - - // update map - executeUpdate(accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index); - - // when update 0 pedal index, update another map - if (accel_mode && accel_pedal_index == 0) { - // copy accel map value to brake map value - update_brake_map_value_.at(accel_pedal_index).at(accel_vel_index) = - update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index); - if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - update_brake_map_value_.at(accel_pedal_index).at(accel_vel_index + 1) = - update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index + 1); - } - } else if (!accel_mode && brake_pedal_index == 0) { - // copy brake map value to accel map value - update_accel_map_value_.at(brake_pedal_index).at(brake_vel_index) = - update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index); - if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - update_accel_map_value_.at(brake_pedal_index).at(brake_vel_index + 1) = - update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index + 1); - } - } - - // take consistency of map - takeConsistencyOfAccelMap(); - takeConsistencyOfBrakeMap(); - - return true; -} - -void AccelBrakeMapCalibrator::executeUpdate( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index) -{ - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); - const double map_acc = accel_mode - ? update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) - : update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index); - RCLCPP_DEBUG_STREAM(get_logger(), "measured_acc: " << measured_acc << ", map_acc: " << map_acc); - - if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL) { - updateEachValOffset( - accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index, - measured_acc, map_acc); - } else if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_TOTAL) { - updateTotalMapOffset(measured_acc, map_acc); - } else if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - updateFourCellAroundOffset( - accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index, - measured_acc); - } - - // add accel data to map - accel_mode ? map_value_data_.at(getUnifiedIndexFromAccelBrakeIndex(true, accel_pedal_index)) - .at(accel_vel_index) - .emplace_back(measured_acc) - : map_value_data_.at(getUnifiedIndexFromAccelBrakeIndex(false, brake_pedal_index)) - .at(brake_vel_index) - .emplace_back(measured_acc); -} - -bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index, const double measured_acc) -{ - // pre-defined - static Map accel_map_offset_vec_( - accel_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); - static Map brake_map_offset_vec_( - brake_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); - static std::vector> accel_covariance_mat_( - accel_map_value_.size() - 1, - std::vector( - accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); - static std::vector> brake_covariance_mat_( - brake_map_value_.size() - 1, - std::vector( - accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); - - auto & update_map_value = accel_mode ? update_accel_map_value_ : update_brake_map_value_; - auto & offset_covariance_value = - accel_mode ? accel_offset_covariance_value_ : brake_offset_covariance_value_; - const auto & map_value = accel_mode ? accel_map_value_ : brake_map_value_; - const auto & pedal_index = accel_mode ? accel_pedal_index : brake_pedal_index; - const auto & pedal_index_ = accel_mode ? accel_pedal_index_ : brake_pedal_index_; - const auto & vel_index = accel_mode ? accel_vel_index : brake_vel_index; - const auto & vel_index_ = accel_mode ? accel_vel_index_ : brake_vel_index_; - const auto & delayed_pedal = - accel_mode ? delayed_accel_pedal_ptr_->data : delayed_brake_pedal_ptr_->data; - auto & map_offset_vec = accel_mode ? accel_map_offset_vec_ : brake_map_offset_vec_; - auto & covariance_mat = accel_mode ? accel_covariance_mat_ : brake_covariance_mat_; - auto & data_mean_mat = accel_mode ? accel_data_mean_mat_ : brake_data_mean_mat_; - auto & data_covariance_mat = accel_mode ? accel_data_covariance_mat_ : brake_data_covariance_mat_; - auto & data_weighted_num = accel_mode ? accel_data_num_ : brake_data_num_; - - const double zll = update_map_value.at(pedal_index + 0).at(vel_index + 0); - const double zhl = update_map_value.at(pedal_index + 1).at(vel_index + 0); - const double zlh = update_map_value.at(pedal_index + 0).at(vel_index + 1); - const double zhh = update_map_value.at(pedal_index + 1).at(vel_index + 1); - - const double xl = pedal_index_.at(pedal_index + 0); - const double xh = pedal_index_.at(pedal_index + 1); - const double rx = (delayed_pedal - xl) / (xh - xl); - const double yl = vel_index_.at(vel_index + 0); - const double yh = vel_index_.at(vel_index + 1); - const double ry = (twist_ptr_->twist.linear.x - yl) / (yh - yl); - - Eigen::Vector4d phi(4); - phi << (1 - rx) * (1 - ry), rx * (1 - ry), (1 - rx) * ry, rx * ry; - - Eigen::Vector4d theta(4); - theta << zll, zhl, zlh, zhh; - - Eigen::Vector4d weighted_sum(4); - weighted_sum << data_weighted_num(pedal_index + 0, vel_index + 0), - data_weighted_num(pedal_index + 1, vel_index + 0), - data_weighted_num(pedal_index + 0, vel_index + 1), - data_weighted_num(pedal_index + 1, vel_index + 1); - - Eigen::Vector4d sigma(4); - sigma << data_covariance_mat(pedal_index + 0, vel_index + 0), - data_covariance_mat(pedal_index + 1, vel_index + 0), - data_covariance_mat(pedal_index + 0, vel_index + 1), - data_covariance_mat(pedal_index + 1, vel_index + 1); - - Eigen::Vector4d mean(4); - mean << data_mean_mat(pedal_index + 0, vel_index + 0), - data_mean_mat(pedal_index + 1, vel_index + 0), data_mean_mat(pedal_index + 0, vel_index + 1), - data_mean_mat(pedal_index + 1, vel_index + 1); - - const int vel_idx_l = vel_index + 0; - const int vel_idx_h = vel_index + 1; - const int ped_idx_l = pedal_index + 0; - const int ped_idx_h = pedal_index + 1; - - Eigen::VectorXd map_offset(4); - map_offset(0) = map_offset_vec.at(ped_idx_l).at(vel_idx_l); - map_offset(1) = map_offset_vec.at(ped_idx_h).at(vel_idx_l); - map_offset(2) = map_offset_vec.at(ped_idx_l).at(vel_idx_h); - map_offset(3) = map_offset_vec.at(ped_idx_h).at(vel_idx_h); - - Eigen::VectorXd updated_map_offset(4); - - Eigen::MatrixXd covariance = covariance_mat.at(ped_idx_l).at(vel_idx_l); - - /* calculate adaptive map offset */ - Eigen::MatrixXd G(4, 4); - Eigen::RowVectorXd phiT(4); - phiT = phi.transpose(); - double rk = phiT * covariance * phi; - - G = covariance * phi / (forgetting_factor_ + rk); - double beta = rk > 0 ? (forgetting_factor_ - (1 - forgetting_factor_) / rk) : 1; - covariance = covariance - covariance * phi * phiT * covariance / (1 / beta + rk); // anti-windup - double eta_hat = phiT * theta; - - const double error_map_offset = measured_acc - eta_hat; - updated_map_offset = map_offset + G * error_map_offset; - - for (int i = 0; i < 4; i++) { - const double pre_mean = mean(i); - mean(i) = (weighted_sum(i) * pre_mean + error_map_offset) / (weighted_sum(i) + 1); - sigma(i) = - (weighted_sum(i) * (sigma(i) + pre_mean * pre_mean) + error_map_offset * error_map_offset) / - (weighted_sum(i) + 1) - - mean(i) * mean(i); - weighted_sum(i) = weighted_sum(i) + 1; - } - - /* input calculated result and update map */ - map_offset_vec.at(ped_idx_l).at(vel_idx_l) = updated_map_offset(0); - map_offset_vec.at(ped_idx_h).at(vel_idx_l) = updated_map_offset(1); - map_offset_vec.at(ped_idx_l).at(vel_idx_h) = updated_map_offset(2); - map_offset_vec.at(ped_idx_h).at(vel_idx_h) = updated_map_offset(3); - - data_covariance_mat(ped_idx_l, vel_idx_l) = sigma(0); - data_covariance_mat(ped_idx_h, vel_idx_l) = sigma(1); - data_covariance_mat(ped_idx_l, vel_idx_h) = sigma(2); - data_covariance_mat(ped_idx_h, vel_idx_h) = sigma(3); - - data_weighted_num(ped_idx_l, vel_idx_l) = weighted_sum(0); - data_weighted_num(ped_idx_h, vel_idx_l) = weighted_sum(1); - data_weighted_num(ped_idx_l, vel_idx_h) = weighted_sum(2); - data_weighted_num(ped_idx_h, vel_idx_h) = weighted_sum(3); - - data_mean_mat(ped_idx_l, vel_idx_l) = mean(0); - data_mean_mat(ped_idx_h, vel_idx_l) = mean(1); - data_mean_mat(ped_idx_l, vel_idx_h) = mean(2); - data_mean_mat(ped_idx_h, vel_idx_h) = mean(3); - - covariance_mat.at(ped_idx_l).at(vel_idx_l) = covariance; - - update_map_value.at(pedal_index + 0).at(vel_index + 0) = - map_value.at(pedal_index + 0).at(vel_index + 0) + map_offset(0); - update_map_value.at(pedal_index + 1).at(vel_index + 0) = - map_value.at(pedal_index + 1).at(vel_index + 0) + map_offset(1); - update_map_value.at(pedal_index + 0).at(vel_index + 1) = - map_value.at(pedal_index + 0).at(vel_index + 1) + map_offset(2); - update_map_value.at(pedal_index + 1).at(vel_index + 1) = - map_value.at(pedal_index + 1).at(vel_index + 1) + map_offset(3); - - offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(0)); - offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(1)); - offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(2)); - offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(3)); - - return true; -} - -bool AccelBrakeMapCalibrator::updateEachValOffset( - const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, - const int brake_pedal_index, const int brake_vel_index, const double measured_acc, - const double map_acc) -{ - // pre-defined - static Map map_offset_vec_( - accel_map_value_.size() + brake_map_value_.size() - 1, - std::vector(accel_map_value_.at(0).size(), map_offset_)); - static Map covariance_vec_( - accel_map_value_.size() + brake_map_value_.size() - 1, - std::vector(accel_map_value_.at(0).size(), covariance_)); - - const int vel_idx = accel_mode ? accel_vel_index : brake_vel_index; - int ped_idx = accel_mode ? accel_pedal_index : brake_pedal_index; - ped_idx = getUnifiedIndexFromAccelBrakeIndex(accel_mode, ped_idx); - double map_offset = map_offset_vec_.at(ped_idx).at(vel_idx); - double covariance = covariance_vec_.at(ped_idx).at(vel_idx); - - /* calculate adaptive map offset */ - const double phi = 1.0; - covariance = (covariance - (covariance * phi * phi * covariance) / - (forgetting_factor_ + phi * covariance * phi)) / - forgetting_factor_; - - const double coef = (covariance * phi) / (forgetting_factor_ + phi * covariance * phi); - - const double error_map_offset = measured_acc - map_acc; - map_offset = map_offset + coef * error_map_offset; - - RCLCPP_DEBUG_STREAM( - get_logger(), "index: " << ped_idx << ", " << vel_idx - << ": map_offset_ = " << map_offset_vec_.at(ped_idx).at(vel_idx) - << " -> " << map_offset << "\t" - << " covariance = " << covariance); - - /* input calculated result and update map */ - map_offset_vec_.at(ped_idx).at(vel_idx) = map_offset; - covariance_vec_.at(ped_idx).at(vel_idx) = covariance; - if (accel_mode) { - update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) = - accel_map_value_.at(accel_pedal_index).at(accel_vel_index) + map_offset; - } else { - update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index) = - brake_map_value_.at(brake_pedal_index).at(brake_vel_index) + map_offset; - } - return true; -} - -void AccelBrakeMapCalibrator::updateTotalMapOffset(const double measured_acc, const double map_acc) -{ - /* calculate adaptive map offset */ - const double phi = 1.0; - covariance_ = (covariance_ - (covariance_ * phi * phi * covariance_) / - (forgetting_factor_ + phi * covariance_ * phi)) / - forgetting_factor_; - - const double coef = (covariance_ * phi) / (forgetting_factor_ + phi * covariance_ * phi); - const double error_map_offset = measured_acc - map_acc; - map_offset_ = map_offset_ + coef * error_map_offset; - - RCLCPP_DEBUG_STREAM( - get_logger(), "map_offset_ = " << map_offset_ << "\t" - << "covariance = " << covariance_); - - /* update map */ - for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_accel_map_value_.at(0).size() - 1; vel_idx++) { - update_accel_map_value_.at(ped_idx).at(vel_idx) = - accel_map_value_.at(ped_idx).at(vel_idx) + map_offset_; - } - } - for (std::size_t ped_idx = 0; ped_idx < update_brake_map_value_.size() - 1; ped_idx++) { - for (std::size_t vel_idx = 0; vel_idx < update_brake_map_value_.at(0).size() - 1; vel_idx++) { - update_brake_map_value_.at(ped_idx).at(vel_idx) = - brake_map_value_.at(ped_idx).at(vel_idx) + map_offset_; - } - } -} - -std::vector AccelBrakeMapCalibrator::getMapColumnFromUnifiedIndex( - const Map & accel_map_value, const Map & brake_map_value, const std::size_t index) -{ - if (index < brake_map_value.size()) { - // input brake map value - return brake_map_value.at(brake_map_value.size() - index - 1); - } else { - // input accel map value - return accel_map_value.at(index - brake_map_value.size() + 1); - } -} - -double AccelBrakeMapCalibrator::getPedalValueFromUnifiedIndex(const std::size_t index) -{ - if (index < brake_pedal_index_.size()) { - // brake index ( minus ) - return -brake_pedal_index_.at(brake_pedal_index_.size() - index - 1); - } else { - // input accel map value - return accel_pedal_index_.at(index - brake_pedal_index_.size() + 1); - } -} - -int AccelBrakeMapCalibrator::getUnifiedIndexFromAccelBrakeIndex( - const bool accel_map, const std::size_t index) -{ - // accel_map=true: accel_map; accel_map=false: brake_map - if (accel_map) { - // accel map - return index + brake_map_value_.size() - 1; - } else { - // brake map - return brake_map_value_.size() - index - 1; - } -} - -double AccelBrakeMapCalibrator::getPitchCompensatedAcceleration() -{ - // It works correctly only when the velocity is positive. - constexpr double gravity = 9.80665; - return gravity * std::sin(pitch_); -} - -void AccelBrakeMapCalibrator::executeEvaluation() -{ - const double full_orig_accel_sq_error = calculateAccelSquaredError( - delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, - accel_map_, brake_map_); - pushDataToVec(full_orig_accel_sq_error, full_mse_que_size_, &full_original_accel_mse_que_); - full_original_accel_rmse_ = getAverage(full_original_accel_mse_que_); - // std::cerr << "rmse : " << sqrt(full_original_accel_rmse_) << std::endl; - - const double full_orig_accel_l1_error = calculateAccelErrorL1Norm( - delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, - accel_map_, brake_map_); - const double full_orig_accel_sq_l1_error = full_orig_accel_l1_error * full_orig_accel_l1_error; - pushDataToVec(full_orig_accel_l1_error, full_mse_que_size_, &full_original_accel_l1_que_); - pushDataToVec(full_orig_accel_sq_l1_error, full_mse_que_size_, &full_original_accel_sq_l1_que_); - full_original_accel_error_l1norm_ = getAverage(full_original_accel_l1_que_); - - /*calculate l1norm_covariance*/ - // const double full_original_accel_error_sql1_ = getAverage(full_original_accel_sq_l1_que_); - // std::cerr << "error_l1norm : " << full_original_accel_error_l1norm_ << std::endl; - // std::cerr << "error_l1_cov : " << - // full_original_accel_error_sql1_-full_original_accel_error_l1norm_*full_original_accel_error_l1norm_ - // << std::endl; - - const double part_orig_accel_sq_error = calculateAccelSquaredError( - delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, - accel_map_, brake_map_); - pushDataToVec(part_orig_accel_sq_error, part_mse_que_size_, &part_original_accel_mse_que_); - part_original_accel_rmse_ = getAverage(part_original_accel_mse_que_); - - const double new_accel_sq_error = calculateAccelSquaredError( - delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, - new_accel_map_, new_brake_map_); - pushDataToVec(new_accel_sq_error, part_mse_que_size_, &new_accel_mse_que_); - new_accel_rmse_ = getAverage(new_accel_mse_que_); -} - -double AccelBrakeMapCalibrator::calculateEstimatedAcc( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map) -{ - const double pedal = throttle - brake; - - double estimated_acc = 0.0; - if (pedal > 0.0) { - accel_map.getAcceleration(pedal, vel, estimated_acc); - } else { - brake_map.getAcceleration(-pedal, vel, estimated_acc); - } - - return estimated_acc; -} - -double AccelBrakeMapCalibrator::calculateAccelSquaredError( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map) -{ - const double estimated_acc = calculateEstimatedAcc(throttle, brake, vel, accel_map, brake_map); - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); - const double dif_acc = measured_acc - estimated_acc; - return dif_acc * dif_acc; -} - -double AccelBrakeMapCalibrator::calculateAccelErrorL1Norm( - const double throttle, const double brake, const double vel, AccelMap & accel_map, - BrakeMap & brake_map) -{ - const double estimated_acc = calculateEstimatedAcc(throttle, brake, vel, accel_map, brake_map); - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); - const double dif_acc = measured_acc - estimated_acc; - return abs(dif_acc); -} - -void AccelBrakeMapCalibrator::pushDataToQue( - const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, - std::queue * que) -{ - que->push(data); - while (que->size() > max_size) { - que->pop(); - } -} - -template -void AccelBrakeMapCalibrator::pushDataToVec( - const T data, const std::size_t max_size, std::vector * vec) -{ - vec->emplace_back(data); - while (vec->size() > max_size) { - vec->erase(vec->begin()); - } -} - -template -T AccelBrakeMapCalibrator::getNearestTimeDataFromVec( - const T base_data, const double back_time, const std::vector & vec) -{ - double nearest_time = std::numeric_limits::max(); - const double target_time = rclcpp::Time(base_data->header.stamp).seconds() - back_time; - T nearest_time_data; - for (const auto & data : vec) { - const double data_time = rclcpp::Time(data->header.stamp).seconds(); - const auto delta_time = std::abs(target_time - data_time); - if (nearest_time > delta_time) { - nearest_time_data = data; - nearest_time = delta_time; - } - } - return nearest_time_data; -} - -DataStampedPtr AccelBrakeMapCalibrator::getNearestTimeDataFromVec( - DataStampedPtr base_data, const double back_time, const std::vector & vec) -{ - double nearest_time = std::numeric_limits::max(); - const double target_time = base_data->data_time.seconds() - back_time; - DataStampedPtr nearest_time_data; - for (const auto & data : vec) { - const double data_time = data->data_time.seconds(); - const auto delta_time = std::abs(target_time - data_time); - if (nearest_time > delta_time) { - nearest_time_data = data; - nearest_time = delta_time; - } - } - return nearest_time_data; -} - -double AccelBrakeMapCalibrator::getAverage(const std::vector & vec) -{ - if (vec.empty()) { - return 0.0; - } - - double sum = 0.0; - for (const auto num : vec) { - sum += num; - } - return sum / vec.size(); -} - -double AccelBrakeMapCalibrator::getStandardDeviation(const std::vector & vec) -{ - if (vec.empty()) { - return 0.0; - } - - const double ave = getAverage(vec); - - double sum = 0.0; - for (const auto num : vec) { - sum += std::pow(num - ave, 2); - } - return std::sqrt(sum / vec.size()); -} - -bool AccelBrakeMapCalibrator::isTimeout( - const builtin_interfaces::msg::Time & stamp, const double timeout_sec) -{ - const double dt = this->now().seconds() - rclcpp::Time(stamp).seconds(); - return dt > timeout_sec; -} - -bool AccelBrakeMapCalibrator::isTimeout( - const DataStampedPtr & data_stamped, const double timeout_sec) -{ - const double dt = (this->now() - data_stamped->data_time).seconds(); - return dt > timeout_sec; -} - -OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( - const std::string frame_id, const double height, const double width, const double resolution, - const std::vector & map_value) -{ - OccupancyGrid occ; - occ.header.frame_id = frame_id; - occ.header.stamp = this->now(); - occ.info.height = height; - occ.info.width = width; - occ.info.map_load_time = this->now(); - occ.info.origin.position.x = 0; - occ.info.origin.position.y = 0; - occ.info.origin.position.z = 0; - occ.info.origin.orientation.x = 0; - occ.info.origin.orientation.y = 0; - occ.info.origin.orientation.z = 0; - occ.info.origin.orientation.w = 1; - occ.info.resolution = resolution; - occ.data = map_value; - return occ; -} - -// function for diagnostics -void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; - using CalibrationStatus = CalibrationStatus; - CalibrationStatus accel_brake_map_status; - int8_t level; - std::string msg; - - accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; - if (is_default_map_ == true) { - accel_brake_map_status.status = CalibrationStatus::NORMAL; - level = DiagStatus::OK; - msg = "OK"; - } else { - accel_brake_map_status.status = CalibrationStatus::UNAVAILABLE; - level = DiagStatus::ERROR; - msg = "Default map is not found in " + csv_default_map_dir_; - } - - if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { - // lack of data - stat.summary(level, msg); - calibration_status_pub_->publish(accel_brake_map_status); - return; - } - - const double rmse_rate = new_accel_rmse_ / part_original_accel_rmse_; - if (rmse_rate < update_suggest_thresh_) { - // The accuracy of original accel/brake map is low. - // Suggest to update accel brake map - level = DiagStatus::WARN; - msg = "Accel/brake map Calibration is required."; - accel_brake_map_status.status = CalibrationStatus::CALIBRATION_REQUIRED; - } - - stat.summary(level, msg); - calibration_status_pub_->publish(accel_brake_map_status); -} - -// function for debug - -void AccelBrakeMapCalibrator::publishMap( - const Map accel_map_value, const Map brake_map_value, const std::string publish_type) -{ - if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Invalid map. The number of velocity index of accel map and brake map is different."); - return; - } - const double h = accel_map_value.size() + brake_map_value.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value.at(0).size(); // velocity - - // publish occupancy map - const int8_t MAX_OCC_VALUE = 100; - std::vector int_map_value; - int_map_value.resize(h * w); - for (int i = 0; i < h; i++) { - for (int j = 0; j < w; j++) { - const double value = - getMapColumnFromUnifiedIndex(accel_map_value_, brake_map_value_, i).at(j); - // convert acc to 0~100 int value - int8_t int_value = - static_cast(MAX_OCC_VALUE * ((value - min_accel_) / (max_accel_ - min_accel_))); - int_map_value.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_value), (int8_t)0); - } - } - - if (publish_type == "original") { - original_map_occ_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, int_map_value)); - } else { - update_map_occ_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, int_map_value)); - } - - // publish raw map - Float32MultiArray float_map; - - float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - float_map.layout.dim[0].label = "height"; - float_map.layout.dim[1].label = "width"; - float_map.layout.dim[0].size = h; - float_map.layout.dim[1].size = w; - float_map.layout.dim[0].stride = h * w; - float_map.layout.dim[1].stride = w; - float_map.layout.data_offset = 0; - std::vector vec(h * w, 0); - for (int i = 0; i < h; i++) { - for (int j = 0; j < w; j++) { - vec[i * w + j] = static_cast( - getMapColumnFromUnifiedIndex(accel_map_value_, brake_map_value_, i).at(j)); - } - } - float_map.data = vec; - if (publish_type == "original") { - original_map_raw_pub_->publish(float_map); - } else { - update_map_raw_pub_->publish(float_map); - } -} - -void AccelBrakeMapCalibrator::publishOffsetCovMap( - const Map accel_map_value, const Map brake_map_value) -{ - if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Invalid map. The number of velocity index of accel map and brake map is different."); - return; - } - const double h = accel_map_value.size() + brake_map_value.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value.at(0).size(); // velocity - - // publish raw map - Float32MultiArray float_map; - - float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); - float_map.layout.dim[0].label = "height"; - float_map.layout.dim[1].label = "width"; - float_map.layout.dim[0].size = h; - float_map.layout.dim[1].size = w; - float_map.layout.dim[0].stride = h * w; - float_map.layout.dim[1].stride = w; - float_map.layout.data_offset = 0; - std::vector vec(h * w, 0); - for (int i = 0; i < h; i++) { - for (int j = 0; j < w; j++) { - vec[i * w + j] = - static_cast(getMapColumnFromUnifiedIndex(accel_map_value, brake_map_value, i).at(j)); - } - } - float_map.data = vec; - offset_covariance_pub_->publish(float_map); -} - -void AccelBrakeMapCalibrator::publishFloat32(const std::string publish_type, const double val) -{ - Float32Stamped msg; - msg.stamp = this->now(); - msg.data = val; - if (publish_type == "current_map_error") { - current_map_error_pub_->publish(msg); - } else if (publish_type == "updated_map_error") { - updated_map_error_pub_->publish(msg); - } else { - map_error_ratio_pub_->publish(msg); - } -} - -void AccelBrakeMapCalibrator::publishCountMap() -{ - if (accel_map_value_.at(0).size() != brake_map_value_.at(0).size()) { - RCLCPP_ERROR_STREAM( - get_logger(), - "Invalid map. The number of velocity index of accel map and brake map is different."); - return; - } - const double h = accel_map_value_.size() + brake_map_value_.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value_.at(0).size(); // velocity - const int8_t MAX_OCC_VALUE = 100; - - std::vector count_map, ave_map, std_map; - count_map.resize(h * w); - ave_map.resize(h * w); - std_map.resize(h * w); - - for (int i = 0; i < h; i++) { - for (int j = 0; j < w; j++) { - const auto data_vec = map_value_data_.at(i).at(j); - if (data_vec.empty()) { - // input *UNKNOWN* value - count_map.at(i * w + j) = -1; - ave_map.at(i * w + j) = -1; - } else { - const auto count_rate = - MAX_OCC_VALUE * (static_cast(data_vec.size()) / max_data_count_); - count_map.at(i * w + j) = static_cast( - std::max(std::min(static_cast(MAX_OCC_VALUE), static_cast(count_rate)), 0)); - // calculate average - { - const double average = getAverage(data_vec); - int8_t int_average = static_cast( - MAX_OCC_VALUE * ((average - min_accel_) / (max_accel_ - min_accel_))); - ave_map.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_average), (int8_t)0); - } - // calculate standard deviation - { - const double std_dev = getStandardDeviation(data_vec); - const double max_std_dev = 0.2; - const double min_std_dev = 0.0; - int8_t int_std_dev = static_cast( - MAX_OCC_VALUE * ((std_dev - min_std_dev) / (max_std_dev - min_std_dev))); - std_map.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_std_dev), (int8_t)0); - } - } - } - } - - // publish average map / standard dev map / count map - data_ave_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, ave_map)); - data_std_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, std_map)); - data_count_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, count_map)); - - // publish count with self pos map - int nearest_pedal_idx = nearestPedalSearch(); - int nearest_vel_idx = nearestVelSearch(); - - // input self pose (to max value / min value ) and publish this - update_success_ - ? count_map.at(nearest_pedal_idx * w + nearest_vel_idx) = std::numeric_limits::max() - : count_map.at(nearest_pedal_idx * w + nearest_vel_idx) = std::numeric_limits::min(); - data_count_with_self_pose_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, count_map)); -} - -void AccelBrakeMapCalibrator::publishIndex() -{ - MarkerArray markers; - const double h = accel_map_value_.size() + brake_map_value_.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value_.at(0).size(); // velocity - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "base_link"; - marker.header.stamp = this->now(); - marker.type = marker.TEXT_VIEW_FACING; - marker.action = marker.ADD; - std_msgs::msg::ColorRGBA color; - color.a = 0.999; - color.b = 0.1; - color.g = 0.1; - color.r = 0.1; - marker.color = color; - marker.frame_locked = true; - marker.pose.position.z = 0.0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - - // pedal value - for (int pedal_idx = 0; pedal_idx < h; pedal_idx++) { - const double pedal_value = getPedalValueFromUnifiedIndex(pedal_idx); - marker.ns = "occ_pedal_index"; - marker.id = pedal_idx; - marker.pose.position.x = -map_resolution_ * 0.5; - marker.pose.position.y = map_resolution_ * (0.5 + pedal_idx); - marker.scale.z = 0.03; - std::stringstream stream; - stream << std::fixed << std::setprecision(2) << pedal_value; - marker.text = stream.str(); - markers.markers.push_back(marker); - } - - // pedal index name - marker.ns = "occ_pedal_index"; - marker.id = h; - marker.pose.position.x = -map_resolution_ * 0.5; - marker.pose.position.y = map_resolution_ * (0.5 + h); - marker.scale.z = 0.03; - marker.text = "(accel_pedal/brake_pedal)"; - markers.markers.push_back(marker); - - // velocity value - for (int vel_idx = 0; vel_idx < w; vel_idx++) { - const double vel_value = accel_vel_index_.at(vel_idx); - marker.ns = "occ_vel_index"; - marker.id = vel_idx; - marker.pose.position.x = map_resolution_ * (0.5 + vel_idx); - marker.pose.position.y = -map_resolution_ * 0.2; - marker.scale.z = 0.02; - std::stringstream stream; - stream << std::fixed << std::setprecision(2) << vel_value; - marker.text = stream.str() + "m/s"; - markers.markers.push_back(marker); - } - - // velocity index name - marker.ns = "occ_vel_index"; - marker.id = w; - marker.pose.position.x = map_resolution_ * (0.5 + w); - marker.pose.position.y = -map_resolution_ * 0.2; - marker.scale.z = 0.02; - marker.text = "(velocity)"; - markers.markers.push_back(marker); - - index_pub_->publish(markers); -} - -void AccelBrakeMapCalibrator::publishUpdateSuggestFlag() -{ - std_msgs::msg::Bool update_suggest; - - if (new_accel_mse_que_.size() < part_mse_que_size_ / 2) { - // lack of data - update_suggest.data = false; - } else { - const double rmse_rate = new_accel_rmse_ / part_original_accel_rmse_; - update_suggest.data = (rmse_rate < update_suggest_thresh_); - if (update_suggest.data) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, - "suggest to update accel/brake map. evaluation score = " << rmse_rate); - } - } - - update_suggest_pub_->publish(update_suggest); -} - -bool AccelBrakeMapCalibrator::writeMapToCSV( - std::vector vel_index, std::vector pedal_index, Map value_map, - std::string filename) -{ - if (update_success_count_ == 0) { - return false; - } - - std::ofstream csv_file(filename); - - if (!csv_file.is_open()) { - RCLCPP_WARN(get_logger(), "Failed to open csv file : %s", filename.c_str()); - return false; - } - - csv_file << "default,"; - for (std::size_t v = 0; v < vel_index.size(); v++) { - csv_file << vel_index.at(v); - if (v != vel_index.size() - 1) { - csv_file << ","; - } - } - csv_file << "\n"; - - for (std::size_t p = 0; p < pedal_index.size(); p++) { - csv_file << pedal_index.at(p) << ","; - for (std::size_t v = 0; v < vel_index.size(); v++) { - csv_file << std::fixed << std::setprecision(precision_) << value_map.at(p).at(v); - if (v != vel_index.size() - 1) { - csv_file << ","; - } - } - csv_file << "\n"; - } - csv_file.close(); - RCLCPP_DEBUG_STREAM(get_logger(), "output map to " << filename); - return true; -} - -void AccelBrakeMapCalibrator::addIndexToCSV(std::ofstream * csv_file) -{ - *csv_file << "timestamp,velocity,accel,pitch_comp_accel,final_accel,accel_pedal,brake_pedal," - << "accel_pedal_speed,brake_pedal_speed,pitch,steer,jerk,full_original_accel_rmse, " - "part_original_accel_rmse,new_accel_rmse, rmse_rate" - << std::endl; -} - -void AccelBrakeMapCalibrator::addLogToCSV( - std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, - const double pitched_accel, const double accel_pedal, const double brake_pedal, - const double accel_pedal_speed, const double brake_pedal_speed, const double pitch, - const double steer, const double jerk, const double full_original_accel_mse, - const double part_original_accel_mse, const double new_accel_mse) -{ - const double rmse_rate = - part_original_accel_mse == 0.0 ? 1.0 : (new_accel_mse / part_original_accel_mse); - *csv_file << timestamp << ", " << velocity << ", " << accel << ", " << pitched_accel << ", " - << accel - pitched_accel << ", " << accel_pedal << ", " << brake_pedal << ", " - << accel_pedal_speed << ", " << brake_pedal_speed << ", " << pitch << ", " << steer - << ", " << jerk << ", " << full_original_accel_mse << ", " << part_original_accel_mse - << ", " << new_accel_mse << "," << rmse_rate << std::endl; -} - -} // namespace accel_brake_map_calibrator diff --git a/vehicle/accel_brake_map_calibrator/src/main.cpp b/vehicle/accel_brake_map_calibrator/src/main.cpp deleted file mode 100644 index 01dfe203..00000000 --- a/vehicle/accel_brake_map_calibrator/src/main.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// -// Copyright 2020 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 "accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - rclcpp::spin(std::make_shared(node_options)); - rclcpp::shutdown(); - return 0; -} diff --git a/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py b/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py deleted file mode 100755 index 1bbb5820..00000000 --- a/vehicle/accel_brake_map_calibrator/test/plot_csv_client_node.py +++ /dev/null @@ -1,122 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2022 Tier IV, Inc. -# -# 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. - -import argparse -import os - -from ament_index_python.packages import get_package_share_directory -import rclpy -from rclpy.node import Node -from tier4_external_api_msgs.srv import GetAccelBrakeMapCalibrationData as CalibData - - -class CalibrationDataRelay(Node): - def __init__(self, args): - super().__init__("plot_client") - self.cli = self.create_client( - CalibData, "/api/external/get/accel_brake_map_calibrator/data" - ) - - while not self.cli.wait_for_service(timeout_sec=1.0): - self.get_logger().info("service not available, waiting again...") - - self.request = CalibData.Request() - - def send_request(self): - self.future = self.cli.call_async(self.request) - - -def main(args=None): - rclpy.init(args=None) - client = CalibrationDataRelay(args) - client.send_request() - while rclpy.ok(): - rclpy.spin_once(client) - if client.future.done(): - try: - response = client.future.result() - save_dir = "./test_data_save" - if not os.path.exists(save_dir): - os.makedirs(save_dir) - svg_name = save_dir + "/graph.svg" - - f_svg = open(svg_name, "w") - svg_byte = bytes(response.graph_image) - text = svg_byte.decode() - f_svg.write(text) - - print("svg done") - - acc_map_name = save_dir + "/accel_map.csv" - f_acc = open(acc_map_name, "w") - f_acc.write(response.accel_map) - - print("accel map done") - - brk_map_name = save_dir + "/brake_map.csv" - f_brk = open(brk_map_name, "w") - f_brk.write(response.brake_map) - - print("brake map done") - - except Exception as e: - client.get_logger().info("Service call failed %r" % (e,)) - - break - - client.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - package_path = get_package_share_directory("accel_brake_map_calibrator") - parser = argparse.ArgumentParser() - parser.add_argument( - "-d", "--default-map-dir", default=None, type=str, help="directory of default map" - ) - parser.add_argument( - "-c", "--calibrated-map-dir", default=None, type=str, help="directory of calibrated map" - ) - parser.add_argument("-s", "--scatter-only", action="store_true", help="show only scatters") - parser.add_argument( - "-l", - "--log-file", - default=package_path + "/config/log.csv", - type=str, - help="path of log.csv", - ) - parser.add_argument( - "--min-vel-thr", default=0.1, type=float, help="valid min velocity threshold" - ) - parser.add_argument( - "--vel-diff-thr", default=0.556, type=float, help="valid velocity diff threshold" - ) - parser.add_argument( - "--pedal-diff-thr", default=0.03, type=float, help="valid pedal diff threshold" - ) - parser.add_argument( - "--max-steer-thr", default=0.2, type=float, help="valid max steer threshold" - ) - parser.add_argument( - "--max-pitch-thr", default=0.02, type=float, help="valid max pitch threshold" - ) - parser.add_argument("--max-jerk-thr", default=0.7, type=float, help="valid max jerk threshold") - parser.add_argument( - "--max-pedal-vel-thr", default=0.7, type=float, help="valid max pedal velocity threshold" - ) - - args = parser.parse_args() - main(args) diff --git a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py deleted file mode 100755 index 9e57813b..00000000 --- a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py +++ /dev/null @@ -1,62 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2022 Tier IV, Inc. -# -# 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. - - -from autoware_auto_vehicle_msgs.msg import VelocityReport -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSHistoryPolicy -from rclpy.qos import QoSProfile -from rclpy.qos import QoSReliabilityPolicy -from tier4_vehicle_msgs.msg import ActuationStatusStamped - - -class ActuationStatusPublisher(Node): - def __init__(self): - super().__init__("actuation_status_publisher") - - qos_profile = QoSProfile(depth=1) - qos_profile.reliability = QoSReliabilityPolicy.RELIABLE - qos_profile.history = QoSHistoryPolicy.KEEP_LAST - qos_profile.durability = QoSDurabilityPolicy.VOLATILE - - self.pub = self.create_publisher( - ActuationStatusStamped, "/vehicle/status/actuation_status", qos_profile - ) - self.sub = self.create_subscription( - VelocityReport, "/vehicle/status/velocity_status", self.callback, qos_profile - ) - - def callback(self, msg): - data = ActuationStatusStamped() - data.header = msg.header - data.status.accel_status = msg.longitudinal_velocity * 0.1 - data.status.brake_status = 0.1 - data.status.steer_status = 0.1 - self.pub.publish(data) - - -def main(args=None): - rclpy.init(args=args) - node = ActuationStatusPublisher() - rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main()