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>

control_launch/config/trajectory_follower/lateral_controller.param.yaml

@@ -0,0 +1,72 @@
+/**:
+  ros__parameters:
+
+    # -- system --
+    ctrl_period: 0.03               # control period [s]
+    traj_resample_dist: 0.1         # path resampling interval [m]
+    enable_yaw_recalculation: false # flag for recalculation of yaw angle after resampling
+    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: 15    # point-to-point index distance used in curvature calculation : 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
+    stop_state_keep_stopping_dist: 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

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.

control_launch/config/trajectory_follower/longitudinal_controller.param.yaml

@@ -0,0 +1,82 @@
+/**:
+  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
+
+    # 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

control_launch/launch/control.launch.py

@@ -19,7 +19,6 @@
 from launch.actions import OpaqueFunction
 from launch.actions import SetLaunchConfiguration
 from launch.conditions import IfCondition
-from launch.conditions import LaunchConfigurationEquals
 from launch.conditions import UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
@@ -32,17 +31,16 @@
 
 
 def launch_setup(context, *args, **kwargs):
-    mpc_follower_param_path = LaunchConfiguration("mpc_follower_param_path").perform(context)
-    with open(mpc_follower_param_path, "r") as f:
-        mpc_follower_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    pure_pursuit_param_path = LaunchConfiguration("pure_pursuit_param_path").perform(context)
-    with open(pure_pursuit_param_path, "r") as f:
-        pure_pursuit_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    velocity_controller_param_path = LaunchConfiguration("velocity_controller_param_path").perform(
-        context
-    )
-    with open(velocity_controller_param_path, "r") as f:
-        velocity_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    lat_controller_param_path = LaunchConfiguration("lat_controller_param_path").perform(context)
+    with open(lat_controller_param_path, "r") as f:
+        lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    lon_controller_param_path = LaunchConfiguration("lon_controller_param_path").perform(context)
+    with open(lon_controller_param_path, "r") as f:
+        lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    latlon_muxer_param_path = LaunchConfiguration("latlon_muxer_param_path").perform(context)
+    with open(latlon_muxer_param_path, "r") as f:
+        latlon_muxer_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
     vehicle_cmd_gate_param_path = LaunchConfiguration("vehicle_cmd_gate_param_path").perform(
         context
     )
@@ -53,54 +51,42 @@ def launch_setup(context, *args, **kwargs):
     ).perform(context)
     with open(lane_departure_checker_param_path, "r") as f:
         lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-    # mpc follower
-    mpc_follower_component = ComposableNode(
-        package="mpc_follower",
-        plugin="MPCFollower",
-        name="mpc_follower",
+
+    # lateral controller
+    lat_controller_component = ComposableNode(
+        package="trajectory_follower_nodes",
+        plugin="autoware::motion::control::trajectory_follower_nodes::LateralController",
+        name="lateral_controller_node_exe",
         namespace="trajectory_follower",
         remappings=[
             ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"),
-            ("~/input/current_velocity", "/localization/kinematic_state"),
+            ("~/input/current_odometry", "/localization/kinematic_state"),
             ("~/input/current_steering", "/vehicle/status/steering"),
-            ("~/output/control_raw", "lateral/control_cmd"),
-            ("~/output/predicted_trajectory", "predicted_trajectory"),
+            ("~/output/control_cmd", "lateral/control_cmd"),
+            ("~/output/predicted_trajectory", "lateral/predicted_trajectory"),
+            ("~/output/diagnostic", "lateral/diagnostic"),
         ],
         parameters=[
-            mpc_follower_param,
-        ],
-        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-    )
-    # pure pursuit
-    pure_pursuit_component = ComposableNode(
-        package="pure_pursuit",
-        plugin="PurePursuitNode",
-        name="pure_pursuit",
-        namespace="trajectory_follower",
-        remappings=[
-            ("input/reference_trajectory", "/planning/scenario_planning/trajectory"),
-            ("input/current_velocity", "/localization/kinematic_state"),
-            ("output/control_raw", "lateral/control_cmd"),
-        ],
-        parameters=[
-            pure_pursuit_param,
+            lat_controller_param,
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
-    # velocity controller
-    velocity_controller_component = ComposableNode(
-        package="velocity_controller",
-        plugin="VelocityController",
-        name="velocity_controller",
+    # longitudinal controller
+    lon_controller_component = ComposableNode(
+        package="trajectory_follower_nodes",
+        plugin="autoware::motion::control::trajectory_follower_nodes::LongitudinalController",
+        name="longitudinal_controller_node_exe",
         namespace="trajectory_follower",
         remappings=[
-            ("~/current_velocity", "/localization/kinematic_state"),
-            ("~/control_cmd", "longitudinal/control_cmd"),
-            ("~/current_trajectory", "/planning/scenario_planning/trajectory"),
+            ("~/input/current_trajectory", "/planning/scenario_planning/trajectory"),
+            ("~/input/current_odometry", "/localization/kinematic_state"),
+            ("~/output/control_cmd", "longitudinal/control_cmd"),
+            ("~/output/slope_angle", "longitudinal/slope_angle"),
+            ("~/output/diagnostic", "longitudinal/diagnostic"),
         ],
         parameters=[
-            velocity_controller_param,
+            lon_controller_param,
             {
                 "control_rate": LaunchConfiguration("control_rate"),
                 "show_debug_info": LaunchConfiguration("show_debug_info"),
@@ -113,15 +99,18 @@ def launch_setup(context, *args, **kwargs):
 
     # latlon muxer
     latlon_muxer_component = ComposableNode(
-        package="latlon_muxer",
-        plugin="LatLonMuxer",
-        name="latlon_muxer",
+        package="trajectory_follower_nodes",
+        plugin="autoware::motion::control::trajectory_follower_nodes::LatLonMuxer",
+        name="latlon_muxer_node_exe",
         namespace="trajectory_follower",
         remappings=[
             ("input/lateral/control_cmd", "lateral/control_cmd"),
             ("input/longitudinal/control_cmd", "longitudinal/control_cmd"),
             ("output/control_cmd", "control_cmd"),
         ],
+        parameters=[
+            latlon_muxer_param,
+        ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
@@ -229,24 +218,19 @@ def launch_setup(context, *args, **kwargs):
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
-            velocity_controller_component,
+            lon_controller_component,
             latlon_muxer_component,
             lane_departure_component,
             shift_decider_component,
             vehicle_cmd_gate_component,
         ],
     )
 
-    mpc_follower_loader = LoadComposableNodes(
-        composable_node_descriptions=[mpc_follower_component],
-        target_container=container,
-        condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc_follower"),
-    )
-
-    pure_pursuit_loader = LoadComposableNodes(
-        composable_node_descriptions=[pure_pursuit_component],
+    # lateral controller is separated since it may be another controller (e.g. pure pursuit)
+    lat_controller_loader = LoadComposableNodes(
+        composable_node_descriptions=[lat_controller_component],
         target_container=container,
-        condition=LaunchConfigurationEquals("lateral_controller_mode", "pure_pursuit"),
+        # condition=LaunchConfigurationEquals("lateral_controller_mode", "mpc"),
     )
 
     group = GroupAction(
@@ -255,8 +239,7 @@ def launch_setup(context, *args, **kwargs):
             container,
             external_cmd_selector_loader,
             external_cmd_converter_loader,
-            mpc_follower_loader,
-            pure_pursuit_loader,
+            lat_controller_loader,
         ]
     )
 
@@ -271,28 +254,35 @@ def add_launch_arg(name: str, default_value=None, description=None):
             DeclareLaunchArgument(name, default_value=default_value, description=description)
         )
 
+    # add_launch_arg(
+    #     "lateral_controller_mode",
+    #     "mpc_follower",
+    #     "lateral controller mode: `mpc_follower` or `pure_pursuit`",
+    # )
+
     add_launch_arg(
-        "lateral_controller_mode",
-        "mpc_follower",
-        "lateral controller mode: `mpc_follower` or `pure_pursuit`",
-    )
-    add_launch_arg(
-        "mpc_follower_param_path",
-        [FindPackageShare("control_launch"), "/config/mpc_follower/mpc_follower.param.yaml"],
-        "path to the parameter file of mpc_follower",
+        "lat_controller_param_path",
+        [
+            FindPackageShare("control_launch"),
+            "/config/trajectory_follower/lateral_controller.param.yaml",
+        ],
+        "path to the parameter file of lateral controller",
     )
     add_launch_arg(
-        "pure_pursuit_param_path",
-        [FindPackageShare("control_launch"), "/config/pure_pursuit/pure_pursuit.param.yaml"],
-        "path to the parameter file of pure_pursuit",
+        "lon_controller_param_path",
+        [
+            FindPackageShare("control_launch"),
+            "/config/trajectory_follower/longitudinal_controller.param.yaml",
+        ],
+        "path to the parameter file of longitudinal controller",
     )
     add_launch_arg(
-        "velocity_controller_param_path",
+        "latlon_muxer_param_path",
         [
             FindPackageShare("control_launch"),
-            "/config/velocity_controller/velocity_controller.param.yaml",
+            "/config/trajectory_follower/latlon_muxer.param.yaml",
         ],
-        "path to the parameter file of velocity controller",
+        "path to the parameter file of latlon muxer",
     )
     add_launch_arg(
         "vehicle_cmd_gate_param_path",