feat(autoware_launch): move config from autoware.universe for control (…

…#134)

* feat(autoware_launch): move config from autoware.universe for control

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update operation_mode_transition_manager.param.yaml

Signed-off-by: kminoda <koji.minoda@tier4.jp>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
Signed-off-by: kminoda <koji.minoda@tier4.jp>
Co-authored-by: kminoda <koji.minoda@tier4.jp>

autoware_launch/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 ament_auto_package(
   INSTALL_TO_SHARE
   launch
+  config
   rviz
   config
 )

autoware_launch/config/control/common/nearest_search.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    # ego
+    ego_nearest_dist_threshold: 3.0 # [m]
+    ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg]

autoware_launch/config/control/components/tier4_control_component.launch.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="vehicle_param_file"/>
+  <arg name="vehicle_id"/>
+  <arg name="enable_obstacle_collision_checker"/>
+  <arg name="check_external_emergency_heartbeat"/>
+  <arg name="lateral_controller_mode"/>
+  <arg name="longitudinal_controller_mode"/>
+
+  <include file="$(find-pkg-share tier4_control_launch)/launch/control.launch.py">
+    <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
+
+    <!-- option -->
+    <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
+    <arg name="vehicle_id" value="$(var vehicle_id)"/>
+    <arg name="enable_obstacle_collision_checker" value="$(var enable_obstacle_collision_checker)"/>
+    <arg name="check_external_emergency_heartbeat" value="$(var check_external_emergency_heartbeat)"/>
+    <arg name="lateral_controller_mode" value="$(var lateral_controller_mode)"/>
+    <arg name="longitudinal_controller_mode" value="$(var longitudinal_controller_mode)"/>
+
+    <!-- common param path -->
+    <arg name="nearest_search_param_path" value="$(find-pkg-share autoware_launch)/config/control/common/nearest_search.param.yaml"/>
+
+    <!-- package param path -->
+    <arg name="lat_controller_param_path" value="$(find-pkg-share autoware_launch)/config/control/trajectory_follower/lateral_controller.param.yaml"/>
+    <arg name="lon_controller_param_path" value="$(find-pkg-share autoware_launch)/config/control/trajectory_follower/longitudinal_controller.param.yaml"/>
+    <arg name="vehicle_cmd_gate_param_path" value="$(find-pkg-share autoware_launch)/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml"/>
+    <arg name="lane_departure_checker_param_path" value="$(find-pkg-share autoware_launch)/config/control/lane_departure_checker/lane_departure_checker.param.yaml"/>
+    <arg name="operation_mode_transition_manager_param_path" value="$(find-pkg-share autoware_launch)/config/control/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml"/>
+    <arg name="shift_decider_param_path" value="$(find-pkg-share autoware_launch)/config/control/shift_decider/shift_decider.param.yaml"/>
+    <arg name="obstacle_collision_checker_param_path" value="$(find-pkg-share autoware_launch)/config/control/obstacle_collision_checker/obstacle_collision_checker.param.yaml"/>
+  </include>
+</launch>

autoware_launch/config/control/lane_departure_checker/lane_departure_checker.param.yaml

@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    # Node
+    update_rate: 10.0
+    visualize_lanelet: false
+
+    # Core
+    footprint_margin_scale: 1.0
+    resample_interval: 0.3
+    max_deceleration: 2.8
+    delay_time: 1.3
+    max_lateral_deviation: 2.0
+    max_longitudinal_deviation: 2.0
+    max_yaw_deviation_deg: 60.0
+    delta_yaw_threshold_for_closest_point: 1.570 #M_PI/2.0, delta yaw thres for closest point

autoware_launch/config/control/obstacle_collision_checker/obstacle_collision_checker.param.yaml

@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    # Node
+    update_rate: 10.0
+
+    # Core
+    delay_time: 0.03 # delay time of vehicle [s]
+    footprint_margin: 0.0 # margin for footprint [m]
+    max_deceleration: 1.5 # max deceleration [m/ss]
+    resample_interval: 0.3 # interval distance to resample point cloud [m]
+    search_radius: 5.0 # search distance from trajectory to point cloud [m]

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

