From c5afec100128e0456aa1b8589187b97b56e0ebe4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 21 Dec 2022 17:08:38 +0900 Subject: [PATCH] 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 * fix Signed-off-by: Takayuki Murooka * update operation_mode_transition_manager.param.yaml Signed-off-by: kminoda Signed-off-by: Takayuki Murooka Signed-off-by: kminoda Co-authored-by: kminoda --- autoware_launch/CMakeLists.txt | 1 + .../control/common/nearest_search.param.yaml | 5 ++ .../tier4_control_component.launch.xml | 33 +++++++ .../lane_departure_checker.param.yaml | 15 ++++ .../obstacle_collision_checker.param.yaml | 11 +++ ...eration_mode_transition_manager.param.yaml | 22 +++++ .../shift_decider/shift_decider.param.yaml | 3 + .../lateral_controller.param.yaml | 77 +++++++++++++++++ .../longitudinal_controller.param.yaml | 86 +++++++++++++++++++ .../vehicle_cmd_gate.param.yaml | 20 +++++ autoware_launch/launch/autoware.launch.xml | 5 +- .../tier4_control_component.launch.xml | 33 +++++++ 12 files changed, 309 insertions(+), 2 deletions(-) create mode 100644 autoware_launch/config/control/common/nearest_search.param.yaml create mode 100644 autoware_launch/config/control/components/tier4_control_component.launch.xml create mode 100644 autoware_launch/config/control/lane_departure_checker/lane_departure_checker.param.yaml create mode 100644 autoware_launch/config/control/obstacle_collision_checker/obstacle_collision_checker.param.yaml create mode 100644 autoware_launch/config/control/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml create mode 100644 autoware_launch/config/control/shift_decider/shift_decider.param.yaml create mode 100644 autoware_launch/config/control/trajectory_follower/lateral_controller.param.yaml create mode 100644 autoware_launch/config/control/trajectory_follower/longitudinal_controller.param.yaml create mode 100644 autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml create mode 100644 autoware_launch/launch/components/tier4_control_component.launch.xml diff --git a/autoware_launch/CMakeLists.txt b/autoware_launch/CMakeLists.txt index 91b32628cd..fd586d287e 100644 --- a/autoware_launch/CMakeLists.txt +++ b/autoware_launch/CMakeLists.txt @@ -12,6 +12,7 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config rviz config ) diff --git a/autoware_launch/config/control/common/nearest_search.param.yaml b/autoware_launch/config/control/common/nearest_search.param.yaml new file mode 100644 index 0000000000..eb6709764c --- /dev/null +++ b/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] diff --git a/autoware_launch/config/control/components/tier4_control_component.launch.xml b/autoware_launch/config/control/components/tier4_control_component.launch.xml new file mode 100644 index 0000000000..e3706f20e1 --- /dev/null +++ b/autoware_launch/config/control/components/tier4_control_component.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware_launch/config/control/lane_departure_checker/lane_departure_checker.param.yaml b/autoware_launch/config/control/lane_departure_checker/lane_departure_checker.param.yaml new file mode 100644 index 0000000000..092a6765aa --- /dev/null +++ b/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 diff --git a/autoware_launch/config/control/obstacle_collision_checker/obstacle_collision_checker.param.yaml b/autoware_launch/config/control/obstacle_collision_checker/obstacle_collision_checker.param.yaml new file mode 100644 index 0000000000..e3c78c90e2 --- /dev/null +++ b/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] diff --git a/autoware_launch/config/control/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/autoware_launch/config/control/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml new file mode 100644 index 0000000000..a86443f5ca --- /dev/null +++ b/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 diff --git a/autoware_launch/config/control/shift_decider/shift_decider.param.yaml b/autoware_launch/config/control/shift_decider/shift_decider.param.yaml new file mode 100644 index 0000000000..4c45b36223 --- /dev/null +++ b/autoware_launch/config/control/shift_decider/shift_decider.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + park_on_goal: true diff --git a/autoware_launch/config/control/trajectory_follower/lateral_controller.param.yaml b/autoware_launch/config/control/trajectory_follower/lateral_controller.param.yaml new file mode 100644 index 0000000000..f3819f155e --- /dev/null +++ b/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 diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal_controller.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal_controller.param.yaml new file mode 100644 index 0000000000..b6e1c3a38c --- /dev/null +++ b/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 diff --git a/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml new file mode 100644 index 0000000000..4876699351 --- /dev/null +++ b/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 diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 449d3238e0..9170923a39 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -121,12 +121,13 @@ - - + + + diff --git a/autoware_launch/launch/components/tier4_control_component.launch.xml b/autoware_launch/launch/components/tier4_control_component.launch.xml new file mode 100644 index 0000000000..e3706f20e1 --- /dev/null +++ b/autoware_launch/launch/components/tier4_control_component.launch.xml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +