diff --git a/autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml b/autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml index 1a870aff7a..f4ed9d5f5c 100644 --- a/autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml +++ b/autoware_launch/config/control/autonomous_emergency_braking/autonomous_emergency_braking.param.yaml @@ -2,7 +2,7 @@ ros__parameters: publish_debug_pointcloud: false 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 @@ -16,5 +16,5 @@ imu_prediction_time_interval: 0.1 mpc_prediction_time_horizon: 1.5 mpc_prediction_time_interval: 0.1 - collision_keeping_sec: 0.0 + collision_keeping_sec: 2.0 aeb_hz: 10.0 diff --git a/autoware_launch/config/control/control_validator/control_validator.param.yaml b/autoware_launch/config/control/control_validator/control_validator.param.yaml index c51cbafba2..e4ac8942fc 100644 --- a/autoware_launch/config/control/control_validator/control_validator.param.yaml +++ b/autoware_launch/config/control/control_validator/control_validator.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - publish_diag: false # if true, diagnostic msg is published + publish_diag: true # if true, diagnostic msg is published # If the number of consecutive invalid predicted_path exceeds this threshold, the Diag will be set to ERROR. # (For example, threshold = 1 means, even if the predicted_path is invalid, Diag will not be ERROR if 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 index a4e8ab2c83..f7a7e0686a 100644 --- 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 @@ -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 diff --git a/autoware_launch/config/control/trajectory_follower/1/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/1/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/1/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/1/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..caf29ec637 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/1/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/10/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/10/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/10/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/10/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..f61d517028 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/10/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/2/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/2/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/2/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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/2/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/2/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..f61d517028 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/2/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/3/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/3/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/3/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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/3/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/3/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..7cc9856ea5 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/3/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + enable_brake_keeping_before_stop: false + brake_keeping_acc: -0.2 + + # smooth stop state + smooth_stop_max_strong_acc: -0.90 + smooth_stop_min_strong_acc: -1.40 + smooth_stop_weak_acc: -0.625 + smooth_stop_weak_stop_acc: -0.75 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/4/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/4/lateral/mpc.param.yaml new file mode 100644 index 0000000000..d85c9ae81a --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/4/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.50 # 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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/4/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/4/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..9fca8003d0 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/4/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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.2 + ki: 0.02 + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + enable_brake_keeping_before_stop: false + brake_keeping_acc: -0.2 + + # smooth stop state + smooth_stop_max_strong_acc: -0.73 + smooth_stop_min_strong_acc: -1.23 + smooth_stop_weak_acc: -0.53 + smooth_stop_weak_stop_acc: -0.73 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/5/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/5/lateral/mpc.param.yaml new file mode 100644 index 0000000000..6fba1855b9 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/5/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.3 # 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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/5/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/5/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..f61d517028 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/5/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/6/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/6/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/6/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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/6/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/6/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..caf29ec637 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/6/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/7/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/7/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/7/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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/7/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/7/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..f61d517028 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/7/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/9/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/9/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/9/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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/9/longitudinal/pid.param.yaml similarity index 84% rename from autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml rename to autoware_launch/config/control/trajectory_follower/9/longitudinal/pid.param.yaml index c39088753f..66dc2605ae 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/9/longitudinal/pid.param.yaml @@ -1,10 +1,10 @@ /**: ros__parameters: - delay_compensation_time: 0.17 + delay_compensation_time: 0.40 enable_smooth_stop: true - enable_overshoot_emergency: true - enable_large_tracking_error_emergency: true + enable_overshoot_emergency: false + enable_large_tracking_error_emergency: false enable_slope_compensation: true enable_keep_stopped_until_steer_convergence: true @@ -20,7 +20,7 @@ emergency_state_traj_rot_dev: 0.7854 # drive state - kp: 1.0 + kp: 2.0 ki: 0.1 kd: 0.0 max_out: 1.0 @@ -39,10 +39,10 @@ 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_max_strong_acc: -0.85 + smooth_stop_min_strong_acc: -1.35 + smooth_stop_weak_acc: -0.6 + smooth_stop_weak_stop_acc: -0.7 smooth_stop_strong_stop_acc: -3.4 smooth_stop_max_fast_vel: 0.5 smooth_stop_min_running_vel: 0.01 @@ -66,7 +66,7 @@ min_acc: -5.0 # jerk limit - max_jerk: 2.0 + max_jerk: 3.5 min_jerk: -5.0 # pitch diff --git a/autoware_launch/config/control/trajectory_follower/awsim_jpt/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/awsim_jpt/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/awsim_jpt/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: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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 diff --git a/autoware_launch/config/control/trajectory_follower/awsim_jpt/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/awsim_jpt/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..f61d517028 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/awsim_jpt/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/default/lateral/mpc.param.yaml similarity index 80% rename from autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml rename to autoware_launch/config/control/trajectory_follower/default/lateral/mpc.param.yaml index 4222082d40..ed67097364 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/default/lateral/mpc.param.yaml @@ -7,7 +7,7 @@ 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 + 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) @@ -23,19 +23,19 @@ 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_steering_input_squared_vel: 1.00 # steering error * velocity weight in matrix R mpc_weight_lat_jerk: 0.1 # 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_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.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_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.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + 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 @@ -45,22 +45,22 @@ # -- 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_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] + 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: [60.0, 50.0, 40.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: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + steer_rate_lim_dps_list_by_velocity: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # 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: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] + 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.001 - stop_state_entry_target_speed: 0.001 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/default/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/default/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..f61d517028 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/default/longitudinal/pid.param.yaml @@ -0,0 +1,76 @@ +/**: + 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 + enable_integration_at_low_speed: false + current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 + 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 diff --git a/autoware_launch/config/control/trajectory_follower/lateral/pure_pursuit.param.yaml b/autoware_launch/config/control/trajectory_follower/lateral/pure_pursuit.param.yaml deleted file mode 100644 index 0b8b464e9f..0000000000 --- a/autoware_launch/config/control/trajectory_follower/lateral/pure_pursuit.param.yaml +++ /dev/null @@ -1,16 +0,0 @@ -/**: - ros__parameters: - ld_velocity_ratio: 2.4 - ld_lateral_error_ratio: 3.6 - ld_curvature_ratio: 120.0 - long_ld_lateral_error_threshold: 0.5 - min_lookahead_distance: 4.35 - max_lookahead_distance: 15.0 - converged_steer_rad: 0.1 - reverse_min_lookahead_distance: 7.0 - prediction_ds: 0.3 - prediction_distance_length: 21.0 - resampling_ds: 0.1 - curvature_calculation_distance: 4.0 - enable_path_smoothing: false - path_filter_moving_ave_num: 25 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 index 7ad685217f..f2c46307b4 100644 --- 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 @@ -16,14 +16,14 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - reference_speed_points: [20.0, 30.0] - steer_lim: [1.0, 0.8] - steer_rate_lim: [1.0, 0.8] - lon_acc_lim: [5.0, 4.0] - lon_jerk_lim: [5.0, 4.0] - lat_acc_lim: [5.0, 4.0] - lat_jerk_lim: [7.0, 6.0] - actual_steer_diff_lim: [1.0, 0.8] + reference_speed_points: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] + steer_lim: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.025, 0.025] + steer_rate_lim: [0.5 , 0.5 , 0.4 , 0.3 , 0.15 ,0.07 , 0.05, 0.035, 0.035] + lon_acc_lim: [100.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0] + lon_jerk_lim: [100.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0] + lat_acc_lim: [5.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0] + lat_jerk_lim: [0.869, 0.869, 0.869, 1.47, 1.74, 1.36, 1.30, 1.78, 2.32] + actual_steer_diff_lim: [1.0, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8] on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] diff --git a/autoware_launch/config/localization/crop_box_filter_measurement_range.param.yaml b/autoware_launch/config/localization/crop_box_filter_measurement_range.param.yaml index ad55423154..be5a50ef0f 100644 --- a/autoware_launch/config/localization/crop_box_filter_measurement_range.param.yaml +++ b/autoware_launch/config/localization/crop_box_filter_measurement_range.param.yaml @@ -2,10 +2,10 @@ ros__parameters: input_frame: "base_link" output_frame: "base_link" - min_x: -60.0 - max_x: 60.0 - min_y: -60.0 - max_y: 60.0 + min_x: -100.0 + max_x: 100.0 + min_y: -100.0 + max_y: 100.0 min_z: -30.0 max_z: 50.0 negative: False diff --git a/autoware_launch/config/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml index 1c16624605..2a3489ee97 100644 --- a/autoware_launch/config/localization/ekf_localizer.param.yaml +++ b/autoware_launch/config/localization/ekf_localizer.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: show_debug_info: false - enable_yaw_bias_estimation: true + enable_yaw_bias_estimation: false predict_frequency: 50.0 tf_rate: 50.0 extend_state_step: 50 diff --git a/autoware_launch/config/localization/localization_error_monitor.param.yaml b/autoware_launch/config/localization/localization_error_monitor.param.yaml index def5c80164..f56fd03893 100644 --- a/autoware_launch/config/localization/localization_error_monitor.param.yaml +++ b/autoware_launch/config/localization/localization_error_monitor.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: scale: 3.0 - error_ellipse_size: 1.5 - warn_ellipse_size: 1.2 - error_ellipse_size_lateral_direction: 0.3 - warn_ellipse_size_lateral_direction: 0.25 + error_ellipse_size: 1.0 + warn_ellipse_size: 0.8 + error_ellipse_size_lateral_direction: 0.35 + warn_ellipse_size_lateral_direction: 0.3 diff --git a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml index 9bc62d3f91..4ee8ac93d5 100644 --- a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml +++ b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml @@ -35,10 +35,10 @@ # If converged_param_type is 1 # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.3 + converged_param_nearest_voxel_transformation_likelihood: 2.2 # The number of particles to estimate initial pose - initial_estimate_particles_num: 200 + initial_estimate_particles_num: 400 # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. diff --git a/autoware_launch/config/localization/pose_initializer.param.yaml b/autoware_launch/config/localization/pose_initializer.param.yaml index a05cc7c35c..6f03dfb49d 100644 --- a/autoware_launch/config/localization/pose_initializer.param.yaml +++ b/autoware_launch/config/localization/pose_initializer.param.yaml @@ -6,8 +6,8 @@ # from gnss gnss_particle_covariance: [ - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, diff --git a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml index 26b027f007..42b2106d4a 100644 --- a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml @@ -4,7 +4,7 @@ voxel_leaf_size: 0.3 min_points_number_per_voxel: 1 min_cluster_size: 10 - max_cluster_size: 3000 + max_cluster_size: 3500 use_height: false input_frame: "base_link" diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml index 0b29a87965..38ea2ea0ac 100644 --- a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml @@ -1,15 +1,16 @@ /**: ros__parameters: class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + rename_car_to_truck_and_bus: true point_feature_size: 4 max_voxel_size: 40000 - point_cloud_range: [-76.8, -76.8, -4.0, 76.8, 76.8, 6.0] - voxel_size: [0.32, 0.32, 10.0] + point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0] + voxel_size: [0.32, 0.32, 8.0] downsample_factor: 1 encoder_in_feature_size: 9 # post-process params - circle_nms_dist_threshold: 0.5 - iou_nms_target_class_names: ["CAR"] + circle_nms_dist_threshold: 0.3 + iou_nms_target_class_names: ["CAR", "TRUCK", "BUS"] iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] diff --git a/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml index fe4d2a51ec..787d02881b 100644 --- a/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml +++ b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: enable_delay_compensation: true - prediction_time_horizon: 10.0 #[s] + prediction_time_horizon: 15.0 #[s] lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] diff --git a/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml index 13f2220655..be2fe52ce4 100644 --- a/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml @@ -35,7 +35,7 @@ 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 2.00, 2.00, 1.50] #PEDESTRIAN min_area_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + [ 0.001, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN 3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR 6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS diff --git a/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml index 4e335e3574..4f730a9460 100644 --- a/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml +++ b/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml @@ -5,4 +5,4 @@ occupied_to_free: 0.05 free_to_occupied: 0.2 free_to_free: 0.8 - v_ratio: 10.0 + v_ratio: 0.5 diff --git a/autoware_launch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml index 8077bdec50..e8344bee3a 100644 --- a/autoware_launch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml +++ b/autoware_launch/config/perception/occupancy_grid_map/pointcloud_based_occupancy_grid_map.param.yaml @@ -19,9 +19,9 @@ gridmap_origin_frame: "base_link" # ray-tracing center: main sensor frame is preferable like: "velodyne_top" # base_link should not be used with "OccupancyGridMapProjectiveBlindSpot" - scan_origin_frame: "base_link" + scan_origin_frame: "velodyne_top" - grid_map_type: "OccupancyGridMapFixedBlindSpot" + grid_map_type: "OccupancyGridMapProjectiveBlindSpot" OccupancyGridMapFixedBlindSpot: distance_margin: 1.0 OccupancyGridMapProjectiveBlindSpot: diff --git a/autoware_launch/config/planning/preset/xx1_preset.yaml b/autoware_launch/config/planning/preset/xx1_preset.yaml new file mode 100644 index 0000000000..d6e4922911 --- /dev/null +++ b/autoware_launch/config/planning/preset/xx1_preset.yaml @@ -0,0 +1,120 @@ +launch: + # behavior path modules + - arg: + name: launch_avoidance_module + default: "true" + - arg: + name: launch_avoidance_by_lane_change_module + default: "true" + - arg: + name: launch_dynamic_avoidance_module + default: "true" + - arg: + name: launch_lane_change_right_module + default: "true" + - arg: + name: launch_lane_change_left_module + default: "true" + - arg: + name: launch_external_request_lane_change_right_module + default: "false" + - arg: + name: launch_external_request_lane_change_left_module + default: "false" + - arg: + name: launch_goal_planner_module + default: "true" + - arg: + name: launch_start_planner_module + default: "true" + - arg: + name: launch_side_shift_module + default: "true" + - arg: + name: use_experimental_lane_change_function + default: "true" + + # behavior velocity modules + - arg: + name: launch_crosswalk_module + default: "true" + - arg: + name: launch_walkway_module + default: "true" + - arg: + name: launch_traffic_light_module + default: "true" + - arg: + name: launch_intersection_module + default: "true" + - arg: + name: launch_merge_from_private_module + default: "true" + - arg: + name: launch_blind_spot_module + default: "true" + - arg: + name: launch_detection_area_module + default: "true" + - arg: + name: launch_virtual_traffic_light_module + default: "false" + - arg: + name: launch_no_stopping_area_module + default: "true" + - arg: + name: launch_stop_line_module + default: "true" + - arg: + name: launch_occlusion_spot_module + default: "false" + - arg: + name: launch_run_out_module + default: "true" + - arg: + name: launch_speed_bump_module + default: "false" + - arg: + name: launch_out_of_lane_module + default: "true" + - arg: + name: launch_no_drivable_lane_module + default: "false" + + # motion planning modules + - arg: + name: motion_path_smoother_type + default: elastic_band + # option: elastic_band + # none + + - arg: + name: motion_path_planner_type + default: obstacle_avoidance_planner + # option: obstacle_avoidance_planner + # path_sampler + # none + + - arg: + name: motion_stop_planner_type + default: obstacle_cruise_planner + # option: obstacle_stop_planner + # obstacle_cruise_planner + # none + + - arg: + name: motion_velocity_smoother_type + default: JerkFiltered + # option: JerkFiltered + # L2 + # Linf(Unstable) + # Analytical + + - arg: + name: launch_surround_obstacle_checker + default: "true" + + # parking modules + - arg: + name: launch_parking_module + default: "true" diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index 03958fda9a..715968f393 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -2,9 +2,9 @@ ros__parameters: # constraints param for normal driving normal: - min_acc: -1.0 # min deceleration [m/ss] + min_acc: -0.5 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] + min_jerk: -0.5 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] slow_down: diff --git a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml index 329714e3d3..f847e81cfa 100644 --- a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml @@ -11,9 +11,9 @@ forward: max_acc: 1.0 - min_acc: -1.0 - max_jerk: 0.3 - min_jerk: -0.3 + min_acc: -0.5 + max_jerk: 1.0 + min_jerk: -0.5 kp: 0.3 backward: diff --git a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml index f74ca045f5..a6906b7117 100644 --- a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: - jerk_weight: 10.0 # weight for "smoothness" cost for jerk - over_v_weight: 100000.0 # weight for "over speed limit" cost - over_a_weight: 5000.0 # weight for "over accel limit" cost - over_j_weight: 2000.0 # weight for "over jerk limit" cost + jerk_weight: 0.1 # weight for "smoothness" cost for jerk + over_v_weight: 10000.0 # weight for "over speed limit" cost + over_a_weight: 500.0 # weight for "over accel limit" cost + over_j_weight: 200.0 # weight for "over jerk limit" cost jerk_filter_ds: 0.1 # resampling ds for jerk filter diff --git a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml index 235c76a5c1..195504e36b 100644 --- a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml @@ -9,24 +9,24 @@ # -- curve parameters -- # common parameters - curvature_calculation_distance: 5.0 # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m] + curvature_calculation_distance: 2.0 # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m] # lateral acceleration limit parameters enable_lateral_acc_limit: true # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime. max_lateral_accel: 1.0 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.74 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] + min_curve_velocity: 2.0 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit - min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] + min_decel_for_lateral_acc_lim_filter: -0.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] # steering angle rate limit parameters enable_steering_rate_limit: true # To toggle the steer rate filter on and off. You can switch it dynamically at runtime. - max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] + max_steering_angle_rate: 11.5 # maximum steering angle rate [degree/s] resample_ds: 0.1 # distance between trajectory points [m] curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] # engage & replan parameters replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s] engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) - engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) + engage_acceleration: 0.6 # engage acceleration [m/ss] (use this acceleration when engagement) engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 0618185dcf..48568b7992 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -170,7 +170,7 @@ check_current_lane: false # [-] check_shift_side_lane: true # [-] check_other_side_lane: false # [-] - check_unavoidable_object: false # [-] + check_unavoidable_object: true # [-] check_other_object: true # [-] # collision check parameters check_all_predicted_path: false # [-] @@ -198,9 +198,9 @@ # avoidance lateral parameters lateral: lateral_execution_threshold: 0.09 # [m] - lateral_small_shift_threshold: 0.101 # [m] + lateral_small_shift_threshold: 0.501 # [m] lateral_avoid_check_threshold: 0.1 # [m] - soft_road_shoulder_margin: 0.3 # [m] + soft_road_shoulder_margin: 0.8 # [m] hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 @@ -254,10 +254,10 @@ constraints: # lateral constraints lateral: - velocity: [1.0, 1.38, 11.1] # [m/s] + velocity: [2.78, 4.17, 11.1] # [m/s] max_accel_values: [0.5, 0.5, 0.5] # [m/ss] min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] - max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] + max_jerk_values: [1.5, 1.0, 1.0] # [m/sss] # longitudinal constraints longitudinal: diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml index 74c6112c0e..a5c1cbf23a 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml @@ -39,6 +39,7 @@ avoid_margin_lateral: 0.0 safety_buffer_lateral: 0.0 unknown: + is_target: false execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager_foa.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager_foa.param.yaml new file mode 100644 index 0000000000..a6171fc64d --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager_foa.param.yaml @@ -0,0 +1,84 @@ +# USE ONLY WHEN THE OPTION COMPILE_WITH_OLD_ARCHITECTURE IS SET TO FALSE. +# https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/CMakeLists.txt +# NOTE: The smaller the priority number is, the higher the module priority is. +/**: + ros__parameters: + external_request_lane_change_left: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: false + enable_simultaneous_execution_as_candidate_module: true + keep_last: false + priority: 6 + max_module_size: 1 + + external_request_lane_change_right: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: false + enable_simultaneous_execution_as_candidate_module: true + keep_last: false + priority: 6 + max_module_size: 1 + + lane_change_left: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: true + enable_simultaneous_execution_as_candidate_module: true + keep_last: false + priority: 5 + max_module_size: 1 + + lane_change_right: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: true + enable_simultaneous_execution_as_candidate_module: true + keep_last: false + priority: 5 + max_module_size: 1 + + start_planner: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: true + enable_simultaneous_execution_as_candidate_module: false + keep_last: false + priority: 0 + max_module_size: 1 + + side_shift: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: false + enable_simultaneous_execution_as_candidate_module: false + keep_last: false + priority: 2 + max_module_size: 1 + + goal_planner: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: false + enable_simultaneous_execution_as_candidate_module: false + keep_last: true + priority: 1 + max_module_size: 1 + + avoidance: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: true + enable_simultaneous_execution_as_candidate_module: false + keep_last: false + priority: 4 + max_module_size: 1 + + avoidance_by_lc: + enable_rtc: true + enable_simultaneous_execution_as_approved_module: false + enable_simultaneous_execution_as_candidate_module: false + keep_last: false + priority: 3 + max_module_size: 1 + + dynamic_avoidance: + enable_rtc: false + enable_simultaneous_execution_as_approved_module: true + enable_simultaneous_execution_as_candidate_module: true + keep_last: true + priority: 7 + max_module_size: 1 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml index b31506918a..523cbea08f 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml @@ -4,7 +4,7 @@ backward_path_length: 5.0 behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 - max_accel: -2.8 + max_accel: -3.0 max_jerk: -5.0 system_delay: 0.5 delay_response_time: 0.5 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 240963309e..ff488bb4a7 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml @@ -50,15 +50,15 @@ min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] - stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) + stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.25 m/s = 0.9 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding disable_stop_for_yield_cancel: true # for the crosswalks with traffic signal disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order - timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] - timeout_ego_stop_for_yield: 3.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. + timeout_map_for_no_intention_to_walk: [1.0, 0.0] # [s] + timeout_ego_stop_for_yield: 1.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. # param for target object filtering object_filtering: diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 4e9eb50f2a..19ce575799 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -36,7 +36,7 @@ consider_wrong_direction_vehicle: false collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 - keep_detection_velocity_threshold: 0.833 + keep_detection_velocity_threshold: 1.5 velocity_profile: use_upstream: true minimum_upstream_velocity: 0.01 @@ -61,13 +61,12 @@ object_margin_to_path: 2.0 occlusion: - enable: false + enable: true occlusion_attention_area_length: 70.0 free_space_max: 43 occupied_min: 58 denoise_kernel: 1.0 attention_lane_crop_curvature_threshold: 0.25 - attention_lane_curvature_calculation_ds: 0.5 creep_during_peeking: enable: false creep_velocity: 0.8333 @@ -79,6 +78,7 @@ temporal_stop_time_before_peeking: 0.1 temporal_stop_before_attention_area: false creep_velocity_without_traffic_light: 1.388 + attention_lane_curvature_calculation_ds: 0.6 static_occlusion_with_traffic_light_timeout: 0.5 debug: diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml index 5fa183d73a..af876b4f1b 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: stop_line: - stop_margin: 0.0 + stop_margin: 1.4 stop_duration_sec: 1.0 use_initialization_stop_line_state: true hold_stop_margin_distance: 2.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml index 23746a61b6..0fb3871a9c 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: traffic_light: - stop_margin: 0.0 + stop_margin: 2.0 tl_state_timeout: 1.0 stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml index f21e3d12db..07f493edcd 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: walkway: - stop_duration: 1.0 # [s] stop time at stop position + stop_duration: 0.1 # [s] stop time at stop position stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml index 9d761b15ee..1b50ff081f 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml @@ -5,7 +5,6 @@ - "crosswalk" - "detection_area" - "intersection" - - "no_stopping_area" - "traffic_light" - "lane_change_left" - "lane_change_right" @@ -22,7 +21,6 @@ - "crosswalk" - "detection_area" - "intersection" - - "no_stopping_area" - "traffic_light" - "lane_change_left" - "lane_change_right" diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer_foa.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer_foa.param.yaml new file mode 100644 index 0000000000..efaa9ed238 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer_foa.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + module_list: + - "blind_spot" + - "crosswalk" + - "detection_area" + - "intersection" + - "traffic_light" + - "lane_change_left" + - "lane_change_right" + - "avoidance_left" + - "avoidance_right" + - "avoidance_by_lane_change_left" + - "avoidance_by_lane_change_right" + - "goal_planner" + - "start_planner" + - "intersection_occlusion" + + default_enable_list: + - "blind_spot" diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 90e897fda3..12abf85bad 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -85,15 +85,15 @@ obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop stop: - max_lat_margin: 0.0 # lateral margin between obstacle and trajectory band with ego's width + max_lat_margin: 0.2 # lateral margin between obstacle and trajectory band with ego's width crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] cruise: max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width outside_obstacle: - obstacle_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] + obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 2.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego slow_down: @@ -187,12 +187,11 @@ static: min_lat_margin: 0.2 max_lat_margin: 1.0 - min_ego_velocity: 2.0 + min_ego_velocity: 4.0 max_ego_velocity: 8.0 moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold - time_margin_on_target_velocity: 1.5 # [s] lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml index 73e7a578fe..e3899e3820 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml @@ -7,34 +7,34 @@ consider_obj_velocity: true # consider forward vehicle velocity to ACC or not # general parameter for ACC - obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] - obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] - emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] + obstacle_velocity_thresh_to_start_acc: 3.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] + obstacle_velocity_thresh_to_stop_acc: 3.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] + emergency_stop_acceleration: -4.5 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] obstacle_emergency_stop_acceleration: -5.0 emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] - min_dist_stop: 4.0 # minimum distance of emergency stop [m] - max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] - min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control + min_dist_stop: 6.0 # minimum distance of emergency stop [m] + max_standard_acceleration: 1.0 # supposed maximum acceleration in active cruise control [m/ss] + min_standard_acceleration: -0.7 # supposed minimum acceleration (deceleration) in active cruise control standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] - min_dist_standard: 4.0 # minimum distance in active cruise control [m] - obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] - margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] + min_dist_standard: 6.0 # minimum distance in active cruise control [m] + obstacle_min_standard_acceleration: -2.0 # supposed minimum acceleration of forward obstacle [m/ss] + margin_rate_to_change_vel: 0.7 # margin to insert upper velocity [-] use_time_compensation_to_calc_distance: true # pid parameter for ACC - p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] + p_coefficient_positive: 0.25 # coefficient P in PID control (used when target dist -current_dist >=0) [-] p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-] - d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-] - d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-] + d_coefficient_positive: 0.15 # coefficient D in PID control (used when delta_dist >=0) [-] + d_coefficient_negative: 0.0 # coefficient D in PID control (used when delta_dist <0) [-] # parameter for object velocity estimation - object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] - object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m] + object_polygon_length_margin: 3.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] + object_polygon_width_margin: 0.8 # The distance to extend the polygon width the object in pointcloud-object matching [m] valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s] valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s] valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] - thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] - lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity + thresh_vel_to_stop: 0.0 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] + lowpass_gain_of_upper_velocity: 0.90 # Lowpass-gain of upper velocity use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####) rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml index c6a0c275db..ab87645d7f 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml @@ -28,6 +28,7 @@ dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module static_map_tags: # linestring tags in the lanelet maps that will be used as static obstacles - guard_rail + - curbstone filter_envelope : false # whether to calculate the apparent safety envelope and use it to filter obstacles rtree_min_points: 500 # from this number of obstacle points, a rtree is used for collision detection rtree_min_segments: 1600 # from this number of obstacle segments, a rtree is used for collision detection diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml index 5ec10572ff..7f72420b3a 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml @@ -9,7 +9,7 @@ surround_check_side_distance: 0.5 surround_check_back_distance: 0.5 unknown: - enable_check: true + enable_check: false surround_check_front_distance: 0.5 surround_check_side_distance: 0.5 surround_check_back_distance: 0.5 @@ -17,22 +17,22 @@ enable_check: true surround_check_front_distance: 0.5 surround_check_side_distance: 0.0 - surround_check_back_distance: 0.5 + surround_check_back_distance: 0.0 truck: enable_check: true surround_check_front_distance: 0.5 surround_check_side_distance: 0.0 - surround_check_back_distance: 0.5 + surround_check_back_distance: 0.0 bus: enable_check: true surround_check_front_distance: 0.5 surround_check_side_distance: 0.0 - surround_check_back_distance: 0.5 + surround_check_back_distance: 0.0 trailer: enable_check: true surround_check_front_distance: 0.5 surround_check_side_distance: 0.0 - surround_check_back_distance: 0.5 + surround_check_back_distance: 0.0 motorcycle: enable_check: true surround_check_front_distance: 0.5 @@ -49,9 +49,9 @@ surround_check_side_distance: 0.5 surround_check_back_distance: 0.5 - surround_check_hysteresis_distance: 0.3 + surround_check_hysteresis_distance: 0.1 - state_clear_time: 2.0 + state_clear_time: 0.2 # ego stop state stop_state_ego_speed: 0.1 #[m/s] diff --git a/autoware_launch/config/system/component_state_monitor/topics.yaml b/autoware_launch/config/system/component_state_monitor/topics.yaml index f8c6f9685f..2481721114 100644 --- a/autoware_launch/config/system/component_state_monitor/topics.yaml +++ b/autoware_launch/config/system/component_state_monitor/topics.yaml @@ -1,3 +1,107 @@ +- module: sensing + mode: [online] + type: launch + args: + node_name_suffix: imu + topic: /sensing/imu/imu_data + topic_type: sensor_msgs/msg/Imu + best_effort: false + transient_local: false + warn_rate: 10.0 + error_rate: 5.0 + timeout: 1.0 + +- module: sensing + mode: [online] + type: launch + args: + node_name_suffix: camera0 + topic: /sensing/camera/camera0/camera_info + topic_type: sensor_msgs/msg/CameraInfo + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: sensing + mode: [online] + type: launch + args: + node_name_suffix: camera1 + topic: /sensing/camera/camera1/camera_info + topic_type: sensor_msgs/msg/CameraInfo + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: sensing + mode: [online] + type: launch + args: + node_name_suffix: camera2 + topic: /sensing/camera/camera2/camera_info + topic_type: sensor_msgs/msg/CameraInfo + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: sensing + mode: [online] + type: launch + args: + node_name_suffix: camera3 + topic: /sensing/camera/camera3/camera_info + topic_type: sensor_msgs/msg/CameraInfo + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: sensing + mode: [online] + type: launch + args: + node_name_suffix: camera4 + topic: /sensing/camera/camera4/camera_info + topic_type: sensor_msgs/msg/CameraInfo + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: sensing + mode: [online] + type: launch + args: + node_name_suffix: camera5 + topic: /sensing/camera/camera5/camera_info + topic_type: sensor_msgs/msg/CameraInfo + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + +- module: sensing + mode: [online] + type: launch + args: + node_name_suffix: camera6 + topic: /sensing/camera/camera6/camera_info + topic_type: sensor_msgs/msg/CameraInfo + best_effort: true + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + - module: map mode: [online, logging_simulation, planning_simulation] type: launch @@ -64,7 +168,7 @@ timeout: 1.0 - module: perception - mode: [online, logging_simulation] + mode: [online, logging_simulation, planning_simulation] type: launch args: node_name_suffix: obstacle_segmentation_pointcloud @@ -72,7 +176,7 @@ topic_type: sensor_msgs/msg/PointCloud2 best_effort: true transient_local: false - warn_rate: 5.0 + warn_rate: 3.0 error_rate: 1.0 timeout: 1.0 @@ -85,7 +189,20 @@ topic_type: autoware_auto_perception_msgs/msg/PredictedObjects best_effort: false transient_local: false - warn_rate: 5.0 + warn_rate: 3.0 + error_rate: 1.0 + timeout: 1.0 + +- module: perception + mode: [online] + type: autonomous + args: + node_name_suffix: traffic_light_recognition_objects + topic: /perception/traffic_light_recognition/traffic_signals + topic_type: autoware_perception_msgs/msg/TrafficSignalArray + best_effort: false + transient_local: false + warn_rate: 3.0 error_rate: 1.0 timeout: 1.0 @@ -124,7 +241,7 @@ topic_type: autoware_auto_planning_msgs/msg/Trajectory best_effort: false transient_local: false - warn_rate: 5.0 + warn_rate: 3.0 error_rate: 1.0 timeout: 1.0 @@ -137,8 +254,8 @@ topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand best_effort: false transient_local: false - warn_rate: 5.0 - error_rate: 1.0 + warn_rate: 20.0 + error_rate: 15.0 timeout: 1.0 - module: control @@ -150,8 +267,8 @@ topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand best_effort: false transient_local: false - warn_rate: 5.0 - error_rate: 1.0 + warn_rate: 20.0 + error_rate: 15.0 timeout: 1.0 - module: vehicle diff --git a/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml b/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml index 43edd109b5..ff3b0881a0 100644 --- a/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml +++ b/autoware_launch/config/system/dummy_diag_publisher/dummy_diag_publisher.param.yaml @@ -14,4 +14,38 @@ /**: ros__parameters: required_diags: - dummy_diag_empty: default + #control + joy_controller_connection: default + + #localization + localization_accuracy: default + ndt_scan_matcher: default + + #system + bagpacker: default + NTP Offset: default + CPU Temperature: default + CPU Usage: default + CPU Thermal Throttling: default + CPU Frequency: default + CPU Load Average: default + GPU Temperature: default + GPU Usage: default + GPU Memory Usage: default + GPU Thermal Throttling: default + Memory Usage: default + Network Usage: default + Network Traffic: default + HDD Temperature: default + HDD Usage: default + HDD PowerOnHours: default + HDD TotalDataWritten: default + High-load: default + High-mem: default + Tasks Summary: default + + #vehicle + vehicle_errors: default + pacmod_errors: default + pacmod_accel_brake_fault: default + accel_brake_map_calibrator: default diff --git a/autoware_launch/config/system/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml b/autoware_launch/config/system/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml index e96e3b3b05..bcc9c388d5 100644 --- a/autoware_launch/config/system/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml +++ b/autoware_launch/config/system/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml @@ -9,3 +9,41 @@ path: vehicle_errors contains: [": vehicle_errors"] timeout: 1.0 + pacmod_errors: + type: diagnostic_aggregator/GenericAnalyzer + path: pacmod_checker + contains: [": pacmod_checker"] + timeout: 1.0 + pacmod_accel_brake_fault: + type: diagnostic_aggregator/GenericAnalyzer + path: pacmod_accel_brake_fault + contains: [": pacmod_accel_brake_fault"] + timeout: 1.0 + node_alive_monitoring: + type: diagnostic_aggregator/AnalyzerGroup + path: node_alive_monitoring + analyzers: + # TODO(Tier IV): Consider splitting sensor input and control command output + topic_status: + type: diagnostic_aggregator/GenericAnalyzer + path: topic_status + contains: [": vehicle_topic_status"] + timeout: 1.0 + calibration: + type: diagnostic_aggregator/AnalyzerGroup + path: calibration + analyzers: + accel_brake_map_calibrator: + type: diagnostic_aggregator/GenericAnalyzer + path: accel_brake_map_calibrator + contains: [": accel_brake_map_calibrator"] + timeout: 1.0 + awsim: + type: diagnostic_aggregator/AnalyzerGroup + path: awsim + analyzers: + vehicle_stuck_detection: + type: diagnostic_aggregator/GenericAnalyzer + path: vehicle_stuck_detection + contains: [": vehicle_stuck_detection"] + timeout: 1.0 diff --git a/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml b/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml index 2f98f9d0e2..64300946fa 100644 --- a/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml +++ b/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml @@ -18,11 +18,16 @@ required_modules: autonomous_driving: /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default + # /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default # tmp + /autoware/control/autonomous_driving/performance_monitoring/control_validator: default + /autoware/control/autonomous_driving/performance_monitoring/trajectory_deviation: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error"} /autoware/control/control_command_gate/node_alive_monitoring: default + # /autoware/control/external_control/local_external_control/device_connection: default /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/localization/performance_monitoring/localization_stability: default /autoware/localization/performance_monitoring/localization_accuracy: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/map/node_alive_monitoring: default @@ -32,21 +37,26 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - # /autoware/sensing/node_alive_monitoring: default + /autoware/sensing/node_alive_monitoring: default + /autoware/sensing/imu/bias_monitoring/gyro_bias_validator: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default + /autoware/system/debug_data_logger/storage_error: { sf_at: "warn", lf_at: "none", spf_at: "none" } + + /autoware/vehicle/calibration: { sf_at: "warn", lf_at: "none", spf_at: "none" } + + /autoware/vehicle/awsim: { sf_at: "warn", lf_at: "none", spf_at: "none" } + external_control: /autoware/control/control_command_gate/node_alive_monitoring: default /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default diff --git a/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml b/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml index 6a763eace4..45038044fa 100644 --- a/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml +++ b/autoware_launch/config/system/system_error_monitor/system_error_monitor.param.yaml @@ -19,10 +19,15 @@ autonomous_driving: /autoware/control/autonomous_driving/node_alive_monitoring: default /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default + /autoware/control/autonomous_driving/performance_monitoring/control_validator: default + /autoware/control/autonomous_driving/performance_monitoring/trajectory_deviation: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error"} /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/external_control/local_external_control/device_connection: default /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/localization/performance_monitoring/localization_stability: default /autoware/localization/performance_monitoring/localization_accuracy: default /autoware/localization/performance_monitoring/sensor_fusion_status: { sf_at: "error", lf_at: "none", spf_at: "none" } @@ -33,22 +38,25 @@ /autoware/planning/node_alive_monitoring: default /autoware/planning/performance_monitoring/trajectory_validation: default - # /autoware/sensing/node_alive_monitoring: default + /autoware/sensing/node_alive_monitoring: default + /autoware/sensing/imu/bias_monitoring/gyro_bias_validator: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } /autoware/system/duplicated_node_checker: default /autoware/vehicle/node_alive_monitoring: default + /autoware/system/debug_data_logger/storage_error: { sf_at: "warn", lf_at: "none", spf_at: "none" } + + /autoware/vehicle/calibration: { sf_at: "warn", lf_at: "none", spf_at: "none" } + external_control: /autoware/control/control_command_gate/node_alive_monitoring: default /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default diff --git a/autoware_launch/config/system/system_error_monitor/system_error_monitor.planning_simulation.param.yaml b/autoware_launch/config/system/system_error_monitor/system_error_monitor.planning_simulation.param.yaml index 3fd6264376..44337072b4 100644 --- a/autoware_launch/config/system/system_error_monitor/system_error_monitor.planning_simulation.param.yaml +++ b/autoware_launch/config/system/system_error_monitor/system_error_monitor.planning_simulation.param.yaml @@ -18,8 +18,10 @@ required_modules: autonomous_driving: /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default + # /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default + /autoware/control/autonomous_driving/performance_monitoring/control_validator: default /autoware/control/autonomous_driving/performance_monitoring/trajectory_deviation: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error"} /autoware/control/control_command_gate/node_alive_monitoring: default /autoware/localization/node_alive_monitoring: default diff --git a/autoware_launch/config/system/system_monitor/cpu_monitor.param.yaml b/autoware_launch/config/system/system_monitor/cpu_monitor.param.yaml index da4d74e53d..cae88d6a96 100644 --- a/autoware_launch/config/system/system_monitor/cpu_monitor.param.yaml +++ b/autoware_launch/config/system/system_monitor/cpu_monitor.param.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: usage_warn: 0.96 - usage_error: 0.96 - usage_warn_count: 1 + usage_error: 1.00 + usage_warn_count: 2 usage_error_count: 2 usage_avg: true msr_reader_port: 7634 diff --git a/autoware_launch/config/system/system_monitor/hdd_monitor.param.yaml b/autoware_launch/config/system/system_monitor/hdd_monitor.param.yaml index d818d848be..cdb48f73e9 100644 --- a/autoware_launch/config/system/system_monitor/hdd_monitor.param.yaml +++ b/autoware_launch/config/system/system_monitor/hdd_monitor.param.yaml @@ -6,12 +6,12 @@ disk0: name: / temp_attribute_id: 0xC2 - temp_warn: 55.0 - temp_error: 70.0 + temp_warn: 75.0 + temp_error: 85.0 power_on_hours_attribute_id: 0x09 power_on_hours_warn: 3000000 total_data_written_attribute_id: 0xF1 - total_data_written_warn: 4915200 # =150TB (1unit=32MB) + total_data_written_warn: 11417920 # =440TB (1unit=32MB) total_data_written_safety_factor: 0.05 recovered_error_attribute_id: 0xC3 recovered_error_warn: 1 diff --git a/autoware_launch/config/system/system_monitor/net_monitor.param.yaml b/autoware_launch/config/system/system_monitor/net_monitor.param.yaml index d72b8d1334..dac4ad1da8 100644 --- a/autoware_launch/config/system/system_monitor/net_monitor.param.yaml +++ b/autoware_launch/config/system/system_monitor/net_monitor.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: - devices: ["*"] - traffic_reader_port: 7636 - monitor_program: "greengrass" - crc_error_check_duration: 1 - crc_error_count_threshold: 1 - reassembles_failed_check_duration: 1 - reassembles_failed_check_count: 1 + devices: [ 'eno1', 'enp3s0', 'enp4s0', 'logging_bond0' ] + traffic_reader_port: 7636 + monitor_program: "greengrass" + crc_error_check_duration: 1 + crc_error_count_threshold: 1 + reassembles_failed_check_duration: 20 + reassembles_failed_check_count: 30000 diff --git a/autoware_launch/config/system/system_monitor/process_monitor.param.yaml b/autoware_launch/config/system/system_monitor/process_monitor.param.yaml index 3d6d82fae5..64303dd472 100644 --- a/autoware_launch/config/system/system_monitor/process_monitor.param.yaml +++ b/autoware_launch/config/system/system_monitor/process_monitor.param.yaml @@ -1,3 +1,3 @@ /**: ros__parameters: - num_of_procs: 5 + num_of_procs: 40 diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 80822d0146..3915b48c0a 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -9,7 +9,7 @@ - + @@ -42,8 +42,39 @@ - + + + + + + + + + + + + + + + + @@ -97,12 +128,20 @@ + + + + + + + + diff --git a/autoware_launch/launch/components/tier4_control_component.launch.xml b/autoware_launch/launch/components/tier4_control_component.launch.xml index d15e050d7a..e3d792ccb6 100644 --- a/autoware_launch/launch/components/tier4_control_component.launch.xml +++ b/autoware_launch/launch/components/tier4_control_component.launch.xml @@ -3,8 +3,8 @@ - - + + @@ -13,6 +13,7 @@ + diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index 88b326b2f7..a9f52a53b5 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 3a968f0f27..929d602e08 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -9,6 +9,8 @@ description="options: obstacle_pointcloud, occupancy_grid (occupancy_grid_map_method must be laserscan_based_occupancy_grid_map)" /> + + @@ -21,6 +23,15 @@ + + + + + + + + + - + + + + + + diff --git a/autoware_launch/launch/components/tier4_sensing_component.launch.xml b/autoware_launch/launch/components/tier4_sensing_component.launch.xml index fe520e1fdc..f70f11ecbc 100644 --- a/autoware_launch/launch/components/tier4_sensing_component.launch.xml +++ b/autoware_launch/launch/components/tier4_sensing_component.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/autoware_launch/launch/e2e_simulator.launch.xml b/autoware_launch/launch/e2e_simulator.launch.xml old mode 100644 new mode 100755 index d920584c6c..a7c6f55d1a --- a/autoware_launch/launch/e2e_simulator.launch.xml +++ b/autoware_launch/launch/e2e_simulator.launch.xml @@ -7,7 +7,7 @@ - + @@ -37,7 +37,36 @@ /> - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -70,8 +99,11 @@ + + - + + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index a2b66e91ed..ce0d432e33 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -8,7 +8,7 @@ - + @@ -35,6 +35,11 @@ + + + + + @@ -70,6 +75,11 @@ + + + + + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 28a0f0e347..f0d11d6f0c 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -7,7 +7,7 @@ - + @@ -30,6 +30,13 @@ + + + + + + + @@ -62,6 +69,10 @@ + + + + diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index a0ade56821..c65e935f4c 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -20,6 +20,8 @@ Panels: Name: AutowareDateTimePanel - Class: rviz_plugins::AutowareStatePanel Name: AutowareStatePanel + - Class: AutowareScreenCapturePanel + Name: AutowareScreenCapturePanel Visualization Manager: Class: "" Displays: @@ -904,7 +906,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light/debug/rois + Value: /perception/traffic_light_recognition/camera6/debug/rois Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -915,7 +917,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers + Value: /perception/traffic_light_recognition/camera6/detection/traffic_light_map_based_detector/debug/markers Value: true Enabled: true Name: TrafficLight @@ -3783,6 +3785,8 @@ Visualization Manager: X: 0 Y: 0 Window Geometry: + AutowareScreenCapturePanel: + collapsed: false AutowareStatePanel: collapsed: false Displays: diff --git a/autoware_launch/rviz/awsim.rviz b/autoware_launch/rviz/awsim.rviz new file mode 100755 index 0000000000..2669764e36 --- /dev/null +++ b/autoware_launch/rviz/awsim.rviz @@ -0,0 +1,3061 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Map1 + - /Map1/Lanelet2VectorMap1 + - /Map1/Lanelet2VectorMap1/Namespaces1 + - /Sensing1 + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Perception1 + Splitter Ratio: 0.557669460773468 + Tree Height: 147 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel + Name: InitialPoseButtonPanel + - Class: AutowareDateTimePanel + Name: AutowareDateTimePanel + - Class: rviz_plugins::AutowareStatePanel + Name: AutowareStatePanel + - Class: AutowareScreenCapturePanel + Name: AutowareScreenCapturePanel + - Class: multi_data_monitor::MultiDataMonitor + Name: MultiDataMonitor + Path: package://autoware_launch/rviz/config/awsim_data_monitor.yaml +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/SteeringAngle + Enabled: true + Left: 64 + Length: 128 + Name: SteeringAngle + Scale: 17 + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/steering_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Class: rviz_plugins/ConsoleMeter + Enabled: true + Left: 256 + Length: 128 + Name: ConsoleMeter + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Alpha: 0.9990000128746033 + Class: rviz_plugins/VelocityHistory + Color Border Vel Max: 3 + Constant Color: + Color: 255; 255; 255 + Value: true + Enabled: true + Name: VelocityHistory + Scale: 0.30000001192092896 + Timeout: 10 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera1/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera1/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera2/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera2/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera3/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera3/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera4/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera4/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera5/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera5/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera6/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera6/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera7/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera7/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gnss_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_front_left: + Alpha: 1 + Show Axes: false + Show Trail: false + livox_front_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_front_right: + Alpha: 1 + Show Axes: false + Show Trail: false + livox_front_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_kit_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + tamagawa/imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: VehicleModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Class: rviz_plugins/MaxVelocity + Enabled: true + Left: 298 + Length: 48 + Name: MaxVelocity + Text Color: 255; 255; 255 + Top: 140 + Topic: /planning/scenario_planning/current_max_velocity + Value: true + Value Scale: 0.25 + - Class: rviz_plugins/TurnSignal + Enabled: true + Height: 128 + Left: 98 + Name: TurnSignal + Top: 175 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/turn_indicators_status + Value: true + Width: 256 + Enabled: true + Name: Vehicle + Enabled: true + Name: System + - Class: rviz_common/Group + Displays: + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.71826171875 + Min Value: -7.4224700927734375 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 211; 215; 207 + Min Intensity: 0 + Name: PointCloudMap + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/pointcloud_map + Use Fixed Frame: true + Use rainbow: false + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + center_lane_line: true + center_line_arrows: true + crosswalk_lanelets: true + lane_start_bound: true + lanelet direction: true + lanelet_id: true + left_lane_bound: true + no_stopping_area: true + parking_lots: true + partitions: true + pedestrian_marking: true + right_lane_bound: true + road_lanelets: false + shoulder_center_lane_line: true + shoulder_center_line_arrows: true + shoulder_lane_start_bound: true + shoulder_lanelet direction: true + shoulder_left_lane_bound: true + shoulder_right_lane_bound: true + shoulder_road_lanelets: false + stop_lines: true + traffic_light: true + traffic_light_id: true + traffic_light_triangle: true + walkway_lanelets: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.9990000128746033 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: MeasurementRange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/crop_box_filter/crop_box_polygon + Value: false + Enabled: true + Name: LiDAR + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 233; 185; 110 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.20000000298023224 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.699999988079071 + Head Radius: 1.2000000476837158 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + Enabled: false + Name: GNSS + Enabled: true + Name: Sensing + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 170; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovInitial + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/initial_pose_with_covariance + Value: false + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovAligned + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose_with_covariance + Value: false + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: false + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Initial + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /localization/util/downsample/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Aligned + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MonteCarloInitialPose + Namespaces: + initial_pose_index_color_marker: true + initial_pose_iteration_color_marker: true + initial_pose_transform_probability_color_marker: true + result_pose_index_color_marker: true + result_pose_iteration_color_marker: true + result_pose_transform_probability_color_marker: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/monte_carlo_initial_pose_marker + Value: true + Enabled: true + Name: NDT + - Class: rviz_common/Group + Displays: + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + Enabled: true + Name: EKF + Enabled: true + Name: Localization + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -2 + Value: false + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 200; 200; 200 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: -5 + Name: NoGroundPointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/obstacle_segmentation/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Segmentation + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: DetectedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Detection + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: TrackedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/tracking/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Tracking + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: false + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: PredictedObjects + Namespaces: + acceleration: true + label: true + path: true + path confidence: true + shape: true + twist: true + uuid: true + velocity: true + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Maneuver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/prediction/maneuver + Value: false + Enabled: true + Name: Prediction + Enabled: true + Name: ObjectRecognition + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RecognitionResultOnImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/camera6/debug/rois + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MapBasedDetectionResult + Namespaces: + beam: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/camera6/detection/traffic_light_map_based_detector/debug/markers + Value: true + Enabled: true + Name: TrafficLight + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map_updates + Use Timestamp: false + Value: true + Enabled: false + Name: OccupancyGrid + Enabled: true + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: RouteArea + Namespaces: + end_lanelets: true + goal_lanelets: true + lane_start_bound: true + left_lane_bound: true + right_lane_bound: true + route_lanelets: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/route_marker + Value: true + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.5 + Name: GoalPose + Shaft Length: 3 + Shaft Radius: 0.20000000298023224 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/echo_back_goal_pose + Value: true + Enabled: true + Name: MissionPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: ScenarioTrajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/trajectory + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: Path + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/path + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_AvoidanceByLC + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/avoidance_by_lane_change + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 210; 110; 10 + Constant Color: true + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_Avoidance + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/avoidance + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 210; 110; 210 + Constant Color: true + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_LaneChangeRight + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/lane_change_right + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 210; 210; 110 + Constant Color: true + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_LaneChangeLeft + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/lane_change_left + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 210; 210; 110 + Constant Color: true + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_GoalPlanner + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/goal_planner + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 110; 110; 210 + Constant Color: true + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: false + Name: PathReference_PullOut + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_reference/pull_out + Value: false + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 210; 110; 110 + Constant Color: true + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_AvoidanceByLC + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/avoidance_by_lane_change + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: (old)PathChangeCandidate_LaneChange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/lane_change + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_LaneChangeRight + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/lane_change_right + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_LaneChangeLeft + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/lane_change_left + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_ExternalRequestLaneChangeRight + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/external_request_lane_change_right + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_ExternalRequestLaneChangeLeft + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/external_request_lane_change_left + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_Avoidance + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/avoidance + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_PullOut + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/pull_out + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_GoalPlanner + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/goal_planner + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Bound + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Avoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/avoidance + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (AvoidanceByLC) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/avoidance_by_lane_change + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (LaneChangeRight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (LaneChangeLeft) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ExtLaneChangeRight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ExtLaneChangeLeft) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (PullOver) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/pull_over + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (PullOut) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/pull_out + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (BlindSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Crosswalk) + Namespaces: + 1628_slow_down_factor_text: true + 1628_slow_down_virtual_wall: true + 1628_stop_factor_text: true + 1628_stop_virtual_wall: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Walkway) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/walkway + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DetectionArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Intersection) + Namespaces: + intersection1391_stop_factor_text: true + intersection1391_stop_virtual_wall: true + intersection_occlusion1391_stop_factor_text: true + intersection_occlusion1391_stop_virtual_wall: true + intersection_occlusion_first_stop1391_stop_factor_text: true + intersection_occlusion_first_stop1391_stop_virtual_wall: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (MergeFromPrivateArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (NoStoppingArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OcclusionSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (StopLine) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (TrafficLight) + Namespaces: + 1391_stop_factor_text: true + 1391_stop_virtual_wall: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (VirtualTrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (RunOut) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SpeedBump) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OutOfLane) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/out_of_lane + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Arrow + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection + Value: false + - Alpha: 0.5 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: color + Color Transformer: IntensityLayer + Enabled: false + Height Layer: elevation + Height Transformer: Layer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: IntersectionOcclusion + Show Grid Lines: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/occlusion_grid + Use Rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Blind Spot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: VirtualTrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: StopLine + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: OcclusionSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: RunOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Avoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneChange + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneFollowing + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_following + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: GoalPlanner + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/goal_planner + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pull_out + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SideShift + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/side_shift + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SpeedBump + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: OutOfLane + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/out_of_lane + Value: false + Enabled: false + Name: DebugMarker + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (Avoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (AvoidanceByLC) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/avoidance_by_lane_change + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (LaneChangeLeft) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_left + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (LaneChangeRight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_right + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (ExtLaneChangeLeft) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_left + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (ExtLaneChangeRight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_right + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (PullOver) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/pull_over + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Info (PullOut) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/pull_out + Value: false + Enabled: false + Name: InfoMarker + Enabled: true + Name: BehaviorPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/trajectory + Value: false + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Constant Width: false + Value: true + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleStop) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SurroundObstacle) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleCruise) + Namespaces: + stop_factor_text: true + stop_virtual_wall: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SurroundObstacleCheck + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Footprint + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 239; 41; 41 + Enabled: false + Name: FootprintOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 10; 21; 255 + Enabled: false + Name: FootprintRecoverOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset + Value: false + Enabled: false + Name: SurroundObstacleChecker + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: CruiseVirtualWall + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SlowDownVirtualWall + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: DebugMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker + Value: true + Enabled: false + Name: ObstacleCruise + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: false + Enabled: false + Name: DebugMarker + Enabled: true + Name: MotionPlanning + Enabled: true + Name: LaneDriving + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates + Use Timestamp: false + Value: false + - Alpha: 0.9990000128746033 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PartialPoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array + Value: true + - Alpha: 0.9990000128746033 + Arrow Length: 0.5 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 255 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array + Value: true + Enabled: true + Name: Parking + - Class: rviz_plugins/PoseWithUuidStamped + Enabled: true + Length: 1.5 + Name: ModifiedGoal + Radius: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/modified_goal + UUID: + Scale: 0.30000001192092896 + Value: false + Value: true + Enabled: true + Name: ScenarioPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: PlanningErrorMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker + Value: true + Enabled: false + Name: Diagnostic + Enabled: true + Name: Planning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: Predicted Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/lateral/predicted_trajectory + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 255; 255; 255 + Constant Color: true + Constant Width: true + Value: true + Width: 0.05000000074505806 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/MPC + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/mpc_follower/debug/markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/PurePursuit + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/controller_node_exe/debug/markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/AEB + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/autonomous_emergency_braking/debug/markers + Value: false + Enabled: true + Name: Control + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Class: tier4_adapi_rviz_plugins::RouteTool + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rviz/routing/rough_goal + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 3 + W vehicle width: 1.7999999523162842 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + W vehicle width: 2.5 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 14.160340309143066 + Target Frame: viewer + Value: TopDownOrtho (rviz_default_plugins) + X: -530.96728515625 + Y: 307.5765075683594 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + AutowareDateTimePanel: + collapsed: false + AutowareScreenCapturePanel: + collapsed: false + AutowareStatePanel: + collapsed: false + Displays: + collapsed: false + Height: 1538 + Hide Left Dock: false + Hide Right Dock: false + InitialPoseButtonPanel: + collapsed: false + MultiDataMonitor: + collapsed: false + QMainWindow State: 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 + RecognitionResultOnImage: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2371 + X: 1293 + Y: 77 diff --git a/autoware_launch/rviz/config/awsim.css b/autoware_launch/rviz/config/awsim.css new file mode 100755 index 0000000000..6ba091bdb5 --- /dev/null +++ b/autoware_launch/rviz/config/awsim.css @@ -0,0 +1,52 @@ +QLabel.value[test="gear"] { + font-size: 24px; + color: white; + background-color: black; + border-width: 1px 1px 1px 1px; + border-style: solid; +} + +QLabel.value[test="red_traffic_light"] { + font-weight: bold; + font-size: 12px; + color: black; + background-color: red; + border-width: 1px 1px 1px 1px; + border-style: solid; +} + +QLabel.value[test="green_traffic_light"] { + font-size: 12px; + font-weight: bold; + color: black; + background-color: green; + border-width: 1px 1px 1px 1px; + border-style: solid; +} + +QLabel.value[test="yellow_traffic_light"] { + font-size: 12px; + font-weight: bold; + color: black; + background-color: yellow; + border-width: 1px 1px 1px 1px; + border-style: solid; +} + +QLabel.value[test="unknown_traffic_light"] { + font-size: 12px; + font-weight: bold; + color: black; + background-color: gray; + border-width: 1px 1px 1px 1px; + border-style: solid; +} + +QLabel.value[test="none_traffic_light"] { + font-size: 12px; + font-weight: bold; + color: black; + background-color: white; + border-width: 1px 1px 1px 1px; + border-style: solid; +} diff --git a/autoware_launch/rviz/config/awsim_data_monitor.yaml b/autoware_launch/rviz/config/awsim_data_monitor.yaml new file mode 100755 index 0000000000..dac8fb56b5 --- /dev/null +++ b/autoware_launch/rviz/config/awsim_data_monitor.yaml @@ -0,0 +1,57 @@ +version: 2.0 + +stylesheets: + - path: package://multi_data_monitor/css/plugins/titled.css + target: Titled + - path: package://autoware_launch/rviz/config/awsim.css + - path: package://multi_data_monitor/css/colors.css + +widgets: + - class: Matrix + cols: 3 + items: + - class: Titled + title: Gear + input: vehicle_gear + rules: + class: SetFirstIf + type: uint + list: + - { eq: 0, value: NONE, attrs: { test: gear } } + - { eq: 1, value: N(Neutral range), attrs: { test: gear } } + - { eq: 22, value: P(Parking), attrs: { test: gear } } + - { eq: 30, value: R(Reverse range), attrs: { test: gear } } + - { eq: 2, value: D(Drive range), attrs: { test: gear } } + - { eq: 23, value: L(Low range), attrs: { test: gear } } + - class: Titled + title: NVTL + input: nvtl + - class: Titled + title: Traffic light + input: traffic_light_color + rules: + - class: Access + fails: ERROR-VALUE + field: [elements, 0, color] + - class: SetFirstIf + type: text + list: + - { eq: 1, value: Red, attrs: { test: red_traffic_light } } + - { eq: 2, value: Yellow, attrs: { test: yellow_traffic_light } } + - { eq: 3, value: Green, attrs: { test: green_traffic_light } } + - { eq: 0, value: Unknown, attrs: { test: unknown_traffic_light } } + - { eq: ERROR-VALUE, value: None, attrs: { test: none_traffic_light } } +streams: + - { class: subscription, label: vehicle_gear, topic: /vehicle/status/gear_status, field: report } + - class: subscription + label: nvtl_raw + topic: /localization/pose_estimator/nearest_voxel_transformation_likelihood + field: data + - { label: nvtl, class: apply, input: nvtl_raw, rules: round_number } + - class: subscription + label: traffic_light_color + topic: /awapi/traffic_light/get/nearest_traffic_signal + field: "" + +filters: + - { class: Precision, label: round_number, digits: 2 }