feat(smart_mpc_trajectory_follower): add smart_mpc_trajectory_follower

control/smart_mpc_trajectory_follower/.gitignore

@@ -0,0 +1,3 @@
+# Files generated when installing smart_mpc_trajectory_follower
+build/
+*.egg-info/

control/smart_mpc_trajectory_follower/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 3.14)
+project(smart_mpc_trajectory_follower)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+install(PROGRAMS
+  smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+ament_auto_package(
+  INSTALL_TO_SHARE
+)

control/smart_mpc_trajectory_follower/package.xml

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>smart_mpc_trajectory_follower</name>
+  <version>1.0.0</version>
+  <description>Nodes to follow a trajectory by generating control commands using smart mpc</description>
+
+  <!-- mpc longitudinal controller -->
+  <maintainer email="[email protected]">Masayuki Aino</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Masayuki Aino</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_system_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>motion_utils</depend>
+  <depend>pybind11_vendor</depend>
+  <depend>python3-numba</depend>
+  <depend>python3-scipy</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <exec_depend>ros2launch</exec_depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_index_python</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+  <test_depend>autoware_testing</test_depend>
+  <test_depend>ros_testing</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

control/smart_mpc_trajectory_follower/setup.py

@@ -0,0 +1,36 @@
+import glob
+import json
+import os
+from pathlib import Path
+
+from setuptools import find_packages
+from setuptools import setup
+
+os.system("pip3 install numba==0.58.1 --force-reinstall")
+os.system("pip3 install pybind11")
+os.system("pip3 install GPy")
+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:
+    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 += "-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]
+)
+setup(
+    name="smart_mpc_trajectory_follower",
+    version="1.0.0",
+    packages=find_packages(),
+    package_data={
+        "smart_mpc_trajectory_follower": ["package_path.json", so_path],
+    },
+)

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore

@@ -0,0 +1,2 @@
+__pycache__/
+*.json

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py

@@ -0,0 +1,17 @@
+import os
+from pathlib import Path
+import shutil
+
+import smart_mpc_trajectory_follower
+
+if __name__ == "__main__":
+    package_dir = str(Path(smart_mpc_trajectory_follower.__file__).parent)
+
+    remove_dirs = [
+        package_dir + "/__pycache__",
+        package_dir + "/scripts/__pycache__",
+        package_dir + "/training_and_data_check/__pycache__",
+    ]
+    for i in range(len(remove_dirs)):
+        if os.path.isdir(remove_dirs[i]):
+            shutil.rmtree(remove_dirs[i])

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml

