Merge remote-tracking branch 'origin/tier4/main' into sync-tier4-upst…

…ream

Signed-off-by: tomoya.kimura <[email protected]>

autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     use_predicted_trajectory: true
-    use_imu_path: true
+    use_imu_path: false
     voxel_grid_x: 0.05
     voxel_grid_y: 0.05
     voxel_grid_z: 100000.0
@@ -13,5 +13,5 @@
     a_obj_min: -1.0
     prediction_time_horizon: 1.5
     prediction_time_interval: 0.1
-    collision_keeping_sec: 0.0
+    collision_keeping_sec: 2.0
     aeb_hz: 10.0

autoware_launch/config/control/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml

@@ -1,12 +1,14 @@
+# These parameters are tuned for xx1
+
 /**:
   ros__parameters:
-    transition_timeout: 10.0
+    transition_timeout: 5.0
     frequency_hz: 10.0
 
     # set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted.
-    enable_engage_on_driving: false
+    enable_engage_on_driving: true
 
-    check_engage_condition: false # set false if you do not want to care about the engage condition.
+    check_engage_condition: true # set false if you do not want to care about the engage condition.
     nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
     nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
     engage_acceptable_limits:
@@ -19,8 +21,8 @@
       lateral_acc_threshold: 1.0
       lateral_acc_diff_threshold: 0.5
     stable_check:
-      duration: 0.1
-      dist_threshold: 1.5
+      duration: 3.0
+      dist_threshold: 0.5
       speed_upper_threshold: 2.0
       speed_lower_threshold: -2.0
       yaw_threshold: 0.262

autoware_launch/config/control/trajectory_follower/1/lateral/mpc.param.yaml

@@ -0,0 +1,76 @@
+/**:
+  ros__parameters:
+    # -- system --
+    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: true    # flag for path smoothing
+    path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
+    curvature_smoothing_num_traj: 15        # 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)
+
+    # -- trajectory extending --
+    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
+
+    # -- 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.2                    # 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.03   # 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: 1.00  # 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.05                   # 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.03   # 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: 2.00  # 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.03                   # 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
+    mpc_min_prediction_length: 5.0               # minimum prediction length
+
+    # -- vehicle model --
+    vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
+    input_delay: 0.1                 # steering input delay time for delay compensation
+    vehicle_model_steer_tau: 0.1     # steering dynamics time constant (1d approximation) [s]
+    steer_rate_lim_dps_list_by_curvature: [20.0, 20.0, 20.0]            # steering angle rate limit list depending on curvature [deg/s]
+    curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01]              # curvature list for steering angle rate limit interpolation in ascending order [/m]
+    steer_rate_lim_dps_list_by_velocity: [20.0, 20.0, 20.0]             # steering angle rate limit list depending on velocity [deg/s]
+    velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0]                  # velocity list for steering angle rate limit interpolation in ascending order [m/s]
+    acceleration_limit: 1.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: 10.0     # cutoff frequency of lowpass filter for steering command [Hz]
+    error_deriv_lpf_cutoff_hz: 5.0
+
+    # stop state: steering command is kept in the previous value in the stop state.
+    stop_state_entry_ego_speed: 0.2
+    stop_state_entry_target_speed: 0.2
+    converged_steer_rad: 0.1
+    keep_steer_control_until_converged: true
+    new_traj_duration_time: 1.0
+    new_traj_end_dist: 0.3
+    mpc_converged_threshold_rps:  0.01 # threshold of mpc convergence check [rad/s]
+
+    # steer offset
+    steering_offset:
+      enable_auto_steering_offset_removal: true
+      update_vel_threshold: 5.56
+      update_steer_threshold: 0.035
+      average_num: 1000
+      steering_offset_limit: 0.02

autoware_launch/config/control/trajectory_follower/1/longitudinal/pid.param.yaml

@@ -0,0 +1,74 @@
+/**:
+  ros__parameters:
+    delay_compensation_time: 0.40
+
+    enable_smooth_stop: true
+    enable_overshoot_emergency: false
+    enable_large_tracking_error_emergency: false
+    enable_slope_compensation: true
+    enable_keep_stopped_until_steer_convergence: 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_duration_time: 0.1
+    stopped_state_entry_vel: 0.01
+    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: 2.0
+    ki: 0.01
+    kd: 0.1
+    max_out: 1.0
+    min_out: -5.0
+    max_p_effort: 0.5
+    min_p_effort: -5.0
+    max_i_effort: 0.3
+    min_i_effort: -0.3
+    max_d_effort: 0.3
+    min_d_effort: -0.3
+    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.8
+    smooth_stop_min_strong_acc: -1.3
+    smooth_stop_weak_acc: -0.6
+    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: 3.5
+    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

autoware_launch/config/control/trajectory_follower/10/lateral/mpc.param.yaml

@@ -0,0 +1,76 @@
+/**:
+  ros__parameters:
+    # -- system --
+    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: true    # flag for path smoothing
+    path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing
+    curvature_smoothing_num_traj: 15        # 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)
+
+    # -- trajectory extending --
+    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
+
+    # -- 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.2                    # 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.03   # 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: 1.00  # 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.05                   # 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.03   # 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: 2.00  # 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.03                   # 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
+    mpc_min_prediction_length: 5.0               # minimum prediction length
+
+    # -- vehicle model --
+    vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics
+    input_delay: 0.1                 # steering input delay time for delay compensation
+    vehicle_model_steer_tau: 0.1     # steering dynamics time constant (1d approximation) [s]
+    steer_rate_lim_dps_list_by_curvature: [20.0, 20.0, 20.0]            # steering angle rate limit list depending on curvature [deg/s]
+    curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01]              # curvature list for steering angle rate limit interpolation in ascending order [/m]
+    steer_rate_lim_dps_list_by_velocity: [20.0, 20.0, 20.0]             # steering angle rate limit list depending on velocity [deg/s]
+    velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0]                  # velocity list for steering angle rate limit interpolation in ascending order [m/s]
+    acceleration_limit: 1.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: 10.0     # cutoff frequency of lowpass filter for steering command [Hz]
+    error_deriv_lpf_cutoff_hz: 5.0
+
+    # stop state: steering command is kept in the previous value in the stop state.
+    stop_state_entry_ego_speed: 0.2
+    stop_state_entry_target_speed: 0.2
+    converged_steer_rad: 0.1
+    keep_steer_control_until_converged: true
+    new_traj_duration_time: 1.0
+    new_traj_end_dist: 0.3
+    mpc_converged_threshold_rps:  0.01 # threshold of mpc convergence check [rad/s]
+
+    # steer offset
+    steering_offset:
+      enable_auto_steering_offset_removal: true
+      update_vel_threshold: 5.56
+      update_steer_threshold: 0.035
+      average_num: 1000
+      steering_offset_limit: 0.02

autoware_launch/config/control/trajectory_follower/10/longitudinal/pid.param.yaml

@@ -0,0 +1,74 @@
+/**:
+  ros__parameters:
+    delay_compensation_time: 0.40
+
+    enable_smooth_stop: true
+    enable_overshoot_emergency: false
+    enable_large_tracking_error_emergency: false
+    enable_slope_compensation: true
+    enable_keep_stopped_until_steer_convergence: 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_duration_time: 0.1
+    stopped_state_entry_vel: 0.01
+    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: 2.0
+    ki: 0.02
+    kd: 0.0
+    max_out: 1.0
+    min_out: -5.0
+    max_p_effort: 0.5
+    min_p_effort: -5.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.8
+    smooth_stop_min_strong_acc: -1.3
+    smooth_stop_weak_acc: -0.6
+    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: 3.5
+    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