Merge branch 'main' into chore/prefix-add-vehicle_cmd_gate

.github/CODEOWNERS

@@ -57,7 +57,7 @@ control/pid_longitudinal_controller/** [email protected] takamasa.horibe@tie
 control/predicted_path_checker/** [email protected]
 control/pure_pursuit/** [email protected]
 control/shift_decider/** [email protected]
-control/smart_mpc_trajectory_follower/** [email protected]
+control/autoware_smart_mpc_trajectory_follower/** [email protected]
 control/trajectory_follower_base/** [email protected] [email protected]
 control/trajectory_follower_node/** [email protected] [email protected]
 control/autoware_vehicle_cmd_gate/** [email protected] [email protected]

control/smart_mpc_trajectory_follower/CMakeLists.txt → 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(

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py → 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__",

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py → 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 ###")
 

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py → 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]

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py → 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)

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py → 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)
 

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py → 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
 

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py → 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(

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py → 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)

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py → 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(
     (

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py → 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(
     (

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py → 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

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py → 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)

control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py → 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

control/smart_mpc_trajectory_follower/package.xml → control/autoware_smart_mpc_trajectory_follower/package.xml

@@ -1,7 +1,7 @@
 <?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>
+  <name>autoware_smart_mpc_trajectory_follower</name>
   <version>1.0.0</version>
   <description>Nodes to follow a trajectory by generating control commands using smart mpc</description>
 

control/smart_mpc_trajectory_follower/setup.py → 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],
     },
 )

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",
     )

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()