@@ -0,0 +1,56 @@
+mpc_parameter:
+  system:
+    mode: ilqr
+  mpc_setting:
+    ctrl_time_step: 0.03333
+    mpc_freq: 3
+    N: 50
+    steer_ctrl_queue_size: 50
+    steer_ctrl_queue_size_core: 15
+    acc_ctrl_queue_size: 12
+    nx_0: 6
+    nu_0: 2
+    timing_Q_c: [25]
+  cost_parameters:
+    Q: [0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    Q_c: [1e+2, 1e+8, 1e+6, 1e+3, 1.0, 1.0, 1.0, 1.0]
+    Q_f: [1e+2, 1e+8, 1e+2, 1e+8, 1.0, 1.0, 1.0, 1.0]
+    R: [10.0, 1000.0]
+    acc_lim_weight: 100.0
+    steer_lim_weight: 100.0
+    acc_rate_lim_weight: 10000.0
+    steer_rate_lim_weight: 10000.0
+    min_steer_rate_transform_for_start: 0.01
+    power_steer_rate_transform_for_start: 5
+    coef_steer_rate_transform_for_start: 3.0
+    min_loose_lateral_cost: 0.00001
+    power_loose_lateral_cost: 10
+    threshold_loose_lateral_cost: 0.2
+    min_loose_yaw_cost: 0.00001
+    power_loose_yaw_cost: 1
+    threshold_loose_yaw_cost: 0.1
+  ilqr:
+    ls_step: 0.9
+    max_iter_ls: 10
+    max_iter_ilqr: 1
+    ilqr_tol: 0.01
+  mppi:
+    lam: 0.1
+    Sigma: [1e-15, 0.01]
+    max_iter_mppi: 1
+    sample_num: 100
+    mppi_tol: 0.01
+    mppi_step: 20
+  preprocessing:
+    reference_horizon: 50
+    cap_pred_error: [0.5, 2.0]
+    use_sg_for_nominal_inputs: true
+    sg_deg_for_nominal_inputs: 0
+    sg_window_size_for_nominal_inputs: 10
+  to_be_deprecated:
+    tighten_horizon: 20
+    min_tighten_steer_rate: 1.0
+    power_tighten_steer_rate_by_lateral_error: 1
+    threshold_tighten_steer_rate_by_lateral_error: 0.05
+    power_tighten_steer_rate_by_yaw_error: 1
+    threshold_tighten_steer_rate_by_yaw_error: 0.05

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml

@@ -0,0 +1,10 @@
+# default parameter is set for sample_vehicle of AWF: https://github.com/autowarefoundation/sample_vehicle_launch/tree/main/sample_vehicle_description
+nominal_parameter:
+  vehicle_info:
+    wheel_base: 2.79
+  acceleration:
+    acc_time_delay: 0.1
+    acc_time_constant: 0.1
+  steering:
+    steer_time_delay: 0.27
+    steer_time_constant: 0.24

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml

@@ -0,0 +1,30 @@
+trained_model_parameter:
+  control_application:
+    use_trained_model: false
+    update_trained_model: false
+    max_train_data_size: 10000
+    error_decay: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
+    use_trained_model_diff: true
+    use_sg_for_trained_model_diff: true
+    sg_deg_for_trained_model_diff: 0
+    sg_window_size_for_trained_model_diff: 25
+    use_sg_for_noise: true
+    sg_deg_for_noise: 0
+    sg_window_size_for_noise: 15
+    use_x_noise: false
+    use_y_noise: false
+    use_v_noise: false
+    use_theta_noise: false
+    use_acc_noise: false
+    use_steer_noise: false
+  smoothing:
+    acc_sigma_for_learning: 5.0
+    steer_sigma_for_learning: 5.0
+    acc_des_sigma_for_learning: 5.0
+    steer_des_sigma_for_learning: 5.0
+    x_out_sigma_for_learning: 30.0
+    y_out_sigma_for_learning: 30.0
+    v_out_sigma_for_learning: 30.0
+    theta_out_sigma_for_learning: 10.0
+    acc_out_sigma_for_learning: 5.0
+    steer_out_sigma_for_learning: 5.0

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore

@@ -0,0 +1,13 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# workspace specific settings
+train_drive_*.png
+test_feedforward_*/
+test_pure_pursuit_*/
+test_python_*/
+python_sim_log_*/
+sim_setting.json
+auto_test_result_*.csv

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv

@@ -0,0 +1,20 @@
+default,  0.00,  1.39,  2.78,  4.17,  5.56,  6.94,  8.33,  9.72, 11.11, 12.50, 13.89, 15.28, 16.67
+-4.0,    -4.40, -4.36, -4.38, -4.12, -4.20, -3.94, -3.98, -3.80, -3.77, -3.76, -3.59, -3.50, -3.40
+-3.5,    -4.00, -3.91, -3.85, -3.64, -3.68, -3.55, -3.42, -3.24, -3.25, -3.00, -3.04, -2.93, -2.80
+-3.0,    -3.40, -3.37, -3.33, -3.00, -3.00, -2.90, -2.88, -2.65, -2.43, -2.44, -2.43, -2.39, -2.30
+-2.5,    -2.80, -2.72, -2.72, -2.62, -2.41, -2.43, -2.26, -2.18, -2.11, -2.03, -1.96, -1.91, -1.85
+-2.0,    -2.30, -2.24, -2.12, -2.02, -1.92, -1.81, -1.67, -1.58, -1.51, -1.49, -1.40, -1.35, -1.30
+-1.5,    -1.70, -1.61, -1.47, -1.46, -1.40, -1.37, -1.29, -1.24, -1.10, -0.99, -0.83, -0.80, -0.78
+-1.0,    -1.30, -1.28, -1.10, -1.09, -1.04, -1.02, -0.98, -0.89, -0.82, -0.61, -0.52, -0.54, -0.56
+-0.8,    -0.96, -0.90, -0.82, -0.74, -0.70, -0.65, -0.63, -0.59, -0.55, -0.44, -0.39, -0.39, -0.35
+-0.6,    -0.77, -0.71, -0.67, -0.65, -0.58, -0.52, -0.51, -0.50, -0.40, -0.33, -0.30, -0.31, -0.30
+-0.4,    -0.45, -0.40, -0.45, -0.44, -0.38, -0.35, -0.31, -0.30, -0.26, -0.30, -0.29, -0.31, -0.25
+-0.2,    -0.24, -0.24, -0.25, -0.22, -0.23, -0.25, -0.27, -0.29, -0.24, -0.22, -0.17, -0.18, -0.12
+ 0.0,     0.00,  0.00, -0.05, -0.05, -0.05, -0.05, -0.08, -0.08, -0.08, -0.08, -0.10, -0.10, -0.10
+ 0.2,     0.16,  0.12,  0.02,  0.02,  0.00,  0.00, -0.05, -0.05, -0.05, -0.05, -0.08, -0.08, -0.08
+ 0.4,     0.38,  0.30,  0.22,  0.25,  0.24,  0.23,  0.20,  0.16,  0.16,  0.14,  0.10,  0.05,  0.05
+ 0.6,     0.52,  0.52,  0.51,  0.49,  0.43,  0.40,  0.35,  0.33,  0.33,  0.33,  0.32,  0.34,  0.34
+ 0.8,     0.82,  0.81,  0.78,  0.68,  0.63,  0.56,  0.53,  0.48,  0.43,  0.41,  0.37,  0.38,  0.40
+ 1.0,     1.00,  1.08,  1.01,  0.88,  0.76,  0.69,  0.66,  0.58,  0.54,  0.49,  0.45,  0.40,  0.40
+ 1.5,     1.52,  1.50,  1.38,  1.26,  1.14,  1.03,  0.91,  0.82,  0.67,  0.61,  0.51,  0.41,  0.41
+ 2.0,     1.80,  1.80,  1.64,  1.43,  1.25,  1.11,  0.96,  0.81,  0.70,  0.59,  0.51,  0.42,  0.42