From dfa31a60b1bc0dfab3014d17bdbe148beffa57a3 Mon Sep 17 00:00:00 2001 From: Autumn60 <37181352+Autumn60@users.noreply.github.com> Date: Fri, 7 Jun 2024 20:01:26 +0900 Subject: [PATCH 1/2] fix(mrm_handler): fix stop judgement (#7362) fix stop judgement Signed-off-by: Autumn60 Co-authored-by: Autumn60 --- system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 44407c40c6787..326fbb392fd35 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -520,7 +520,7 @@ bool MrmHandler::isStopped() auto odom = sub_odom_.takeData(); if (odom == nullptr) return false; constexpr auto th_stopped_velocity = 0.001; - return (std::abs(odom->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity); + return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); } bool MrmHandler::isDrivingBackwards() From ed038d8c1462d19232e7270479ceb01ebea7b382 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Fri, 7 Jun 2024 21:06:36 +0900 Subject: [PATCH 2/2] chore(smart_mpc_trajectory_follower): add prefix autoware_ to smart_mpc_trajectory_follower (#7367) * add prefix Signed-off-by: Go Sakayori * fix pre-commit Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori --- .github/CODEOWNERS | 2 +- .../.gitignore | 0 .../CMakeLists.txt | 4 +- .../README.md | 66 +++++++++--------- .../.gitignore | 0 .../__init__.py | 0 .../clear_pycache.py | 4 +- .../param/mpc_param.yaml | 0 .../param/nominal_param.yaml | 0 .../param/trained_model_param.yaml | 0 .../python_simulator/.gitignore | 0 .../python_simulator/accel_map.csv | 0 .../plot_auto_test_result.ipynb | 0 .../python_simulator/python_simulator.py | 4 +- .../python_simulator/run_python_simulator.py | 2 +- .../python_simulator/run_sim.py | 2 +- .../python_simulator/slalom_course_data.csv | 0 .../scripts/.gitignore | 0 .../scripts/__init__.py | 0 .../scripts/drive_GP.py | 2 +- .../scripts/drive_NN.py | 4 +- .../scripts/drive_controller.py | 10 +-- .../scripts/drive_functions.py | 8 ++- .../scripts/drive_iLQR.py | 2 +- .../scripts/drive_mppi.py | 2 +- .../scripts/proxima_calc.cpp | 0 .../scripts/pympc_trajectory_follower.py | 4 +- .../training_and_data_check/.gitignore | 0 .../training_and_data_check/__init__.py | 0 .../add_training_data_from_csv.py | 4 +- .../data_checker.ipynb | 0 .../execute_train_drive_NN_model.ipynb | 0 .../training_and_data_check/rosbag2.bash | 0 .../train_drive_GP_model.py | 0 .../train_drive_NN_model.py | 8 ++- .../images/autoware_smart_mpc.png | Bin .../images/lateral_error_nominal_model.png | Bin .../images/lateral_error_trained_model.png | Bin .../images/proxima_logo.png | Bin ...python_sim_lateral_error_nominal_model.png | Bin ...python_sim_lateral_error_trained_model.png | Bin .../images/test_route.png | Bin .../package.xml | 2 +- .../setup.py | 14 ++-- .../launch/control.launch.py | 2 +- 45 files changed, 75 insertions(+), 71 deletions(-) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/.gitignore (100%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/CMakeLists.txt (60%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/README.md (85%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/.gitignore (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/__init__.py (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/clear_pycache.py (88%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/param/mpc_param.yaml (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/param/nominal_param.yaml (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/param/trained_model_param.yaml (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/python_simulator/.gitignore (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/python_simulator/accel_map.csv (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/python_simulator/plot_auto_test_result.ipynb (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/python_simulator/python_simulator.py (99%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/python_simulator/run_python_simulator.py (92%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/python_simulator/run_sim.py (99%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/python_simulator/slalom_course_data.csv (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/.gitignore (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/__init__.py (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/drive_GP.py (98%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/drive_NN.py (98%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/drive_controller.py (99%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/drive_functions.py (99%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/drive_iLQR.py (99%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/drive_mppi.py (98%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/proxima_calc.cpp (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/scripts/pympc_trajectory_follower.py (99%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/training_and_data_check/.gitignore (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/training_and_data_check/__init__.py (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/training_and_data_check/add_training_data_from_csv.py (98%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/training_and_data_check/data_checker.ipynb (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/training_and_data_check/execute_train_drive_NN_model.ipynb (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/training_and_data_check/rosbag2.bash (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/training_and_data_check/train_drive_GP_model.py (100%) rename control/{smart_mpc_trajectory_follower/smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower}/training_and_data_check/train_drive_NN_model.py (99%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/images/autoware_smart_mpc.png (100%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/images/lateral_error_nominal_model.png (100%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/images/lateral_error_trained_model.png (100%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/images/proxima_logo.png (100%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/images/python_sim_lateral_error_nominal_model.png (100%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/images/python_sim_lateral_error_trained_model.png (100%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/images/test_route.png (100%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/package.xml (96%) rename control/{smart_mpc_trajectory_follower => autoware_smart_mpc_trajectory_follower}/setup.py (60%) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index c0eb60e5a1316..cbab68f91f512 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -57,7 +57,7 @@ control/pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tie control/predicted_path_checker/** berkay@leodrive.ai control/pure_pursuit/** takamasa.horibe@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp -control/smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com +control/autoware_smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp diff --git a/control/smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/.gitignore diff --git a/control/smart_mpc_trajectory_follower/CMakeLists.txt b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt similarity index 60% rename from control/smart_mpc_trajectory_follower/CMakeLists.txt rename to control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt index 7aecab2597dcd..57e7b596790c2 100644 --- a/control/smart_mpc_trajectory_follower/CMakeLists.txt +++ b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.14) -project(smart_mpc_trajectory_follower) +project(autoware_smart_mpc_trajectory_follower) find_package(autoware_cmake REQUIRED) autoware_package() install(PROGRAMS - smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py + autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py DESTINATION lib/${PROJECT_NAME} ) ament_auto_package( diff --git a/control/smart_mpc_trajectory_follower/README.md b/control/autoware_smart_mpc_trajectory_follower/README.md similarity index 85% rename from control/smart_mpc_trajectory_follower/README.md rename to control/autoware_smart_mpc_trajectory_follower/README.md index 8fcc13142d68b..fd135319ff900 100644 --- a/control/smart_mpc_trajectory_follower/README.md +++ b/control/autoware_smart_mpc_trajectory_follower/README.md @@ -19,7 +19,7 @@ This technology makes it relatively easy to operate model predictive control, wh ## Setup -After building autoware, move to `control/smart_mpc_trajectory_follower` and run the following command: +After building autoware, move to `control/autoware_smart_mpc_trajectory_follower` and run the following command: ```bash pip3 install . @@ -37,7 +37,7 @@ This package provides smart MPC logic for path-following control as well as mech ### Trajectory following control based on iLQR/MPPI -The control mode can be selected from "ilqr", "mppi", or "mppi_ilqr", and can be set as `mpc_parameter:system:mode` in [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml). +The control mode can be selected from "ilqr", "mppi", or "mppi_ilqr", and can be set as `mpc_parameter:system:mode` in [mpc_param.yaml](./autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml). In "mppi_ilqr" mode, the initial value of iLQR is given by the MPPI solution. > [!NOTE] @@ -50,7 +50,7 @@ ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autowa ``` > [!NOTE] -> When running with the nominal model set in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml), set `trained_model_parameter:control_application:use_trained_model` to `false` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). To run using the trained model, set `trained_model_parameter:control_application:use_trained_model` to `true`, but the trained model must have been generated according to the following procedure. +> When running with the nominal model set in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml), set `trained_model_parameter:control_application:use_trained_model` to `false` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). To run using the trained model, set `trained_model_parameter:control_application:use_trained_model` to `true`, but the trained model must have been generated according to the following procedure. ### Training of model and reflection in control @@ -60,7 +60,7 @@ To obtain training data, start autoware, perform a drive, and record rosbag data ros2 bag record /localization/kinematic_state /localization/acceleration /vehicle/status/steering_status /control/command/control_cmd /control/trajectory_follower/control_cmd /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw /system/operation_mode/state /vehicle/status/control_mode /sensing/imu/imu_data /debug_mpc_x_des /debug_mpc_y_des /debug_mpc_v_des /debug_mpc_yaw_des /debug_mpc_acc_des /debug_mpc_steer_des /debug_mpc_X_des_converted /debug_mpc_x_current /debug_mpc_error_prediction /debug_mpc_max_trajectory_err /debug_mpc_emergency_stop_mode /debug_mpc_goal_stop_mode /debug_mpc_total_ctrl_time /debug_mpc_calc_u_opt_time ``` -Move [rosbag2.bash](./smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash) to the rosbag directory recorded above and execute the following command on the directory +Move [rosbag2.bash](./autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash) to the rosbag directory recorded above and execute the following command on the directory ```bash bash rosbag2.bash @@ -75,7 +75,7 @@ This converts rosbag data into CSV format for training models. Instead, the same result can be obtained by executing the following command in a python environment: ```python -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model model_trainer = train_drive_NN_model.train_drive_NN_model() model_trainer.transform_rosbag_to_csv(rosbag_dir) ``` @@ -86,7 +86,7 @@ At this time, all CSV files in `rosbag_dir` are automatically deleted first. The paths of the rosbag directories used for training, `dir_0`, `dir_1`, `dir_2`,... and the directory `save_dir` where you save the models, the model can be saved in the python environment as follows: ```python -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model model_trainer = train_drive_NN_model.train_drive_NN_model() model_trainer.add_data_from_csv(dir_0) model_trainer.add_data_from_csv(dir_1) @@ -112,12 +112,12 @@ If only polynomial regression is performed and no NN model is used, run the foll model_trainer.get_trained_model(use_polynomial_reg=True,force_NN_model_to_zero=True) ``` -Move `model_for_test_drive.pth` and `polynomial_reg_info.npz` saved in `save_dir` to the home directory and set `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml) to `true` to reflect the trained model in the control. +Move `model_for_test_drive.pth` and `polynomial_reg_info.npz` saved in `save_dir` to the home directory and set `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) to `true` to reflect the trained model in the control. ### Performance evaluation Here, as an example, we describe the verification of the adaptive performance when the wheel base of the sample_vehicle is 2.79 m, but an incorrect value of 2.0 m is given to the controller side. -To give the controller 2.0 m as the wheel base, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.0, and run the following command: +To give the controller 2.0 m as the wheel base, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.0, and run the following command: ```bash python3 -m smart_mpc_trajectory_follower.clear_pycache @@ -125,7 +125,7 @@ python3 -m smart_mpc_trajectory_follower.clear_pycache #### Test on autoware -To perform a control test on autoware with the nominal model before training, make sure that `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml) is `false` and launch autoware in the manner described in "Trajectory following control based on iLQR/MPPI". This time, the following route will be used for the test: +To perform a control test on autoware with the nominal model before training, make sure that `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) is `false` and launch autoware in the manner described in "Trajectory following control based on iLQR/MPPI". This time, the following route will be used for the test:

@@ -136,7 +136,7 @@ Record rosbag and train the model in the manner described in "Training of model To control using the trained model obtained here, set `trained_model_parameter:control_application:use_trained_model` to `true`, start autoware in the same way, and drive the same route recording rosbag. After the driving is complete, convert the rosbag file to CSV format using the method described in "Training of model and reflection in control". -A plot of the lateral deviation is obtained by running the `lateral_error_visualize` function in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb` for the nominal and training model rosbag files `rosbag_nominal` and `rosbag_trained`, respectively, as follows: +A plot of the lateral deviation is obtained by running the `lateral_error_visualize` function in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb` for the nominal and training model rosbag files `rosbag_nominal` and `rosbag_trained`, respectively, as follows: ```python lateral_error_visualize(dir_name=rosbag_nominal,ylim=[-1.2,1.2]) @@ -152,7 +152,7 @@ The following results were obtained. #### Test on python simulator -First, to give wheel base 2.79 m in the python simulator, create the following file and save it in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator` with the name `sim_setting.json`: +First, to give wheel base 2.79 m in the python simulator, create the following file and save it in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` with the name `sim_setting.json`: ```json { "wheel_base": 2.79 } @@ -162,7 +162,7 @@ Next, run the following commands to test the slalom driving on the python simula ```python import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model initial_error = [0.0, 0.03, 0.01, -0.01, 0.0, 0.0] save_dir = "test_python_sim" python_simulator.slalom_drive(save_dir=save_dir,use_trained_model=False,initial_error=initial_error) @@ -172,7 +172,7 @@ Here, `initial_error` is the initial error from the target trajectory, in the or and `save_dir` is the directory where the driving test results are saved. > [!NOTE] -> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). +> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). Run the following commands to perform training using driving data of the nominal model. @@ -211,21 +211,21 @@ It can be seen that the lateral deviation has improved significantly. Here we have described wheel base, but the parameters that can be passed to the python simulator are as follows. -| Parameter | Type | Description | -| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| steer_bias | float | steer bias [rad] | -| steer_rate_lim | float | steer rate limit [rad/s] | -| vel_rate_lim | float | acceleration limit [m/s^2] | -| wheel_base | float | wheel base [m] | -| steer_dead_band | float | steer dead band [rad] | -| adaptive_gear_ratio_coef | list[float] | List of floats of length 6 specifying information on speed-dependent gear ratios from tire angle to steering wheel angle. | -| acc_time_delay | float | acceleration time delay [s] | -| steer_time_delay | float | steer time delay [s] | -| acc_time_constant | float | acceleration time constant [s] | -| steer_time_constant | float | steer time constant [s] | -| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. | -| acc_scaling | float | acceleration scaling | -| steer_scaling | float | steer scaling | +| Parameter | Type | Description | +| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| steer_bias | float | steer bias [rad] | +| steer_rate_lim | float | steer rate limit [rad/s] | +| vel_rate_lim | float | acceleration limit [m/s^2] | +| wheel_base | float | wheel base [m] | +| steer_dead_band | float | steer dead band [rad] | +| adaptive_gear_ratio_coef | list[float] | List of floats of length 6 specifying information on speed-dependent gear ratios from tire angle to steering wheel angle. | +| acc_time_delay | float | acceleration time delay [s] | +| steer_time_delay | float | steer time delay [s] | +| acc_time_constant | float | acceleration time constant [s] | +| steer_time_constant | float | steer time constant [s] | +| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. | +| acc_scaling | float | acceleration scaling | +| steer_scaling | float | steer scaling | For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad] of steer dead band, edit the `sim_setting.json` as follows. @@ -237,13 +237,13 @@ For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad Here, we describe a method for testing adaptive performance by giving the simulation side a predefined range of model parameters while the control side is given constant model parameters. -First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command: +First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command: ```bash python3 -m smart_mpc_trajectory_follower.clear_pycache ``` -To run a driving experiment within the parameter change range set in [run_sim.py](./smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator` and run the following command: +To run a driving experiment within the parameter change range set in [run_sim.py](./autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` and run the following command: ```bash python3 run_sim.py --param_name steer_bias @@ -270,11 +270,11 @@ In `run_sim.py`, the following parameters can be set: | USE_INTERCEPT | bool | When a polynomial regression including bias is performed, whether to use or discard the resulting bias information.
It is meaningful only if FIT_INTERCEPT is True.
If it is False, discard the bias in the polynomial regression in the hope that the NN model can remove the bias term, even if the polynomial regression is performed with the bias included. | > [!NOTE] -> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). +> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). ## Change of nominal parameters and their reloading -The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml). +The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml). After changing the nominal parameters, the cache must be deleted by running the following command: ```bash @@ -293,7 +293,7 @@ The nominal parameters include the following: ## Change of control parameters and their reloading -The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). +The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). Although it is possible to reflect parameter changes by restarting autoware, the following command allows us to do so without leaving autoware running: ```bash diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py similarity index 88% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py index fafcb5bdecf05..2458de3a80f99 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py @@ -16,10 +16,10 @@ from pathlib import Path import shutil -import smart_mpc_trajectory_follower +import autoware_smart_mpc_trajectory_follower if __name__ == "__main__": - package_dir = str(Path(smart_mpc_trajectory_follower.__file__).parent) + package_dir = str(Path(autoware_smart_mpc_trajectory_follower.__file__).parent) remove_dirs = [ package_dir + "/__pycache__", diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py index d706c342f04ec..0eaa6ce1c0fbf 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py @@ -18,14 +18,14 @@ import datetime import os +from autoware_smart_mpc_trajectory_follower.scripts import drive_controller +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions import matplotlib.pyplot as plt from numba import njit import numpy as np import pandas as pd import scipy.interpolate import simplejson as json -from smart_mpc_trajectory_follower.scripts import drive_controller -from smart_mpc_trajectory_follower.scripts import drive_functions print("\n\n### import python_simulator.py ###") diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py similarity index 92% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py index 6b477bb804ac9..68d8a9f64e73f 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model import numpy as np import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model initial_error = np.array( [0.001, 0.03, 0.01, -0.001, 0, 2 * python_simulator.measurement_steer_bias] diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py index 3125719556559..d2bb4ea26f0a6 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py @@ -23,9 +23,9 @@ import traceback from typing import Dict +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model import numpy as np import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model parser = argparse.ArgumentParser() parser.add_argument("--param_name", default=None) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py index e75eedcb4bf65..4d36afd3d4324 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py @@ -16,9 +16,9 @@ from functools import partial +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions sqrt_mpc_time_step = np.sqrt(drive_functions.mpc_time_step) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py index f35789ee39fd3..f13aa07b0f795 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py @@ -13,9 +13,9 @@ # limitations under the License. +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.scripts import proxima_calc import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.scripts import proxima_calc import torch from torch import nn diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py index b21fcf72da931..228b0c37c5e7b 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py @@ -21,14 +21,14 @@ import threading import time +from autoware_smart_mpc_trajectory_follower.scripts import drive_GP +from autoware_smart_mpc_trajectory_follower.scripts import drive_NN +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.scripts import drive_iLQR +from autoware_smart_mpc_trajectory_follower.scripts import drive_mppi import numpy as np import scipy.interpolate # type: ignore from sklearn.preprocessing import PolynomialFeatures -from smart_mpc_trajectory_follower.scripts import drive_GP -from smart_mpc_trajectory_follower.scripts import drive_NN -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.scripts import drive_iLQR -from smart_mpc_trajectory_follower.scripts import drive_mppi import torch ctrl_index_for_polynomial_reg = np.concatenate( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py index c11b5c886b49a..fb96e134f7746 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py @@ -31,7 +31,9 @@ with open(package_path_json, "r") as file: package_path = json.load(file) -mpc_param_path = package_path["path"] + "/smart_mpc_trajectory_follower/param/mpc_param.yaml" +mpc_param_path = ( + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml" +) with open(mpc_param_path, "r") as yml: mpc_param = yaml.safe_load(yml) @@ -101,7 +103,7 @@ cap_pred_error = np.array(mpc_param["mpc_parameter"]["preprocessing"]["cap_pred_error"]) nominal_param_path = ( - package_path["path"] + "/smart_mpc_trajectory_follower/param/nominal_param.yaml" + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml" ) with open(nominal_param_path, "r") as yml: nominal_param = yaml.safe_load(yml) @@ -172,7 +174,7 @@ mpc_param["mpc_parameter"]["preprocessing"]["sg_window_size_for_nominal_inputs"] ) trained_model_param_path = ( - package_path["path"] + "/smart_mpc_trajectory_follower/param/trained_model_param.yaml" + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml" ) with open(trained_model_param_path, "r") as yml: trained_model_param = yaml.safe_load(yml) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py index 261a72e680778..2307586f23552 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py @@ -21,9 +21,9 @@ import time from typing import Callable +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions index_cost = np.concatenate( ( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py index ca6e6f15f42ef..5e88c79172097 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py @@ -16,9 +16,9 @@ from typing import Callable +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions index_cost = np.concatenate( ( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/proxima_calc.cpp b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/proxima_calc.cpp rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 992a5fd53a10c..78a120fe601f2 100755 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -22,6 +22,8 @@ from autoware_control_msgs.msg import Control from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint +from autoware_smart_mpc_trajectory_follower.scripts import drive_controller +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from autoware_vehicle_msgs.msg import SteeringReport from builtin_interfaces.msg import Duration from geometry_msgs.msg import AccelWithCovarianceStamped @@ -34,8 +36,6 @@ import scipy.interpolate from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp -from smart_mpc_trajectory_follower.scripts import drive_controller -from smart_mpc_trajectory_follower.scripts import drive_functions from std_msgs.msg import String from tier4_debug_msgs.msg import BoolStamped from tier4_debug_msgs.msg import Float32MultiArrayStamped diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py index 3f6ef7a9f78d1..5338f3cbaddce 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py @@ -23,11 +23,11 @@ import os from pathlib import Path +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions import numpy as np import scipy.interpolate from scipy.ndimage import gaussian_filter from scipy.spatial.transform import Rotation -from smart_mpc_trajectory_follower.scripts import drive_functions def data_smoothing(data: np.ndarray, sigma: float) -> np.ndarray: @@ -76,7 +76,7 @@ def transform_rosbag_to_csv(self, dir_name: str, delete_csv_first: bool = True) os.system( "cp " + package_path["path"] - + "/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash " + + "/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash " + dir_name ) os.chdir(dir_name) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py index 269b577f83c25..449ddda45f460 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py @@ -15,13 +15,15 @@ # cspell: ignore optim savez suptitle """Class for training neural nets from driving data.""" +from autoware_smart_mpc_trajectory_follower.scripts import drive_NN +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.training_and_data_check import ( + add_training_data_from_csv, +) import matplotlib.pyplot as plt import numpy as np from sklearn import linear_model from sklearn.preprocessing import PolynomialFeatures -from smart_mpc_trajectory_follower.scripts import drive_NN -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.training_and_data_check import add_training_data_from_csv import torch from torch import nn from torch.utils.data import DataLoader diff --git a/control/smart_mpc_trajectory_follower/images/autoware_smart_mpc.png b/control/autoware_smart_mpc_trajectory_follower/images/autoware_smart_mpc.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/autoware_smart_mpc.png rename to control/autoware_smart_mpc_trajectory_follower/images/autoware_smart_mpc.png diff --git a/control/smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png diff --git a/control/smart_mpc_trajectory_follower/images/lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/lateral_error_trained_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png diff --git a/control/smart_mpc_trajectory_follower/images/proxima_logo.png b/control/autoware_smart_mpc_trajectory_follower/images/proxima_logo.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/proxima_logo.png rename to control/autoware_smart_mpc_trajectory_follower/images/proxima_logo.png diff --git a/control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png diff --git a/control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png diff --git a/control/smart_mpc_trajectory_follower/images/test_route.png b/control/autoware_smart_mpc_trajectory_follower/images/test_route.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/test_route.png rename to control/autoware_smart_mpc_trajectory_follower/images/test_route.png diff --git a/control/smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml similarity index 96% rename from control/smart_mpc_trajectory_follower/package.xml rename to control/autoware_smart_mpc_trajectory_follower/package.xml index b25a7e2ce8dd6..a05f06cd865ff 100644 --- a/control/smart_mpc_trajectory_follower/package.xml +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -1,7 +1,7 @@ - smart_mpc_trajectory_follower + autoware_smart_mpc_trajectory_follower 1.0.0 Nodes to follow a trajectory by generating control commands using smart mpc diff --git a/control/smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py similarity index 60% rename from control/smart_mpc_trajectory_follower/setup.py rename to control/autoware_smart_mpc_trajectory_follower/setup.py index 4dca4d72929a8..fee1d04e826c0 100644 --- a/control/smart_mpc_trajectory_follower/setup.py +++ b/control/autoware_smart_mpc_trajectory_follower/setup.py @@ -13,25 +13,25 @@ os.system("pip3 install torch") package_path = {} package_path["path"] = str(Path(__file__).parent) -with open("smart_mpc_trajectory_follower/package_path.json", "w") as f: +with open("autoware_smart_mpc_trajectory_follower/package_path.json", "w") as f: json.dump(package_path, f) build_cpp_command = "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) " -build_cpp_command += "smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " -build_cpp_command += ( - "-o smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " -) +build_cpp_command += "autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " +build_cpp_command += "-o autoware_smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " build_cpp_command += "-lrt -I/usr/include/eigen3" os.system(build_cpp_command) so_path = ( "scripts/" - + glob.glob("smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[-1] + + glob.glob("autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[ + -1 + ] ) setup( name="smart_mpc_trajectory_follower", version="1.0.0", packages=find_packages(), package_data={ - "smart_mpc_trajectory_follower": ["package_path.json", so_path], + "autoware_smart_mpc_trajectory_follower": ["package_path.json", so_path], }, ) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 239a65f8260bf..d9e6e3e6757d6 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -432,7 +432,7 @@ def launch_setup(context, *args, **kwargs): ) smart_mpc_trajectory_follower = Node( - package="smart_mpc_trajectory_follower", + package="autoware_smart_mpc_trajectory_follower", executable="pympc_trajectory_follower.py", name="pympc_trajectory_follower", )