feat: add control_launch package (autowarefoundation#157)

* release v0.4.0

* load velocity_controller_param.yaml from launcher (autowarefoundation#52)

Signed-off-by: Shinnosuke Hirakawa <shinnosuke.hirakawa@tier4.jp>

* Include enable_slope_compensation to yaml file (autowarefoundation#61)

* Sync mpc param (autowarefoundation#74)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* add stop dist param (autowarefoundation#73)

* add stop dist param

* add keep stopping dist param

* removed ROS1 package

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "removed ROS1 package"

This reverts commit 85eb34207aa7b90314d93a6eb370213e10fcd7bd.

* add COLCON_IGNORE file to all ROS1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* rename *.launch files to *.launch.xml

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* port control launch (#7)

* port control launch

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix license

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* ROS2 Linting: control_launch (autowarefoundation#29)

* Add linter tests and completepackage dependencies list

* Reorder alphabetically

* Comment out pure_pursuit dependency - not ported yet

* Add back pure_pursuit package post port

* Ros2 port autoware launch (autowarefoundation#35)

* [autoware_launch] port CMakelists.txt and remove COLCON_IGNORE

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [autoware_launch] fix planning_simulator.launch.xml

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [autoware_launch] add rviz config

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [autoware_launch] first port of autoware_launch

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* lanuch rviz with config file

Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp>

* modify launch file for making psim work

Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp>

* add vehicle model in launch

Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp>

* change from vehicle_model to vehicle_param_file

Signed-off-by: kosuke murakami <kosuke.murakami@tier4.jp>

* [autoware_launch] add autoware_web_controller and system launch

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add rosbrdige_suite to build_depends.repos

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Update rviz

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* update autoware.rviz

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* remove autoware_ros2.rviz

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

Co-authored-by: kosuke murakami <kosuke.murakami@tier4.jp>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Ros2 v0.8.0 control launch (autowarefoundation#53)

* restore file name for v0.8.0 update

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Add use_emergency_handling arg to vehicle_cmd_gate.launch (autowarefoundation#83)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add parameter for using steer prediction (autowarefoundation#88)

* change stop dist param (autowarefoundation#85)

* mpc: change param name steer_rate_lim_deg -> steer_rate_lim_dps (autowarefoundation#79)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add mpc parameter (autowarefoundation#105)

* Add parameters for stop state (autowarefoundation#126)

* Add parameters for stop state

* Change default value

* Add vehicle cmd gate config (autowarefoundation#136)

* Add config_file of vehicle_cmd_gate

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add use_emergency_stop

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename emergency_stop to external_emergency_stop

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix command_gate diag

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add lane_departure_checker (autowarefoundation#123)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Revert "restore file name for v0.8.0 update"

This reverts commit 516b366819f7f9d69b9bc3e2de180d4523794bcd.

* fix control launch

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix args

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix param type

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* fix lane departure checker

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* [velocity controller]: Fix parameter type (autowarefoundation#62)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Rename ROS-related .yaml to .param.yaml (autowarefoundation#65)

* Rename ROS-related .yaml to .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add missing '--'

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename vehicle_info.yaml to vehicle_info.param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix livox param name

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ros2 update v0.9.0 (autowarefoundation#67)

* Add pose history into rviz config

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* add blocked by obstacle option (autowarefoundation#164)

* fix tab name (autowarefoundation#166)

* disenable ndt visualization (autowarefoundation#169)

* disenable ndt visualization

* change alpha

* dont visualize position covariance

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* change global frame to map (autowarefoundation#171)

* add param (autowarefoundation#156)

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* Fix typo in control launch (autowarefoundation#91)

* Fix typo in control module

* change admissible_yaw_error -> admissible_yaw_error_rad

* Fix various typos in launch files (autowarefoundation#97)

* add use_sim-time option (autowarefoundation#99)

* Replace doc by description (autowarefoundation#182)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Sync public repo (autowarefoundation#185)

* add tier4 usbcam (autowarefoundation#104)

* add tier4 usbcam

* change version

* tier4/ros2

* Ros2 vehicle info param server (autowarefoundation#96)

* add vehicle info param server

* delete vehicle param file

Co-authored-by: taikitanaka <ttatcoder@outlook.jp>

* Ros2 fix topic name part2 (autowarefoundation#89)

* Fix topic name for traffic_light_classifier

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name for traffic_light_visualization

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name for traffic_light_ssd_fine_detector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: taikitanaka <ttatcoder@outlook.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

* Use set_parameter for use_sim_time (autowarefoundation#198)

* Use set_parameter for use_sim_time

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add default parameter for scenario simulator

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add control composed launch (autowarefoundation#201)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix mistakes in control.launch.py (autowarefoundation#209)

* Fix typo in control.launch.py

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add remote_cmd_converter_component

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Feature/add external cmd selector (autowarefoundation#206)

* Add remote_cmd_selector to control.launch (autowarefoundation#172)

* Add remote_cmd_selector to control.launch

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename remote_cmd_selector to external_cmd_selector

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Change default external cmd source (autowarefoundation#173)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* modify to use control.launch.xml

* add external_cmd_selector to control.launch.py

* Fix remapping in control.launch.py

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix remapping in control.launch.py again

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Keisuke Shima <keisuke.shima@tier4.jp>

* Format launch files (autowarefoundation#228)

* Format launch files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Format launch.py

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix lint

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* add external commands (autowarefoundation#210)

* add remote commands

* fix topic

* ToExternalCmd

* fix topic in remote cmd converter

* fix ament

* add description for control_launch (autowarefoundation#330)

* add description

* fix sentence

* use of

* add README.md and svg files (autowarefoundation#328)

* add md and svg

* fix typo

* fix typo

* fix word

* fix typo

* add lack of things

* Update README

* fix depending packages

* fix word

* Feature/expand footprint launcher (autowarefoundation#318)

* fix typo in control.launch.py autowarefoundation#363

* Fix description in control launch arg (autowarefoundation#364)

* Add autoware api (autowarefoundation#376)

* Add external api adaptor (autowarefoundation#267)

* Add external api adaptor

* Add api adaptor to logging simulator

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add engage status output

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add internal api adaptor (autowarefoundation#273)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add map hash generator (autowarefoundation#319)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add autoware api launch (autowarefoundation#326)

* Add autoware api launch

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Apply autoware api launch

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add deprecated comment

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Remove unused parameter (autowarefoundation#325)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add api parameter (autowarefoundation#341)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add start request API (autowarefoundation#321)

* Add use start request option

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix lint

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Feature external cmd selector heartbeat (autowarefoundation#356)

* Rename external command topic

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Modify command topic name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix topic name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Rename remote_cmd_converter

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Remove gate mode from external command

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix latest external command name (autowarefoundation#361)

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix merge conflict

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* change external traffic light topic name in behavior velocity planner (autowarefoundation#310)

* fix topic

* change internal topic name

Co-authored-by: yabuta <makoto.yabuta@tier4.jp>

* Move launch file of external_cmd_selector (autowarefoundation#386)

* Move launch file of external_cmd_selector

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add utility function to include launch.py

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Add missing parameter

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix external command api name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* move common parameters

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Fix format

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* Use EmergencyState instead of deprecated EmergencyMode (autowarefoundation#390)

* Use EmergencyState instead of deprecated EmergencyMode

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Update control.launch.py

* Add selected external command API  (autowarefoundation#397)

* Fix pre-commit (autowarefoundation#407)

* Fix pre-commit errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix package.xml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add emergency status API (autowarefoundation#439)

* remove yaw-recalc param in mpc (autowarefoundation#190) (autowarefoundation#294)

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* Master sync for velocity controller (autowarefoundation#297)

* update velocity_controller_param.yaml for refactoring velocity_controller (autowarefoundation#238)

* update velocity_controller_param.yaml (autowarefoundation#247)

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>

* Fix parameter type (autowarefoundation#299)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* add smoothing_num parameter (autowarefoundation#263) (autowarefoundation#301)

* add smoothing_num parameter

* change param

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* add stopped_jerk (autowarefoundation#276) (autowarefoundation#315)

* add stopped_jerk

* set stopped_jerk -5.0

Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>

* Rename param from smooth_stop_max_fast_vel to smooth_stop_min_fast_vel (autowarefoundation#351)

* Feature/vehicle gate parametrize acc (autowarefoundation#320)

* [vehicle_cmd_gate] add acc parameters

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* fix typo

Signed-off-by: TakaHoribe <horibe.takamasa@gmail.com>

* Update control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* Change formatter to black (autowarefoundation#488)

* Update pre-commit settings

* Apply Black

* Replace ament_lint_common with autoware_lint_common

* Update build_depends.repos

* Fix build_depends

* twist -> odometry (autowarefoundation#109)

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* remove unused depend/launcher (autowarefoundation#112)

* Auto/control psim (autowarefoundation#113)

* succeeded in launching control nodes

* fix input/output of control_launch

* Remove multiple package

* Add newline

* Update control_launch/launch/control.launch.py

Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* Apply pre-commit

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* Fix remapping in control.launch.py (autowarefoundation#115)

* Fix remappings

* Add comment

* Fix remappings (autowarefoundation#116)

* fix topics

* add external hazard indicator

* add param (autowarefoundation#126)

* fix yaml (autowarefoundation#127)

* remove unused files

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* add default parameter of vehicle information

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

* ci(pre-commit): autofix

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: shin <8327162+0x126@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: Jilada Eccleston <jilada.eccleston@tier4.jp>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: kosuke murakami <kosuke.murakami@tier4.jp>
Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: taikitanaka <ttatcoder@outlook.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Keisuke Shima <keisuke.shima@tier4.jp>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: yabuta <makoto.yabuta@tier4.jp>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Maxime CLEMENT <maxime.clement@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

launch/control_launch/CMakeLists.txt

@@ -0,0 +1,23 @@
+cmake_minimum_required(VERSION 3.5)
+project(control_launch)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  config
+  launch
+)

launch/control_launch/README.md

@@ -0,0 +1,24 @@
+# control_launch
+
+## Structure
+
+![control_launch](./control_launch.drawio.svg)
+
+## Package Dependencies
+
+Please see `<exec_depend>` in `package.xml`.
+
+## Usage
+
+You can include as follows in `*.launch.xml` to use `control.launch.py`.
+
+```xml
+<include file="$(find-pkg-share control_launch)/launch/control.launch.py">
+  <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
+  <arg name="lateral_controller_mode" value="mpc_follower" />
+</include>
+```
+
+## Notes
+
+For reducing processing load, we use the [Component](https://docs.ros.org/en/galactic/Concepts/About-Composition.html) feature in ROS2 (similar to Nodelet in ROS1 )

launch/control_launch/config/trajectory_follower/lateral_controller.param.yaml

@@ -0,0 +1,71 @@
+/**:
+  ros__parameters:
+
+    # -- system --
+    ctrl_period: 0.03               # control period [s]
+    traj_resample_dist: 0.1         # path resampling interval [m]
+    use_steer_prediction: false     # flag for using steer prediction (do not use steer measurement)
+    admissible_position_error: 5.0  # stop mpc calculation when error is larger than the following value
+    admissible_yaw_error_rad: 1.57  # stop mpc calculation when error is larger than the following value
+
+    # -- path smoothing --
+    enable_path_smoothing: false   # flag for path smoothing
+    path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
+    curvature_smoothing_num_traj: 1         # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
+    curvature_smoothing_num_ref_steer: 15   # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
+
+    # -- mpc optimization --
+    qp_solver_type: "osqp"                       # optimization solver option (unconstraint_fast or osqp)
+    mpc_prediction_horizon: 50                   # prediction horizon step
+    mpc_prediction_dt: 0.1                       # prediction horizon period [s]
+    mpc_weight_lat_error: 0.1                    # lateral error weight in matrix Q
+    mpc_weight_heading_error: 0.0                # heading error weight in matrix Q
+    mpc_weight_heading_error_squared_vel: 0.3    # heading error * velocity weight in matrix Q
+    mpc_weight_steering_input: 1.0               # steering error weight in matrix R
+    mpc_weight_steering_input_squared_vel: 0.25  # steering error * velocity weight in matrix R
+    mpc_weight_lat_jerk: 0.0                     # lateral jerk weight in matrix R
+    mpc_weight_steer_rate: 0.0                   # steering rate weight in matrix R
+    mpc_weight_steer_acc: 0.000001               # steering angular acceleration weight in matrix R
+    mpc_low_curvature_weight_lat_error: 0.1                    # lateral error weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_heading_error: 0.0                # heading error weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_heading_error_squared_vel: 0.3    # heading error * velocity weight in matrix Q in low curvature point
+    mpc_low_curvature_weight_steering_input: 1.0               # steering error weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steering_input_squared_vel: 0.25  # steering error * velocity weight in matrix R in low curvature point
+    mpc_low_curvature_weight_lat_jerk: 0.0                     # lateral jerk weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steer_rate: 0.0                   # steering rate weight in matrix R in low curvature point
+    mpc_low_curvature_weight_steer_acc: 0.000001               # steering angular acceleration weight in matrix R in low curvature point
+    mpc_low_curvature_thresh_curvature: 0.0                    # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03)
+    mpc_weight_terminal_lat_error: 1.0           # terminal lateral error weight in matrix Q to improve mpc stability
+    mpc_weight_terminal_heading_error: 0.1       # terminal heading error weight in matrix Q to improve mpc stability
+    mpc_zero_ff_steer_deg: 0.5                   # threshold that feed-forward angle becomes zero
+    mpc_acceleration_limit: 2.0                  # limit on the vehicle's acceleration
+    mpc_velocity_time_constant: 0.3              # time constant used for velocity smoothing
+
+    # -- vehicle model --
+    vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
+    input_delay: 0.24                # steering input delay time for delay compensation
+    vehicle_model_steer_tau: 0.1     # steering dynamics time constant (1d approximation) [s]
+    steer_lim_deg: 20.0              # steering angle limit [deg]
+    steer_rate_lim_dps: 600.0        # steering angle rate limit [deg/s]
+    acceleration_limit: 2.0          # acceleration limit for trajectory velocity modification [m/ss]
+    velocity_time_constant: 0.3      # velocity dynamics time constant  for trajectory velocity modification [s]
+
+    # -- lowpass filter for noise reduction --
+    steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz]
+    error_deriv_lpf_cutoff_hz: 5.0
+
+    # stop state
+    stop_state_entry_ego_speed: 0.2
+    stop_state_entry_target_speed: 0.5
+
+    # vehicle parameters
+    vehicle:
+      cg_to_front_m: 1.228
+      cg_to_rear_m: 1.5618
+      mass_kg: 2400.0
+      mass_fl: 600.0
+      mass_fr: 600.0
+      mass_rl: 600.0
+      mass_rr: 600.0
+      cf: 155494.663
+      cr: 155494.663

launch/control_launch/config/trajectory_follower/latlon_muxer.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    timeout_thr_sec: 0.5 # [s] Time limit after which input messages are discarded.

launch/control_launch/config/trajectory_follower/longitudinal_controller.param.yaml

@@ -0,0 +1,84 @@
+/**:
+  ros__parameters:
+    control_rate: 30.0
+    delay_compensation_time: 0.17
+
+    enable_smooth_stop: true
+    enable_overshoot_emergency: true
+    enable_slope_compensation: false
+
+    # state transition
+    drive_state_stop_dist: 0.5
+    drive_state_offset_stop_dist: 1.0
+    stopping_state_stop_dist: 0.49
+    stopped_state_entry_vel: 0.1
+    stopped_state_entry_acc: 0.1
+    emergency_state_overshoot_stop_dist: 1.5
+    emergency_state_traj_trans_dev: 3.0
+    emergency_state_traj_rot_dev: 0.7
+
+    # drive state
+    kp: 1.0
+    ki: 0.1
+    kd: 0.0
+    max_out: 1.0
+    min_out: -1.0
+    max_p_effort: 1.0
+    min_p_effort: -1.0
+    max_i_effort: 0.3
+    min_i_effort: -0.3
+    max_d_effort: 0.0
+    min_d_effort: 0.0
+    lpf_vel_error_gain: 0.9
+    current_vel_threshold_pid_integration: 0.5
+    enable_brake_keeping_before_stop: false
+    brake_keeping_acc: -0.2
+
+    # smooth stop state
+    smooth_stop_max_strong_acc: -0.5
+    smooth_stop_min_strong_acc: -1.0
+    smooth_stop_weak_acc: -0.3
+    smooth_stop_weak_stop_acc: -0.8
+    smooth_stop_strong_stop_acc: -3.4
+    smooth_stop_max_fast_vel: 0.5
+    smooth_stop_min_running_vel: 0.01
+    smooth_stop_min_running_acc: 0.01
+    smooth_stop_weak_stop_time: 0.8
+    smooth_stop_weak_stop_dist: -0.3
+    smooth_stop_strong_stop_dist: -0.5
+
+    # stopped state
+    stopped_vel: 0.0
+    stopped_acc: -3.4
+    stopped_jerk: -5.0
+
+    # emergency state
+    emergency_vel: 0.0
+    emergency_acc: -5.0
+    emergency_jerk: -3.0
+
+    # acceleration limit
+    max_acc: 3.0
+    min_acc: -5.0
+
+    # jerk limit
+    max_jerk: 2.0
+    min_jerk: -5.0
+
+    # pitch
+    use_trajectory_for_pitch_calculation: false
+    lpf_pitch_gain: 0.95
+    max_pitch_rad: 0.1
+    min_pitch_rad: -0.1
+
+    # vehicle parameters
+    vehicle:
+      cg_to_front_m: 1.228
+      cg_to_rear_m: 1.5618
+      mass_kg: 2400.0
+      mass_fl: 600.0
+      mass_fr: 600.0
+      mass_rl: 600.0
+      mass_rr: 600.0
+      cf: 155494.663
+      cr: 155494.663

launch/control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml

@@ -0,0 +1,12 @@
+/**:
+  ros__parameters:
+    update_rate: 10.0
+    system_emergency_heartbeat_timeout: 0.5
+    external_emergency_stop_heartbeat_timeout: 0.0
+    stop_hold_acceleration: -1.5
+    emergency_acceleration: -2.4
+    vel_lim: 25.0
+    lon_acc_lim: 5.0
+    lon_jerk_lim: 5.0
+    lat_acc_lim: 5.0
+    lat_jerk_lim: 5.0