feat(autoware_launch, control_launch): separate control params for ve…

…hicle simulation (autowarefoundation#108)

* Separate control params for vehicle simulation

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Refactor params loading

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Format

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Format

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

autoware_launch/launch/planning_simulator.launch.xml

@@ -52,6 +52,7 @@
     <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
     <arg name="lateral_controller_mode" value="mpc_follower" />
     <arg name="lat_controller_param_path" value="$(find-pkg-share control_launch)/config/trajectory_follower/$(var lateral_controller_mode).param.yaml"/>
+    <arg name="vehicle_simulation" value="$(var vehicle_simulation)"/>
   </include>
 
   <!-- Autoware API -->

control_launch/config/trajectory_follower/longitudinal_controller_for_vehicle_sim.param.yaml

@@ -0,0 +1,72 @@
+/**:
+  ros__parameters:
+    control_rate: 30.0
+    delay_compensation_time: 0.0
+
+    enable_smooth_stop: false
+    enable_overshoot_emergency: true
+    enable_slope_compensation: true
+
+    # state transition
+    drive_state_stop_dist: 0.5
+    drive_state_offset_stop_dist: 1.0
+    stopping_state_stop_dist: 0.5
+    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.7854
+
+    # 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: true
+    brake_keeping_acc: -0.2
+
+    # smooth stop state
+    smooth_stop_max_strong_acc: -0.5
+    smooth_stop_min_strong_acc: -0.8
+    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: -0.3
+    stopped_jerk: -5.0
+
+    # emergency state
+    emergency_vel: 0.0
+    emergency_acc: -2.5
+    emergency_jerk: -1.5
+
+    # acceleration limit
+    max_acc: 1.0
+    min_acc: -2.5
+
+    # jerk limit
+    max_jerk: 2.0
+    min_jerk: -3.0
+
+    # pitch
+    use_trajectory_for_pitch_calculation: false
+    lpf_pitch_gain: 0.95
+    max_pitch_rad: 0.1
+    min_pitch_rad: -0.1

control_launch/config/trajectory_follower/mpc_follower_for_vehicle_sim.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.3     # steering dynamics time constant (1d approximation) [s]
+    steer_lim_deg: 28.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

control_launch/launch/control.launch.py

@@ -32,10 +32,23 @@
 
 
 def launch_setup(context, *args, **kwargs):
-    lat_controller_param_path = LaunchConfiguration("lat_controller_param_path").perform(context)
+    vehicle_simulation = LaunchConfiguration("vehicle_simulation").perform(context)
+    if vehicle_simulation == "true":
+        lat_controller_param_path = LaunchConfiguration(
+            "lat_controller_param_path_for_vehicle_sim"
+        ).perform(context)
+        lon_controller_param_path = LaunchConfiguration(
+            "lon_controller_param_path_for_vehicle_sim"
+        ).perform(context)
+    else:
+        lat_controller_param_path = LaunchConfiguration("lat_controller_param_path").perform(
+            context
+        )
+        lon_controller_param_path = LaunchConfiguration("lon_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)
@@ -306,6 +319,14 @@ def add_launch_arg(name: str, default_value=None, description=None):
         ],
         "path to the parameter file of lateral controller. default is `mpc_follower`",
     )
+    add_launch_arg(
+        "lat_controller_param_path_for_vehicle_sim",
+        [
+            FindPackageShare("control_launch"),
+            "/config/trajectory_follower/mpc_follower_for_vehicle_sim.param.yaml",
+        ],
+        "path to the parameter file of lateral controller for vehicle simulation. default is `mpc_follower`",
+    )
     add_launch_arg(
         "lon_controller_param_path",
         [
@@ -314,6 +335,14 @@ def add_launch_arg(name: str, default_value=None, description=None):
         ],
         "path to the parameter file of longitudinal controller",
     )
+    add_launch_arg(
+        "lon_controller_param_path_for_vehicle_sim",
+        [
+            FindPackageShare("control_launch"),
+            "/config/trajectory_follower/longitudinal_controller_for_vehicle_sim.param.yaml",
+        ],
+        "path to the parameter file of longitudinal controller for vehicle simulation",
+    )
     add_launch_arg(
         "latlon_muxer_param_path",
         [
@@ -335,6 +364,9 @@ def add_launch_arg(name: str, default_value=None, description=None):
         [FindPackageShare("lane_departure_checker"), "/config/lane_departure_checker.param.yaml"],
     )
 
+    # vehicle simulation mode
+    add_launch_arg("vehicle_simulation", "false", "run on vehicle simulator")
+
     # vehicle cmd gate
     add_launch_arg("use_emergency_handling", "true", "use emergency handling")
     add_launch_arg("use_external_emergency_stop", "true", "use external emergency stop")