@@ -0,0 +1,22 @@
+/**:
+  ros__parameters:
+    transition_timeout: 10.0
+    frequency_hz: 10.0
+    check_engage_condition: false # 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:
+      allow_autonomous_in_stopped: true  # no check if the velocity is zero, always allowed.
+      dist_threshold: 1.5
+      yaw_threshold: 0.524
+      speed_upper_threshold: 10.0
+      speed_lower_threshold: -10.0
+      acc_threshold: 1.5
+      lateral_acc_threshold: 1.0
+      lateral_acc_diff_threshold: 0.5
+    stable_check:
+      duration: 0.1
+      dist_threshold: 1.5
+      speed_upper_threshold: 2.0
+      speed_lower_threshold: -2.0
+      yaw_threshold: 0.262

autoware_launch/config/control/shift_decider/shift_decider.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    park_on_goal: true

autoware_launch/config/control/trajectory_follower/lateral_controller.param.yaml

@@ -0,0 +1,77 @@
+/**:
+  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: false   # 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.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
+    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.24                # steering input delay time for delay compensation
+    vehicle_model_steer_tau: 0.3     # steering dynamics time constant (1d approximation) [s]
+    steer_rate_lim_dps: 40.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: steering command is kept in the previous value in the stop state.
+    stop_state_entry_ego_speed: 0.001
+    stop_state_entry_target_speed: 0.001
+    converged_steer_rad: 0.1
+    keep_steer_control_until_converged: true
+    new_traj_duration_time: 1.0
+    new_traj_end_dist: 0.3
+
+    # 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

autoware_launch/config/control/trajectory_follower/longitudinal_controller.param.yaml

@@ -0,0 +1,86 @@
+/**:
+  ros__parameters:
+    delay_compensation_time: 0.17
+
+    enable_smooth_stop: true
+    enable_overshoot_emergency: true
+    enable_large_tracking_error_emergency: true
+    enable_slope_compensation: false
+    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.49
+    stopped_state_entry_duration_time: 0.1
+    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

autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml

@@ -0,0 +1,20 @@
+/**:
+  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
+    stopped_state_entry_duration_time: 0.1
+    nominal:
+      vel_lim: 25.0
+      lon_acc_lim: 5.0
+      lon_jerk_lim: 5.0
+      lat_acc_lim: 5.0
+      lat_jerk_lim: 5.0
+    on_transition:
+      vel_lim: 50.0
+      lon_acc_lim: 1.0
+      lon_jerk_lim: 0.5
+      lat_acc_lim: 1.2
+      lat_jerk_lim: 0.75

autoware_launch/launch/autoware.launch.xml

@@ -121,12 +121,13 @@
 
   <!-- Control -->
   <group if="$(var launch_control)">
-    <include file="$(find-pkg-share tier4_control_launch)/launch/control.launch.py">
-      <arg name="lateral_controller_mode" value="mpc_follower"/>
+    <include file="$(find-pkg-share autoware_launch)/launch/components/tier4_control_component.launch.xml">
       <arg name="vehicle_param_file" value="$(find-pkg-share $(var vehicle_model)_description)/config/vehicle_info.param.yaml"/>
       <arg name="vehicle_id" value="$(var vehicle_id)"/>
       <arg name="enable_obstacle_collision_checker" value="$(var enable_obstacle_collision_checker)"/>
       <arg name="check_external_emergency_heartbeat" value="$(var check_external_emergency_heartbeat)"/>
+      <arg name="lateral_controller_mode" value="mpc_follower"/>
+      <arg name="longitudinal_controller_mode" value="pid"/>
     </include>
   </group>
 

autoware_launch/launch/components/tier4_control_component.launch.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="vehicle_param_file"/>
+  <arg name="vehicle_id"/>
+  <arg name="enable_obstacle_collision_checker"/>
+  <arg name="check_external_emergency_heartbeat"/>
+  <arg name="lateral_controller_mode"/>
+  <arg name="longitudinal_controller_mode"/>
+
+  <include file="$(find-pkg-share tier4_control_launch)/launch/control.launch.py">
+    <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
+
+    <!-- option -->
+    <arg name="vehicle_param_file" value="$(var vehicle_param_file)"/>
+    <arg name="vehicle_id" value="$(var vehicle_id)"/>
+    <arg name="enable_obstacle_collision_checker" value="$(var enable_obstacle_collision_checker)"/>
+    <arg name="check_external_emergency_heartbeat" value="$(var check_external_emergency_heartbeat)"/>
+    <arg name="lateral_controller_mode" value="$(var lateral_controller_mode)"/>
+    <arg name="longitudinal_controller_mode" value="$(var longitudinal_controller_mode)"/>
+
+    <!-- common param path -->
+    <arg name="nearest_search_param_path" value="$(find-pkg-share autoware_launch)/config/control/common/nearest_search.param.yaml"/>
+
+    <!-- package param path -->
+    <arg name="lat_controller_param_path" value="$(find-pkg-share autoware_launch)/config/control/trajectory_follower/lateral_controller.param.yaml"/>
+    <arg name="lon_controller_param_path" value="$(find-pkg-share autoware_launch)/config/control/trajectory_follower/longitudinal_controller.param.yaml"/>
+    <arg name="vehicle_cmd_gate_param_path" value="$(find-pkg-share autoware_launch)/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml"/>
+    <arg name="lane_departure_checker_param_path" value="$(find-pkg-share autoware_launch)/config/control/lane_departure_checker/lane_departure_checker.param.yaml"/>
+    <arg name="operation_mode_transition_manager_param_path" value="$(find-pkg-share autoware_launch)/config/control/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml"/>
+    <arg name="shift_decider_param_path" value="$(find-pkg-share autoware_launch)/config/control/shift_decider/shift_decider.param.yaml"/>
+    <arg name="obstacle_collision_checker_param_path" value="$(find-pkg-share autoware_launch)/config/control/obstacle_collision_checker/obstacle_collision_checker.param.yaml"/>
+  </include>
+</launch>