diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 65cf91e8c4..5ad60de003 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -18,18 +18,18 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.33.0 + rev: v0.34.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v3.0.0-alpha.6 + rev: v3.0.0-alpha.9-for-vscode hooks: - id: prettier - repo: https://github.com/adrienverge/yamllint - rev: v1.30.0 + rev: v1.32.0 hooks: - id: yamllint @@ -44,7 +44,7 @@ repos: - id: sort-package-xml - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.9.0.2 + rev: v0.9.0.5 hooks: - id: shellcheck diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md index c493cad4da..8dbcfb8510 100644 --- a/CODE_OF_CONDUCT.md +++ b/CODE_OF_CONDUCT.md @@ -60,7 +60,7 @@ representative at an online or offline event. Instances of abusive, harassing, or otherwise unacceptable behavior may be reported to the community leaders responsible for enforcement at -conduct@autoware.org. +. All complaints will be reviewed and investigated promptly and fairly. All community leaders are obligated to respect the privacy and security of the 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 6019eaae50..1a870aff7a 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 @@ -1,5 +1,6 @@ /**: ros__parameters: + publish_debug_pointcloud: false use_predicted_trajectory: true use_imu_path: true voxel_grid_x: 0.05 @@ -11,6 +12,9 @@ t_response: 1.0 a_ego_min: -3.0 a_obj_min: -1.0 - prediction_time_horizon: 1.5 - prediction_time_interval: 0.1 + imu_prediction_time_horizon: 1.5 + imu_prediction_time_interval: 0.1 + mpc_prediction_time_horizon: 1.5 + mpc_prediction_time_interval: 0.1 + collision_keeping_sec: 0.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 new file mode 100644 index 0000000000..ffd259e5e5 --- /dev/null +++ b/autoware_launch/config/control/control_validator/control_validator.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + + # 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 + # the next predicted_path is valid.) + diag_error_count_threshold: 0 + + display_on_terminal: false # show error msg on terminal + + thresholds: + max_distance_deviation: 1.0 diff --git a/autoware_launch/config/control/lane_departure_checker/lane_departure_checker.param.yaml b/autoware_launch/config/control/lane_departure_checker/lane_departure_checker.param.yaml index 59e25cbc5d..253737609b 100644 --- a/autoware_launch/config/control/lane_departure_checker/lane_departure_checker.param.yaml +++ b/autoware_launch/config/control/lane_departure_checker/lane_departure_checker.param.yaml @@ -1,5 +1,10 @@ /**: ros__parameters: + # Enable feature + will_out_of_lane_checker: false + out_of_lane_checker: false + boundary_departure_checker: true + # Node update_rate: 10.0 visualize_lanelet: false @@ -7,6 +12,7 @@ include_left_lanes: false include_opposite_lanes: false include_conflicting_lanes: false + boundary_types_to_detect: [curbstone] # Core footprint_margin_scale: 1.0 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 a86443f5ca..a4e8ab2c83 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 @@ -2,6 +2,10 @@ ros__parameters: transition_timeout: 10.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 + check_engage_condition: false # set false if you do not want to care about the engage condition. nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index diff --git a/autoware_launch/config/control/predicted_path_checker/predicted_path_checker.param.yaml b/autoware_launch/config/control/predicted_path_checker/predicted_path_checker.param.yaml new file mode 100644 index 0000000000..780d86b5e9 --- /dev/null +++ b/autoware_launch/config/control/predicted_path_checker/predicted_path_checker.param.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + # Node + update_rate: 10.0 + delay_time: 0.17 + max_deceleration: 1.5 + resample_interval: 0.5 + stop_margin: 0.5 # [m] + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + min_trajectory_check_length: 1.5 # [m] + trajectory_check_time: 3.0 + distinct_point_distance_threshold: 0.3 + distinct_point_yaw_threshold: 5.0 # [deg] + filtering_distance_threshold: 1.5 # [m] + use_object_prediction: true + + collision_checker_params: + width_margin: 0.2 + chattering_threshold: 0.2 + z_axis_filtering_buffer: 0.3 + enable_z_axis_obstacle_filtering: false diff --git a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml index 0bd5f74fd9..7820c562e8 100644 --- a/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/lateral/mpc.param.yaml @@ -13,7 +13,7 @@ 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 + extend_trajectory_for_end_yaw_control: false # flag of trajectory extending for terminal yaw control # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) @@ -47,7 +47,10 @@ vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics input_delay: 0.24 # steering input delay time for delay compensation vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_rate_lim_dps: 40.0 # steering angle rate limit [deg/s] + steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.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] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] @@ -62,6 +65,7 @@ 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: @@ -70,3 +74,5 @@ update_steer_threshold: 0.035 average_num: 1000 steering_offset_limit: 0.02 + + debug_publish_predicted_trajectory: false # publish debug predicted trajectory in Frenet coordinate diff --git a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml index bc3213081d..ad6217663f 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -32,7 +32,9 @@ 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 @@ -67,8 +69,9 @@ max_jerk: 2.0 min_jerk: -5.0 - # pitch - use_trajectory_for_pitch_calculation: false + # slope compensation lpf_pitch_gain: 0.95 + slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 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 9cc038352e..54c87f45b6 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 @@ -3,23 +3,34 @@ update_rate: 10.0 system_emergency_heartbeat_timeout: 0.5 use_emergency_handling: false + check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat) use_start_request: false + enable_cmd_limit_filter: true + filter_activated_count_threshold: 5 + filter_activated_velocity_threshold: 1.0 external_emergency_stop_heartbeat_timeout: 0.0 stop_hold_acceleration: -1.5 emergency_acceleration: -2.4 moderate_stop_service_acceleration: -1.5 stopped_state_entry_duration_time: 0.1 + stop_check_duration: 1.0 nominal: vel_lim: 25.0 - lon_acc_lim: 5.0 - lon_jerk_lim: 5.0 - lat_acc_lim: 5.0 - lat_jerk_lim: 5.0 - actual_steer_diff_lim: 1.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] on_transition: vel_lim: 50.0 - lon_acc_lim: 1.0 - lon_jerk_lim: 0.5 - lat_acc_lim: 2.0 - lat_jerk_lim: 7.0 - actual_steer_diff_lim: 1.0 + reference_speed_points: [20.0, 30.0] + steer_lim: [1.0, 0.8] + steer_rate_lim: [1.0, 0.8] + lon_acc_lim: [1.0, 0.9] + lon_jerk_lim: [0.5, 0.4] + lat_acc_lim: [2.0, 1.8] + lat_jerk_lim: [7.0, 6.0] + actual_steer_diff_lim: [1.0, 0.8] diff --git a/autoware_launch/config/localization/ar_tag_based_localizer.param.yaml b/autoware_launch/config/localization/ar_tag_based_localizer.param.yaml new file mode 100644 index 0000000000..e8f9016537 --- /dev/null +++ b/autoware_launch/config/localization/ar_tag_based_localizer.param.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + # marker_size + marker_size: 0.6 + + # target_tag_ids + target_tag_ids: ['0','1','2','3','4','5','6'] + + # base_covariance + # This value is dynamically scaled according to the distance at which AR tags are detected. + base_covariance: [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.02] + + # Detect AR-Tags within this range and publish the pose of ego vehicle + distance_threshold: 13.0 # [m] + + # consider_orientation + consider_orientation: false + + # Detector parameters + # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 + detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] + min_marker_size: 0.02 + + # Parameters for comparison with EKF Pose + # If the difference between the EKF pose and the current pose is within the range of values set below, the current pose is published. + # [How to determine the value] + # * ekf_time_tolerance: Since it is abnormal if the data comes too old from EKF, the tentative tolerance value is set at 5 seconds. + # This value is assumed to be unaffected even if it is increased or decreased by some amount. + # * ekf_position_tolerance: Since it is possible that multiple AR tags with the same ID could be placed, the tolerance should be as small as possible. + # And if the vehicle is running only on odometry in a section without AR tags, + # it is possible that self-position estimation could be off by a few meters. + # it should be fixed by AR tag detection, so tolerance should not be smaller than 10 meters. + # Therefore, the tolerance is set at 10 meters. + ekf_time_tolerance: 5.0 # [s] + ekf_position_tolerance: 10.0 # [m] diff --git a/autoware_launch/config/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml index 322325d239..4cc71e0904 100644 --- a/autoware_launch/config/localization/ekf_localizer.param.yaml +++ b/autoware_launch/config/localization/ekf_localizer.param.yaml @@ -1,23 +1,46 @@ /**: ros__parameters: - show_debug_info: false - enable_yaw_bias_estimation: True - predict_frequency: 50.0 - tf_rate: 50.0 - extend_state_step: 50 + node: + show_debug_info: false + enable_yaw_bias_estimation: true + predict_frequency: 50.0 + tf_rate: 50.0 + publish_tf: true + extend_state_step: 50 - # for Pose measurement - pose_additional_delay: 0.0 - pose_measure_uncertainty_time: 0.01 - pose_smoothing_steps: 5 - pose_gate_dist: 10000.0 + pose_measurement: + # for Pose measurement + pose_additional_delay: 0.0 + pose_measure_uncertainty_time: 0.01 + pose_smoothing_steps: 5 + pose_gate_dist: 10000.0 - # for twist measurement - twist_additional_delay: 0.0 - twist_smoothing_steps: 2 - twist_gate_dist: 10000.0 + twist_measurement: + # for twist measurement + twist_additional_delay: 0.0 + twist_smoothing_steps: 2 + twist_gate_dist: 10000.0 - # for process model - proc_stddev_yaw_c: 0.005 - proc_stddev_vx_c: 10.0 - proc_stddev_wz_c: 5.0 + process_noise: + # for process model + proc_stddev_yaw_c: 0.005 + proc_stddev_vx_c: 10.0 + proc_stddev_wz_c: 5.0 + + simple_1d_filter_parameters: + #Simple1DFilter parameters + z_filter_proc_dev: 1.0 + roll_filter_proc_dev: 0.01 + pitch_filter_proc_dev: 0.01 + + diagnostics: + # for diagnostics + pose_no_update_count_threshold_warn: 50 + pose_no_update_count_threshold_error: 100 + twist_no_update_count_threshold_warn: 50 + twist_no_update_count_threshold_error: 100 + + misc: + # for velocity measurement limitation (Set 0.0 if you want to ignore) + threshold_observable_velocity_mps: 0.0 # [m/s] + pose_frame_id: "map" diff --git a/autoware_launch/config/localization/localization_error_monitor.param.yaml b/autoware_launch/config/localization/localization_error_monitor.param.yaml index 026daf0532..def5c80164 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.0 - warn_ellipse_size: 0.8 + error_ellipse_size: 1.5 + warn_ellipse_size: 1.2 error_ellipse_size_lateral_direction: 0.3 - warn_ellipse_size_lateral_direction: 0.2 + warn_ellipse_size_lateral_direction: 0.25 diff --git a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher.param.yaml deleted file mode 100644 index 4c29059581..0000000000 --- a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml +++ /dev/null @@ -1,84 +0,0 @@ -/**: - ros__parameters: - # Use dynamic map loading - use_dynamic_map_loading: true - - # Vehicle reference frame - base_frame: "base_link" - - # Subscriber queue size - input_sensor_points_queue_size: 1 - - # The maximum difference between two consecutive - # transformations in order to consider convergence - trans_epsilon: 0.01 - - # The newton line search maximum step length - step_size: 0.1 - - # The ND voxel grid resolution - resolution: 2.0 - - # The number of iterations required to calculate alignment - max_iterations: 30 - - # Converged param type - # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - converged_param_type: 1 - - # If converged_param_type is 0 - # Threshold for deciding whether to trust the estimation result - converged_param_transform_probability: 3.0 - - # If converged_param_type is 1 - # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.3 - - # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 - - # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] - initial_pose_timeout_sec: 1.0 - - # Tolerance of distance difference between two initial poses used for linear interpolation. [m] - initial_pose_distance_tolerance_m: 10.0 - - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - - # Number of threads used for parallel computing - num_threads: 4 - - # The covariance of output pose - # Do note that this covariance matrix is empirically derived - output_pose_covariance: - [ - 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, - ] - - # Regularization switch - regularization_enabled: false - - # Regularization scale factor - regularization_scale_factor: 0.01 - - # Dynamic map loading distance - dynamic_map_loading_update_distance: 20.0 - - # Dynamic map loading loading radius - dynamic_map_loading_map_radius: 150.0 - - # Radius of input LiDAR range (used for diagnostics of dynamic map loading) - lidar_radius: 100.0 - - # A flag for using scan matching score based on de-grounded LiDAR scan - estimate_scores_for_degrounded_scan: false - - # If lidar_point.z - base_link.z <= this threshold , the point will be removed - z_margin_for_ground_removal: 0.8 diff --git a/autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml new file mode 100644 index 0000000000..144449ce75 --- /dev/null +++ b/autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml @@ -0,0 +1,114 @@ +/**: + ros__parameters: + frame: + # Vehicle reference frame + base_frame: "base_link" + + # NDT reference frame + ndt_base_frame: "ndt_base_link" + + # Map frame + map_frame: "map" + + + ndt: + # The maximum difference between two consecutive + # transformations in order to consider convergence + trans_epsilon: 0.01 + + # The newton line search maximum step length + step_size: 0.1 + + # The ND voxel grid resolution + resolution: 2.0 + + # The number of iterations required to calculate alignment + max_iterations: 30 + + # Number of threads used for parallel computing + num_threads: 4 + + regularization: + enable: false + + # Regularization scale factor + scale_factor: 0.01 + + + initial_pose_estimation: + # The number of particles to estimate initial pose + particles_num: 200 + + # 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. + # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. + n_startup_trials: 20 + + + validation: + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] + lidar_topic_timeout_sec: 1.0 + + # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] + initial_pose_timeout_sec: 1.0 + + # Tolerance of distance difference between two initial poses used for linear interpolation. [m] + initial_pose_distance_tolerance_m: 10.0 + + # The execution time which means probably NDT cannot matches scans properly. [ms] + critical_upper_bound_exe_time_ms: 100.0 + + + score_estimation: + # Converged param type + # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD + converged_param_type: 1 + + # If converged_param_type is 0 + # Threshold for deciding whether to trust the estimation result + converged_param_transform_probability: 3.0 + + # If converged_param_type is 1 + # Threshold for deciding whether to trust the estimation result + converged_param_nearest_voxel_transformation_likelihood: 2.3 + + # Scan matching score based on no ground LiDAR scan + no_ground_points: + enable: false + + # If lidar_point.z - base_link.z <= this threshold , the point will be removed + z_margin_for_ground_removal: 0.8 + + + covariance: + # The covariance of output pose + # Note that this covariance matrix is empirically derived + output_pose_covariance: + [ + 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, + ] + + # 2D Real-time covariance estimation with multiple searches (output_pose_covariance is the minimum value) + covariance_estimation: + enable: false + + # Offset arrangement in covariance estimation [m] + # initial_pose_offset_model_x & initial_pose_offset_model_y must have the same number of elements. + initial_pose_offset_model_x: [0.0, 0.0, 0.5, -0.5, 1.0, -1.0] + initial_pose_offset_model_y: [0.5, -0.5, 0.0, 0.0, 0.0, 0.0] + + + dynamic_map_loading: + # Dynamic map loading distance + update_distance: 20.0 + + # Dynamic map loading loading radius + map_radius: 150.0 + + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 diff --git a/autoware_launch/config/localization/crop_box_filter_measurement_range.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml similarity index 100% rename from autoware_launch/config/localization/crop_box_filter_measurement_range.param.yaml rename to autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/crop_box_filter_measurement_range.param.yaml diff --git a/autoware_launch/config/localization/random_downsample_filter.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/random_downsample_filter.param.yaml similarity index 100% rename from autoware_launch/config/localization/random_downsample_filter.param.yaml rename to autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/random_downsample_filter.param.yaml diff --git a/autoware_launch/config/localization/voxel_grid_filter.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/voxel_grid_filter.param.yaml similarity index 100% rename from autoware_launch/config/localization/voxel_grid_filter.param.yaml rename to autoware_launch/config/localization/ndt_scan_matcher/pointcloud_preprocessor/voxel_grid_filter.param.yaml diff --git a/autoware_launch/config/localization/pose_initializer.logging_simulator.gnss.param.yaml b/autoware_launch/config/localization/pose_initializer.logging_simulator.gnss.param.yaml deleted file mode 100644 index f32ac98604..0000000000 --- a/autoware_launch/config/localization/pose_initializer.logging_simulator.gnss.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - gnss_enabled: true - ndt_enabled: false - ekf_enabled: true - stop_check_enabled: false diff --git a/autoware_launch/config/localization/pose_initializer.logging_simulator.lidar.param.yaml b/autoware_launch/config/localization/pose_initializer.logging_simulator.lidar.param.yaml deleted file mode 100644 index 0895bf80bb..0000000000 --- a/autoware_launch/config/localization/pose_initializer.logging_simulator.lidar.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - gnss_enabled: true - ndt_enabled: true - ekf_enabled: true - stop_check_enabled: false diff --git a/autoware_launch/config/localization/pose_initializer.param.yaml b/autoware_launch/config/localization/pose_initializer.param.yaml index c1486b71aa..88acbfb526 100644 --- a/autoware_launch/config/localization/pose_initializer.param.yaml +++ b/autoware_launch/config/localization/pose_initializer.param.yaml @@ -1,6 +1,31 @@ /**: ros__parameters: - gnss_enabled: true - ndt_enabled: true - ekf_enabled: true - stop_check_enabled: true + gnss_pose_timeout: 3.0 # [sec] + stop_check_duration: 3.0 # [sec] + ekf_enabled: $(var ekf_enabled) + gnss_enabled: $(var gnss_enabled) + yabloc_enabled: $(var yabloc_enabled) + ndt_enabled: $(var ndt_enabled) + stop_check_enabled: $(var stop_check_enabled) + + # 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, + 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, + 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, + ] + + # output + output_pose_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, + 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, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, + ] diff --git a/autoware_launch/config/localization/pose_initializer.planning_simulator.param.yaml b/autoware_launch/config/localization/pose_initializer.planning_simulator.param.yaml deleted file mode 100644 index d6c960d08a..0000000000 --- a/autoware_launch/config/localization/pose_initializer.planning_simulator.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - gnss_enabled: false - ndt_enabled: false - ekf_enabled: false - stop_check_enabled: false diff --git a/autoware_launch/config/localization/pose_initializer_common.param.yaml b/autoware_launch/config/localization/pose_initializer_common.param.yaml deleted file mode 100644 index a05cc7c35c..0000000000 --- a/autoware_launch/config/localization/pose_initializer_common.param.yaml +++ /dev/null @@ -1,26 +0,0 @@ -/**: - ros__parameters: - gnss_pose_timeout: 3.0 # [sec] - stop_check_duration: 3.0 # [sec] - - # 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, - 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, - 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, - ] - - # output - output_pose_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, - 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, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, - ] diff --git a/autoware_launch/config/localization/twist2accel.param.yaml b/autoware_launch/config/localization/twist2accel.param.yaml new file mode 100644 index 0000000000..e58e029248 --- /dev/null +++ b/autoware_launch/config/localization/twist2accel.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + use_odom: true + accel_lowpass_gain: 0.9 diff --git a/autoware_launch/config/localization/yabloc/camera_particle_corrector.param.yaml b/autoware_launch/config/localization/yabloc/camera_particle_corrector.param.yaml new file mode 100644 index 0000000000..17c5a604bf --- /dev/null +++ b/autoware_launch/config/localization/yabloc/camera_particle_corrector.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + # Declared in AbstractParticleCorrector + acceptable_max_delay: 1.0 + visualize: false + + # Declared in ll2_cost_map + image_size: 800 # cost map image made by lanelet2 + max_range: 40.0 # [m] a cost map scale size + gamma: 5.0 # cost map intensity gradient + + min_prob: 0.1 # minimum weight of particles + far_weight_gain: 0.001 # exp(-far_weight_gain_ * squared_norm) is multiplied each measurement + enabled_at_first: true # developing feature diff --git a/autoware_launch/config/localization/yabloc/camera_pose_initializer.param.yaml b/autoware_launch/config/localization/yabloc/camera_pose_initializer.param.yaml new file mode 100644 index 0000000000..a0c38682b3 --- /dev/null +++ b/autoware_launch/config/localization/yabloc/camera_pose_initializer.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + angle_resolution: 30 diff --git a/autoware_launch/config/localization/yabloc/gnss_particle_corrector.param.yaml b/autoware_launch/config/localization/yabloc/gnss_particle_corrector.param.yaml new file mode 100644 index 0000000000..32267ce6f1 --- /dev/null +++ b/autoware_launch/config/localization/yabloc/gnss_particle_corrector.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + # Declared in AbstractParticleCorrector + acceptable_max_delay: 1.0 + visualize: false + + mahalanobis_distance_threshold: 30.0 # If the distance to GNSS observation exceeds this, the correction is skipped. + # + for_fixed/max_weight: 5.0 + for_fixed/flat_radius: 0.5 + for_fixed/max_radius: 10.0 + for_fixed/min_weight: 0.5 + # + for_not_fixed/flat_radius: 5.0 + for_not_fixed/max_radius: 20.0 + for_not_fixed/min_weight: 0.5 + for_not_fixed/max_weight: 1.0 diff --git a/autoware_launch/config/localization/yabloc/graph_segment.param.yaml b/autoware_launch/config/localization/yabloc/graph_segment.param.yaml new file mode 100644 index 0000000000..21879a2984 --- /dev/null +++ b/autoware_launch/config/localization/yabloc/graph_segment.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + # graph_node selects a road surface area from around this height + target_height_ratio: 0.65 + + # + target_candidate_box_width: 15 + + # graph_segment_node will pickup additional road-like areas + pickup_additional_graph_segment: true + + # used when the pickup_additional_graph_segment: true + similarity_score_threshold: 0.8 + + # parameters for cv::ximgproc::segmentation + sigma: 0.5 + k: 300.0 + min_size: 100.0 diff --git a/autoware_launch/config/localization/yabloc/ground_server.param.yaml b/autoware_launch/config/localization/yabloc/ground_server.param.yaml new file mode 100644 index 0000000000..25873ce799 --- /dev/null +++ b/autoware_launch/config/localization/yabloc/ground_server.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + force_zero_tilt: false + K: 50 + R: 10 diff --git a/autoware_launch/config/localization/yabloc/ll2_decomposer.param.yaml b/autoware_launch/config/localization/yabloc/ll2_decomposer.param.yaml new file mode 100644 index 0000000000..1bc6394a17 --- /dev/null +++ b/autoware_launch/config/localization/yabloc/ll2_decomposer.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + road_marking_labels: [cross_walk, zebra_marking, line_thin, line_thick, pedestrian_marking, stop_line, road_border] + sign_board_labels: [sign-board] + bounding_box_labels: [none] diff --git a/autoware_launch/config/localization/yabloc/predictor.param.yaml b/autoware_launch/config/localization/yabloc/predictor.param.yaml new file mode 100644 index 0000000000..c7e7b62d14 --- /dev/null +++ b/autoware_launch/config/localization/yabloc/predictor.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + visualize: true + + static_linear_covariance: 0.04 + static_angular_covariance: 0.006 + + resampling_interval_seconds: 1.0 + num_of_particles: 500 + prediction_rate: 50.0 + + cov_xx_yy: [2.0,0.25] diff --git a/autoware_launch/config/localization/yabloc/segment_filter.param.yaml b/autoware_launch/config/localization/yabloc/segment_filter.param.yaml new file mode 100644 index 0000000000..e2ba24d267 --- /dev/null +++ b/autoware_launch/config/localization/yabloc/segment_filter.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + min_segment_length: 1.5 # if it is negative, it is unlimited + max_segment_distance: 30.0 # if it is negative, it is unlimited + max_lateral_distance: 10.0 # if it is negative, it is unlimited + publish_image_with_segment_for_debug: true + max_range: 20.0 + image_size: 800 # debug diff --git a/autoware_launch/config/localization/yabloc/undistort.param.yaml b/autoware_launch/config/localization/yabloc/undistort.param.yaml new file mode 100644 index 0000000000..77f9be2c4a --- /dev/null +++ b/autoware_launch/config/localization/yabloc/undistort.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + use_sensor_qos: true + width: 800 + override_frame_id: "" # Value for overriding the camera's frame_id. If blank, frame_id of static_tf is not overwritten diff --git a/autoware_launch/config/map/lanelet2_map_loader.param.yaml b/autoware_launch/config/map/lanelet2_map_loader.param.yaml index 9abbaf2901..b830e038f1 100755 --- a/autoware_launch/config/map/lanelet2_map_loader.param.yaml +++ b/autoware_launch/config/map/lanelet2_map_loader.param.yaml @@ -1,7 +1,4 @@ /**: ros__parameters: - lanelet2_map_projector_type: MGRS # Options: MGRS, UTM, local - latitude: 40.81187906 # Latitude of map_origin, using in UTM - longitude: 29.35810110 # Longitude of map_origin, using in UTM - center_line_resolution: 5.0 # [m] + lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/autoware_launch/config/map/map_projection_loader.param.yaml b/autoware_launch/config/map/map_projection_loader.param.yaml new file mode 100644 index 0000000000..6ec300309a --- /dev/null +++ b/autoware_launch/config/map/map_projection_loader.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + map_projector_info_path: $(var map_projector_info_path) + lanelet2_map_path: $(var lanelet2_map_path) diff --git a/autoware_launch/config/map/map_tf_generator.param.yaml b/autoware_launch/config/map/map_tf_generator.param.yaml new file mode 100644 index 0000000000..90790808ac --- /dev/null +++ b/autoware_launch/config/map/map_tf_generator.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + map_frame: map + viewer_frame: viewer diff --git a/autoware_launch/config/map/pointcloud_map_loader.param.yaml b/autoware_launch/config/map/pointcloud_map_loader.param.yaml index ba4c032d3e..48d10944cb 100644 --- a/autoware_launch/config/map/pointcloud_map_loader.param.yaml +++ b/autoware_launch/config/map/pointcloud_map_loader.param.yaml @@ -3,8 +3,9 @@ enable_whole_load: true enable_downsampled_whole_load: false enable_partial_load: true - enable_differential_load: true enable_selected_load: false # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] + pcd_paths_or_directory: [$(var pointcloud_map_path)] # Path to the pointcloud map file or directory + pcd_metadata_path: $(var pointcloud_map_metadata_path) # Path to pointcloud metadata file diff --git a/autoware_launch/config/perception/object_recognition/detection/clustering/euclidean_cluster.param.yaml b/autoware_launch/config/perception/object_recognition/detection/clustering/euclidean_cluster.param.yaml new file mode 100644 index 0000000000..1411f766b4 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/clustering/euclidean_cluster.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + max_cluster_size: 1000 + min_cluster_size: 10 + tolerance: 0.7 + use_height: false diff --git a/autoware_launch/config/perception/object_recognition/detection/clustering/radar_object_clustering.param.yaml b/autoware_launch/config/perception/object_recognition/detection/clustering/radar_object_clustering.param.yaml new file mode 100644 index 0000000000..2abbf14622 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/clustering/radar_object_clustering.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # clustering parameter + angle_threshold: 0.174 # [rad] (10 deg) + distance_threshold: 10.0 # [m] + velocity_threshold: 4.0 # [m/s] + + # output object settings + # set false if you want to use the object information from radar + is_fixed_label: true + fixed_label: "CAR" + is_fixed_size: true + size_x: 4.0 # [m] + size_y: 1.5 # [m] + size_z: 1.5 # [m] 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 new file mode 100644 index 0000000000..26b027f007 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + tolerance: 0.7 + voxel_leaf_size: 0.3 + min_points_number_per_voxel: 1 + min_cluster_size: 10 + max_cluster_size: 3000 + use_height: false + input_frame: "base_link" + + # low height crop box filter param + max_x: 200.0 + min_x: -200.0 + max_y: 200.0 + min_y: -200.0 + max_z: 2.0 + min_z: -10.0 diff --git a/autoware_launch/config/perception/object_recognition/detection/detected_object_validation/obstacle_pointcloud_based_validator.param.yaml b/autoware_launch/config/perception/object_recognition/detection/detected_object_validation/obstacle_pointcloud_based_validator.param.yaml new file mode 100644 index 0000000000..b98f8adfb5 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/detected_object_validation/obstacle_pointcloud_based_validator.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + min_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10,10, 10, 10] + + max_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10,10, 10, 10] + + min_points_and_distance_ratio: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [800.0, 880.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] + + using_2d_validator: true diff --git a/autoware_launch/config/perception/object_recognition/detection/detection_by_tracker/detection_by_tracker.param.yaml b/autoware_launch/config/perception/object_recognition/detection/detection_by_tracker/detection_by_tracker.param.yaml new file mode 100644 index 0000000000..fac8687c5e --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/detection_by_tracker/detection_by_tracker.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + tracker_ignore_label.UNKNOWN : true + tracker_ignore_label.CAR : false + tracker_ignore_label.TRUCK : false + tracker_ignore_label.BUS : false + tracker_ignore_label.TRAILER : false + tracker_ignore_label.MOTORCYCLE : false + tracker_ignore_label.BICYCLE : false + tracker_ignore_label.PEDESTRIAN : false diff --git a/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_cluster_fusion.param.yaml b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_cluster_fusion.param.yaml new file mode 100644 index 0000000000..016a133227 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_cluster_fusion.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + fusion_distance: 100.0 + trust_object_distance: 100.0 + trust_object_iou_mode: "iou" + non_trust_object_iou_mode: "iou_x" + use_cluster_semantic_type: false + only_allow_inside_cluster: true + roi_scale_factor: 1.1 + iou_threshold: 0.65 + unknown_iou_threshold: 0.1 + remove_unknown: false diff --git a/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_detected_object_fusion.param.yaml b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_detected_object_fusion.param.yaml new file mode 100755 index 0000000000..bd49dc6574 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_detected_object_fusion.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + # UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + passthrough_lower_bound_probability_thresholds: [0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.35, 0.50] + trust_distances: [50.0, 100.0, 100.0, 100.0, 100.0, 50.0, 50.0, 50.0] + min_iou_threshold: 0.5 + use_roi_probability: false + roi_probability_threshold: 0.5 + + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN <-roi_msg + [1, 0, 0, 0, 0, 0, 0, 0, # UNKNOWN <-detected_objects + 0, 1, 1, 1, 1, 0, 0, 0, # CAR + 0, 1, 1, 1, 1, 0, 0, 0, # TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, # BUS + 0, 1, 1, 1, 1, 0, 0, 0, # TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, # MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, # BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] # PEDESTRIAN diff --git a/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_pointcloud_fusion.param.yaml b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_pointcloud_fusion.param.yaml new file mode 100644 index 0000000000..5b86b8e81d --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_pointcloud_fusion.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + fuse_unknown_only: true + min_cluster_size: 2 + cluster_2d_tolerance: 0.5 diff --git a/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_sync.param.yaml b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_sync.param.yaml new file mode 100644 index 0000000000..99d85089be --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/image_projection_based_fusion/roi_sync.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0] + timeout_ms: 70.0 + match_threshold_ms: 50.0 + image_buffer_size: 15 + debug_mode: false + filter_scope_min_x: -100.0 + filter_scope_min_y: -100.0 + filter_scope_min_z: -100.0 + filter_scope_max_x: 100.0 + filter_scope_max_y: 100.0 + filter_scope_max_z: 100.0 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 new file mode 100644 index 0000000000..a9c3174846 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + 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] + downsample_factor: 1 + encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + 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] + score_threshold: 0.35 + has_twist: false + trt_precision: fp16 + densification_num_past_frames: 1 + densification_world_frame_id: map + + # weight files + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_tiny.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_tiny.param.yaml new file mode 100644 index 0000000000..474d0e2b69 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_tiny.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + 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] + downsample_factor: 2 + encoder_in_feature_size: 9 + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + 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] + score_threshold: 0.35 + has_twist: false + trt_precision: fp16 + densification_num_past_frames: 1 + densification_world_frame_id: map + + # weight files + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/detection_class_remapper.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/detection_class_remapper.param.yaml new file mode 100644 index 0000000000..baea087c96 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/detection_class_remapper.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + allow_remapping_by_area_matrix: + # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. + # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] + # row: original class. column: class to remap to + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 0, 1, 0, 1, 0, 0, 0, #CAR + 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK + 0, 0, 0, 0, 1, 0, 0, 0, #BUS + 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE + 0, 0, 0, 0, 0, 0, 0, 0] #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.000, 0.000, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN + + + max_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.000, 0.000, 36.000, 0.000, 999.999, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/pointpainting.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/pointpainting.param.yaml new file mode 100755 index 0000000000..3abaffb243 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/pointpainting.param.yaml @@ -0,0 +1,32 @@ +/**: + ros__parameters: + trt_precision: fp16 + encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx" + encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine" + head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx" + head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine" + + model_params: + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"] + point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle + max_voxel_size: 40000 + 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: 12 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] + has_twist: false + densification_params: + world_frame_id: "map" + num_past_frames: 0 + post_process_params: + # post-process params + circle_nms_dist_threshold: 0.3 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + score_threshold: 0.35 + omp_params: + # omp params + num_threads: 1 diff --git a/autoware_launch/config/perception/object_recognition/detection/object_lanelet_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_filter/object_lanelet_filter.param.yaml similarity index 100% rename from autoware_launch/config/perception/object_recognition/detection/object_lanelet_filter.param.yaml rename to autoware_launch/config/perception/object_recognition/detection/object_filter/object_lanelet_filter.param.yaml diff --git a/autoware_launch/config/perception/object_recognition/detection/object_position_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_filter/object_position_filter.param.yaml similarity index 100% rename from autoware_launch/config/perception/object_recognition/detection/object_position_filter.param.yaml rename to autoware_launch/config/perception/object_recognition/detection/object_filter/object_position_filter.param.yaml diff --git a/autoware_launch/config/perception/object_recognition/detection/object_filter/radar_lanelet_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_filter/radar_lanelet_filter.param.yaml new file mode 100644 index 0000000000..62051e1c5e --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/object_filter/radar_lanelet_filter.param.yaml @@ -0,0 +1,11 @@ +/**: + ros__parameters: + filter_target_label: + UNKNOWN : true + CAR : true + TRUCK : true + BUS : true + TRAILER : true + MOTORCYCLE : true + BICYCLE : true + PEDESTRIAN : true diff --git a/autoware_launch/config/perception/object_recognition/detection/object_merger/data_association_matrix.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_merger/data_association_matrix.param.yaml new file mode 100644 index 0000000000..e1bf2c67b3 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/object_merger/data_association_matrix.param.yaml @@ -0,0 +1,44 @@ +/**: + ros__parameters: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN diff --git a/autoware_launch/config/perception/object_recognition/detection/object_merger/overlapped_judge.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_merger/overlapped_judge.param.yaml new file mode 100644 index 0000000000..15ca8dd713 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/object_merger/overlapped_judge.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + distance_threshold_list: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] #UNKNOWN + + generalized_iou_threshold: + [-0.1, -0.1,-0.1, -0.6, -0.6, -0.1, -0.1, -0.1] diff --git a/autoware_launch/config/perception/object_recognition/detection/object_range_splitter/object_range_splitter_radar.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_range_splitter/object_range_splitter_radar.param.yaml new file mode 100644 index 0000000000..ed1ce176ed --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/object_range_splitter/object_range_splitter_radar.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + split_range: 80.0 diff --git a/autoware_launch/config/perception/object_recognition/detection/object_range_splitter/object_range_splitter_radar_fusion.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_range_splitter/object_range_splitter_radar_fusion.param.yaml new file mode 100644 index 0000000000..a8b2562883 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/object_range_splitter/object_range_splitter_radar_fusion.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + split_range: 70.0 diff --git a/autoware_launch/config/perception/object_recognition/detection/object_velocity_splitter/object_velocity_splitter_radar.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_velocity_splitter/object_velocity_splitter_radar.param.yaml new file mode 100644 index 0000000000..c6351bea72 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/object_velocity_splitter/object_velocity_splitter_radar.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_threshold: 5.5 diff --git a/autoware_launch/config/perception/object_recognition/detection/object_velocity_splitter/object_velocity_splitter_radar_fusion.param.yaml b/autoware_launch/config/perception/object_recognition/detection/object_velocity_splitter/object_velocity_splitter_radar_fusion.param.yaml new file mode 100644 index 0000000000..6a40ac9616 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/object_velocity_splitter/object_velocity_splitter_radar_fusion.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_threshold: 4.5 diff --git a/autoware_launch/config/perception/object_recognition/detection/pointcloud_map_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml similarity index 82% rename from autoware_launch/config/perception/object_recognition/detection/pointcloud_map_filter.param.yaml rename to autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml index deffe001e0..62b3074c15 100644 --- a/autoware_launch/config/perception/object_recognition/detection/pointcloud_map_filter.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml @@ -1,7 +1,5 @@ /**: ros__parameters: - # use downsample filter before compare map - use_down_sample_filter: False # voxel size for downsample filter down_sample_voxel_size: 0.1 @@ -9,6 +7,9 @@ # distance threshold for compare compare distance_threshold: 0.5 + # ratio to reduce voxel_leaf_size and neighbor points distance threshold in z axis + downsize_ratio_z_axis: 0.6 + # publish voxelized map pointcloud for debug publish_debug_pcd: False diff --git a/autoware_launch/config/perception/object_recognition/detection/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter.param.yaml new file mode 100644 index 0000000000..f52edf641f --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + angle_threshold: 1.2210 + velocity_threshold: 1.5 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 new file mode 100644 index 0000000000..f8ad371ab9 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml @@ -0,0 +1,46 @@ +/**: + ros__parameters: + enable_delay_compensation: true + prediction_time_horizon: 10.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] + min_crosswalk_user_velocity: 1.39 #[m/s] + max_crosswalk_user_delta_yaw_threshold_for_lanelet: 0.785 #[m/s] + dist_threshold_for_searching_lanelet: 3.0 #[m] + delta_yaw_threshold_for_searching_lanelet: 0.785 #[rad] + sigma_lateral_offset: 0.5 #[m] + sigma_yaw_angle_deg: 5.0 #[angle degree] + object_buffer_time_length: 2.0 #[s] + history_time_length: 1.0 #[s] + check_lateral_acceleration_constraints: false # whether to check if the predicted path complies with lateral acceleration constraints + max_lateral_accel: 2.0 # [m/ss] max acceptable lateral acceleration for predicted vehicle paths + min_acceleration_before_curve: -2.0 # [m/ss] min acceleration a vehicle might take to decelerate before a curve + use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not + speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value + acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds + crosswalk_with_signal: + use_crosswalk_signal: true + threshold_velocity_assumed_as_stopping: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped + # If the pedestrian does not move for X seconds against green signal, the module judge that the pedestrian has no intention to walk. + distance_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order, keys of map + timeout_set_for_no_intention_to_walk: [1.0, 0.0] # [s] values of map + # parameter for shoulder lane prediction + prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8 + + # parameters for lc prediction + lane_change_detection: + method: "lat_diff_distance" # choose from "lat_diff_distance" or "time_to_change_lane" + time_to_change_lane: + dist_threshold_for_lane_change_detection: 1.0 #[m] + time_threshold_for_lane_change_detection: 5.0 #[s] + cutoff_freq_of_velocity_for_lane_change_detection: 0.1 #[Hz] + lat_diff_distance: + dist_ratio_threshold_to_left_bound: -0.5 #[ratio] + dist_ratio_threshold_to_right_bound: 0.5 #[ratio + diff_dist_threshold_to_left_bound: 0.29 #[m] + diff_dist_threshold_to_right_bound: -0.29 #[m] + num_continuous_state_transition: 3 + consider_only_routable_neighbours: false + + reference_path_resolution: 0.5 #[m] 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 69af202e7a..13f2220655 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 @@ -30,9 +30,9 @@ 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER - 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE - 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE - 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.50, #MOTORBIKE + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.50, #BICYCLE + 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 diff --git a/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/multi_object_tracker_node.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/multi_object_tracker_node.param.yaml new file mode 100644 index 0000000000..32a12f8bf5 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/multi_object_tracker/multi_object_tracker_node.param.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + # default tracker models for each class + car_tracker: "multi_vehicle_tracker" + truck_tracker: "multi_vehicle_tracker" + bus_tracker: "multi_vehicle_tracker" + trailer_tracker: "multi_vehicle_tracker" + pedestrian_tracker: "pedestrian_and_bicycle_tracker" + bicycle_tracker: "pedestrian_and_bicycle_tracker" + motorcycle_tracker: "pedestrian_and_bicycle_tracker" + + # default tracker node parameters + publish_rate: 10.0 + world_frame_id: map + enable_delay_compensation: true + pass_through_unknown_objects: false + publish_untracked_objects: false + + # debug parameters + publish_processing_time: true + publish_tentative_objects: false + diagnostics_warn_delay: 0.5 # [sec] + diagnostics_error_delay: 1.0 # [sec] diff --git a/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/data_association_matrix.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/data_association_matrix.param.yaml new file mode 100644 index 0000000000..69628414e6 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/data_association_matrix.param.yaml @@ -0,0 +1,66 @@ +/**: + ros__parameters: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN <-Measurement + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN <-Tracker + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN + 4.0, 8.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #CAR + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #BUS + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRAILER + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE + 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + max_area_matrix: + # NOTE: The size of truck is 12 m length x 3 m width. + # NOTE: The size of trailer is 20 m length x 3 m width. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN + 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR + 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS + 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE + 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE + 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #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 + 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 + 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE + 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, -0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN diff --git a/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/default_tracker.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/default_tracker.param.yaml new file mode 100644 index 0000000000..6c26034860 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/default_tracker.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + car_tracker: "constant_turn_rate_motion_tracker" + truck_tracker: "constant_turn_rate_motion_tracker" + bus_tracker: "constant_turn_rate_motion_tracker" + trailer_tracker: "constant_turn_rate_motion_tracker" + pedestrian_tracker: "constant_turn_rate_motion_tracker" + bicycle_tracker: "constant_turn_rate_motion_tracker" + motorcycle_tracker: "constant_turn_rate_motion_tracker" diff --git a/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml new file mode 100644 index 0000000000..3a1e9f02c3 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml @@ -0,0 +1,29 @@ +# general parameters for radar_object_tracker node +/**: + ros__parameters: + # basic settings + world_frame_id: "map" + tracker_lifetime: 1.0 # [sec] + measurement_count_threshold: 3 # object will be published if it is tracked more than this threshold + + # delay compensate parameters + publish_rate: 10.0 + enable_delay_compensation: true + + # logging + enable_logging: false + logging_file_path: "/tmp/association_log.json" + + # filtering + ## 1. distance based filtering: remove closer objects than this threshold + use_distance_based_noise_filtering: true + minimum_range_threshold: 60.0 # [m] + + ## 2. lanelet map based filtering + use_map_based_noise_filtering: true + max_distance_from_lane: 5.0 # [m] + max_angle_diff_from_lane: 0.785398 # [rad] (45 deg) + max_lateral_velocity: 7.0 # [m/s] + + # tracking model parameters + tracking_config_directory: $(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/radar_object_tracker/tracking/ diff --git a/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/constant_turn_rate_motion_tracker.yaml b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/constant_turn_rate_motion_tracker.yaml new file mode 100644 index 0000000000..f80f881cab --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/constant_turn_rate_motion_tracker.yaml @@ -0,0 +1,36 @@ +default: + # This file defines the parameters for the linear motion tracker. + # All this parameter coordinate is assumed to be in the vehicle coordinate system. + # So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle. + ekf_params: + # random walk noise is used to model the acceleration noise + process_noise_std: # [m/s^2] + x: 0.5 + y: 0.5 + yaw: 0.1 + vx: 1.0 # assume 1m/s velocity noise + wz: 0.4 + measurement_noise_std: + x: 4.0 # [m] + y: 4.0 # [m] + # y: 0.02 # rad/m if use_polar_coordinate_in_measurement_noise is true + yaw: 0.2 # [rad] + vx: 10 # [m/s] + initial_covariance_std: + x: 3.0 # [m] + y: 6.0 # [m] + yaw: 10.0 # [rad] + vx: 100.0 # [m/s] + wz: 10.0 # [rad/s] + # input flag + trust_yaw_input: false # set true if yaw input of sensor is reliable + trust_twist_input: false # set true if twist input of sensor is reliable + use_polar_coordinate_in_measurement_noise: false # set true if you want to define the measurement noise in polar coordinate + assume_zero_yaw_rate: false # set true if you want to assume zero yaw rate + # output limitation + limit: + max_speed: 80.0 # [m/s] + # low pass filter is used to smooth the yaw and shape estimation + low_pass_filter: + time_constant: 1.0 # [s] + sampling_time: 0.1 # [s] diff --git a/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/linear_motion_tracker.yaml b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/linear_motion_tracker.yaml new file mode 100644 index 0000000000..5e813558a2 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/radar_object_tracker/tracking/linear_motion_tracker.yaml @@ -0,0 +1,38 @@ +default: + # This file defines the parameters for the linear motion tracker. + # All this parameter coordinate is assumed to be in the vehicle coordinate system. + # So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle. + ekf_params: + # random walk noise is used to model the acceleration noise + process_noise_std: # [m/s^2] + ax: 0.98 # assume 0.1G acceleration noise + ay: 0.98 + vx: 1.0 # assume 1m/s velocity noise + vy: 1.0 + x: 1.0 # assume 1m position noise + y: 1.0 + measurement_noise_std: + x: 0.6 # [m] + # y: 4.0 # [m] + y: 0.01 # rad/m if use_polar_coordinate_in_measurement_noise is true + vx: 10 # [m/s] + vy: 100 # [m/s] + initial_covariance_std: + x: 3.0 # [m] + y: 6.0 # [m] + vx: 1000.0 # [m/s] + vy: 1000.0 # [m/s] + ax: 1000.0 # [m/s^2] + ay: 1000.0 # [m/s^2] + estimate_acc: false # set true if you want to estimate the acceleration + # input flag + trust_yaw_input: false # set true if yaw input of sensor is reliable + trust_twist_input: false # set true if twist input of sensor is reliable + use_polar_coordinate_in_measurement_noise: true # set true if you want to define the measurement noise in polar coordinate + # output limitation + limit: + max_speed: 80.0 # [m/s] + # low pass filter is used to smooth the yaw and shape estimation + low_pass_filter: + time_constant: 1.0 # [s] + sampling_time: 0.1 # [s] diff --git a/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml new file mode 100644 index 0000000000..c232dbde40 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/data_association_matrix.param.yaml @@ -0,0 +1,168 @@ +/**: + ros__parameters: + lidar-lidar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN + + lidar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 5.5, 6.0, 6.0, 6.0, 1.0, 1.0, 1.0, #CAR + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #BUS + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN + + radar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 7.0, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #CAR + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #BUS + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN diff --git a/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml new file mode 100644 index 0000000000..e4ea2becd7 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger.param.yaml @@ -0,0 +1,26 @@ +# Node parameters +/**: + ros__parameters: + base_link_frame_id: "base_link" + time_sync_threshold: 0.999 + sub_object_timeout_sec: 0.8 + publish_interpolated_sub_objects: true #for debug + + # choose the input sensor type for each object type + # "lidar", "radar", "camera" are available + main_sensor_type: "lidar" + sub_sensor_type: "radar" + + # tracker settings + tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.5 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.5 + decay_rate: 0.1 + max_dt: 1.0 + + # logging + enable_logging: false + logging_file_path: "/tmp/decorative_tracker_merger.log" diff --git a/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger_policy.param.yaml b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger_policy.param.yaml new file mode 100644 index 0000000000..0b98e1b202 --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/tracking/tracking_object_merger/decorative_tracker_merger_policy.param.yaml @@ -0,0 +1,16 @@ +# Merger policy for decorative tracker merger node +# decorative tracker merger works by merging the sub-object trackers into a main object tracker result +# There are 3 merger policy: +# 1. "skip": skip the sub-object tracker result +# 2. "overwrite": overwrite the main object tracker result with sub-object tracker result +# 3. "fusion": merge the main object tracker result with sub-object tracker result by using covariance based fusion +/**: + ros__parameters: + kinematics_to_be_merged: "velocity" # currently only support "velocity" + + # choose the merger policy for each object type + # : "skip", "overwrite", "fusion" + kinematics_merge_policy: "overwrite" + classification_merge_policy: "skip" + existence_prob_merge_policy: "skip" + shape_merge_policy: "skip" diff --git a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml index b367fef386..4c58c066ed 100644 --- a/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ b/autoware_launch/config/perception/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml @@ -30,3 +30,6 @@ detection_range_z_max: 2.5 elevation_grid_mode: true use_recheck_ground_cluster: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 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 new file mode 100644 index 0000000000..4e335e3574 --- /dev/null +++ b/autoware_launch/config/perception/occupancy_grid_map/binary_bayes_filter_updater.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + probability_matrix: + occupied_to_occupied: 0.95 + occupied_to_free: 0.05 + free_to_occupied: 0.2 + free_to_free: 0.8 + v_ratio: 10.0 diff --git a/autoware_launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml b/autoware_launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml index 3899b9f464..9dcc722587 100644 --- a/autoware_launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml +++ b/autoware_launch/config/perception/occupancy_grid_map/laserscan_based_occupancy_grid_map.param.yaml @@ -7,7 +7,11 @@ # ray-tracing center: main sensor frame is preferable like: "velodyne_top" scan_origin_frame: "base_link" - use_height_filter: true + height_filter: + use_height_filter: true + min_height: -1.0 + max_height: 2.0 + enable_single_frame_mode: false map_length: 150.0 map_width: 150.0 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 ad93512d64..8077bdec50 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 @@ -3,7 +3,11 @@ map_length: 150.0 # [m] map_resolution: 0.5 # [m] - use_height_filter: true + height_filter: + use_height_filter: true + min_height: -1.0 + max_height: 2.0 + enable_single_frame_mode: false # use sensor pointcloud to filter obstacle pointcloud filter_obstacle_pointcloud_by_raw_pointcloud: false @@ -14,4 +18,13 @@ # center of the grid map 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" + + grid_map_type: "OccupancyGridMapFixedBlindSpot" + OccupancyGridMapFixedBlindSpot: + distance_margin: 1.0 + OccupancyGridMapProjectiveBlindSpot: + projection_dz_threshold: 0.01 # [m] for avoiding null division + obstacle_separation_threshold: 1.0 # [m] fill the interval between obstacles with unknown for this length + pub_debug_grid: false diff --git a/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml b/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml new file mode 100644 index 0000000000..dfe12ff352 --- /dev/null +++ b/autoware_launch/config/perception/traffic_light_arbiter/traffic_light_arbiter.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + external_time_tolerance: 5.0 + perception_time_tolerance: 1.0 + external_priority: false + enable_signal_matching: false diff --git a/autoware_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml b/autoware_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml index d596eb9816..9b7dcffbc6 100644 --- a/autoware_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml +++ b/autoware_launch/config/planning/mission_planning/mission_planner/mission_planner.param.yaml @@ -8,3 +8,5 @@ enable_correct_goal_pose: false reroute_time_threshold: 10.0 minimum_reroute_length: 30.0 + consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not. + check_footprint_inside_lanes: true diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml new file mode 100644 index 0000000000..69328a9b79 --- /dev/null +++ b/autoware_launch/config/planning/preset/default_preset.yaml @@ -0,0 +1,123 @@ +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: "false" + - arg: + name: launch_sampling_planner_module + default: "false" # Warning, experimental module, use only in simulations + - 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" + + # 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" + - arg: + name: launch_dynamic_obstacle_stop_module + default: "true" + + # 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_stop_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: "false" + + # 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 6bb130e805..face8610c7 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + max_vel: 11.1 # max velocity limit [m/s] + # constraints param for normal driving normal: min_acc: -1.0 # min deceleration [m/ss] @@ -7,6 +9,10 @@ min_jerk: -1.0 # min jerk [m/sss] max_jerk: 1.0 # max jerk [m/sss] + slow_down: + min_acc: -1.0 # min deceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + # constraints to be observed limit: min_acc: -2.5 # min deceleration limit [m/ss] diff --git a/autoware_launch/config/planning/scenario_planning/common/costmap_generator.param.yaml b/autoware_launch/config/planning/scenario_planning/common/costmap_generator.param.yaml new file mode 100644 index 0000000000..cf4f73dc79 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/common/costmap_generator.param.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + costmap_frame: "map" + vehicle_frame: "base_link" + map_frame: "map" + update_rate: 10.0 + activate_by_scenario: False + grid_min_value: 0.0 + grid_max_value: 1.0 + grid_resolution: 0.2 + grid_length_x: 70.0 + grid_length_y: 70.0 + grid_position_x: 0.0 + grid_position_y: 0.0 + maximum_lidar_height_thres: 0.3 + minimum_lidar_height_thres: -2.2 + use_wayarea: true + use_parkinglot: true + use_objects: true + use_points: true + expand_polygon_size: 1.0 + size_of_expansion_kernel: 9 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 a6906b7117..f74ca045f5 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: 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_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_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 e600334ee2..ff97ae8dfb 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 @@ -1,23 +1,31 @@ /**: ros__parameters: # motion state constraints - max_velocity: 20.0 # max velocity limit [m/s] stop_decel: 0.0 # deceleration at a stop point[m/ss] # external velocity limit parameter margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] - # curve parameters + # -- curve parameters -- + # common parameters + 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] + # 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: 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.5 # 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] @@ -48,11 +56,7 @@ post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] - # steering angle rate limit parameters - max_steering_angle_rate: 40.0 # 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] - curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m] - # system over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point + + plan_from_ego_speed_on_manual_mode: true # planning is done from ego velocity/acceleration on MANUAL mode. This should be true for smooth transition from MANUAL to AUTONOMOUS, but could be false for debugging. diff --git a/autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml b/autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml index 658a968906..da337d70b1 100644 --- a/autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/planning_validator/planning_validator.param.yaml @@ -26,3 +26,14 @@ steering_rate: 10.0 velocity_deviation: 100.0 distance_deviation: 100.0 + longitudinal_distance_deviation: 1.0 + + parameters: + # The required trajectory length is calculated as the distance needed + # to stop from the current speed at this deceleration. + forward_trajectory_length_acceleration: -3.0 + + # An error is raised if the required trajectory length is less than this distance. + # Setting it to 0 means an error will occur if even slightly exceeding the end of the path, + # therefore, a certain margin is necessary. + forward_trajectory_length_margin: 2.0 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 e0b709cfe8..3d59774e4a 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 @@ -4,21 +4,22 @@ avoidance: resample_interval_for_planning: 0.3 # [m] resample_interval_for_output: 4.0 # [m] - detection_area_right_expand_dist: 0.0 # [m] - detection_area_left_expand_dist: 1.0 # [m] drivable_area_right_bound_offset: 0.0 # [m] drivable_area_left_bound_offset: 0.0 # [m] # avoidance module common setting enable_bound_clipping: false - enable_avoidance_over_same_direction: true - enable_avoidance_over_opposite_direction: true - enable_update_path_when_object_is_gone: false - enable_force_avoidance_for_stopped_vehicle: false - enable_safety_check: true enable_yield_maneuver: true + enable_yield_maneuver_during_shifting: false + enable_cancel_maneuver: true disable_path_update: false - use_hatched_road_markings: false + + # drivable area setting + use_adjacent_lane: true + use_opposite_lane: true + use_intersection_areas: true + use_hatched_road_markings: true + use_freespace_areas: true # for debug publish_debug_marker: false @@ -27,90 +28,206 @@ # avoidance is performed for the object type with true target_object: car: - enable: true - envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 - safety_buffer_longitudinal: 0.0 + execute_num: 1 # [-] + moving_speed_threshold: 1.0 # [m/s] + moving_time_threshold: 2.0 # [s] + max_expand_ratio: 0.0 # [-] + envelope_buffer_margin: 0.5 # [m] + avoid_margin_lateral: 0.7 # [m] + safety_buffer_lateral: 0.3 # [m] + safety_buffer_longitudinal: 0.0 # [m] + use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: - enable: true - envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 + execute_num: 1 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 2.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + avoid_margin_lateral: 0.9 + safety_buffer_lateral: 0.1 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bus: - enable: true - envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 + execute_num: 1 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 2.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + avoid_margin_lateral: 0.9 + safety_buffer_lateral: 0.1 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true trailer: - enable: true - envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 + execute_num: 1 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 2.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + avoid_margin_lateral: 0.9 + safety_buffer_lateral: 0.1 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true unknown: - enable: false - envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 + execute_num: 1 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.1 + avoid_margin_lateral: 0.7 + safety_buffer_lateral: -0.2 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bicycle: - enable: false - envelope_buffer_margin: 0.8 - safety_buffer_lateral: 1.0 + execute_num: 1 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + avoid_margin_lateral: 0.7 + safety_buffer_lateral: 0.5 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true motorcycle: - enable: false - envelope_buffer_margin: 0.8 - safety_buffer_lateral: 1.0 + execute_num: 1 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + avoid_margin_lateral: 0.7 + safety_buffer_lateral: 0.3 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true pedestrian: - enable: false - envelope_buffer_margin: 0.8 - safety_buffer_lateral: 1.0 + execute_num: 1 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.5 + avoid_margin_lateral: 0.7 + safety_buffer_lateral: 0.5 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true + lower_distance_for_polygon_expansion: 30.0 # [m] + upper_distance_for_polygon_expansion: 100.0 # [m] # For target object filtering target_filtering: - # filtering moving objects - threshold_speed_object_is_stopped: 1.0 # [m/s] - threshold_time_object_is_moving: 1.0 # [s] - threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] + # avoidance target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: true # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] # detection range - object_ignore_distance_traffic_light: 30.0 # [m] - object_ignore_distance_crosswalk_forward: 30.0 # [m] - object_ignore_distance_crosswalk_backward: 30.0 # [m] - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 100.0 # [m] - object_check_goal_distance: 20.0 # [m] + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] - object_check_shiftable_ratio: 0.6 # [-] + object_check_shiftable_ratio: 0.8 # [-] object_check_min_road_shoulder_width: 0.5 # [m] # lost object compensation object_last_seen_threshold: 2.0 + # detection area generation parameters + detection_area: + static: false # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + + # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. + force_avoidance: + enable: true # [-] + time_threshold: 3.0 # [s] + distance_threshold: 1.0 # [m] + ignore_area: + traffic_light: + front_distance: 100.0 # [m] + crosswalk: + front_distance: 30.0 # [m] + behind_distance: 30.0 # [m] + + # params for filtering objects that are in intersection + intersection: + yaw_deviation: 0.349 # [rad] (default 20.0deg) + # For safety check safety_check: + # safety check target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: false # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] + # safety check configuration + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] + # collision check parameters + check_all_predicted_path: false # [-] safety_check_backward_distance: 100.0 # [m] - safety_check_time_horizon: 10.0 # [s] - safety_check_idling_time: 1.5 # [s] - safety_check_accel_for_rss: 2.5 # [m/ss] - safety_check_hysteresis_factor: 2.0 # [-] + hysteresis_factor_expand_rate: 1.5 # [-] + hysteresis_factor_safe_count: 10 # [-] + # predicted path parameters + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 3.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] + # rss parameters + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.8 # [s] # For avoidance maneuver avoidance: # avoidance lateral parameters lateral: - lateral_collision_margin: 1.0 # [m] - lateral_execution_threshold: 0.499 # [m] - road_shoulder_safety_margin: 0.3 # [m] + lateral_execution_threshold: 0.09 # [m] + lateral_small_shift_threshold: 0.101 # [m] + lateral_avoid_check_threshold: 0.1 # [m] + soft_road_shoulder_margin: 0.3 # [m] + hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 + max_deviation_from_lane: 0.2 # [m] + # approve the next shift line after reaching this percentage of the current shift line length. + # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. + # this feature can be disabled by setting this parameter to 0.0. + ratio_for_return_shift_approval: 0.5 # [-] # avoidance distance parameters longitudinal: - prepare_time: 2.0 # [s] + min_prepare_time: 1.0 # [s] + max_prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] - min_avoidance_distance: 10.0 # [m] - min_nominal_avoidance_speed: 7.0 # [m/s] - min_sharp_avoidance_speed: 1.0 # [m/s] + min_slow_down_speed: 1.38 # [m/s] + buf_slow_down_speed: 0.56 # [m/s] + nominal_avoidance_speed: 8.33 # [m/s] + # return dead line + return_dead_line: + goal: + enable: true # [-] + buffer: 3.0 # [m] + traffic_light: + enable: true # [-] + buffer: 3.0 # [m] # For yield maneuver yield: @@ -118,37 +235,48 @@ # For stop maneuver stop: - min_distance: 10.0 # [m] max_distance: 20.0 # [m] + stop_buffer: 1.0 # [m] - constraints: - # vehicle slows down under longitudinal constraints - use_constraints_for_decel: false # [-] + policy: + # policy for rtc request. select "per_shift_line" or "per_avoidance_maneuver". + # "per_shift_line": request approval for each shift line. + # "per_avoidance_maneuver": request approval for avoidance maneuver (avoid + return). + make_approval_request: "per_shift_line" + # policy for vehicle slow down behavior. select "best_effort" or "reliable". + # "best_effort": slow down deceleration & jerk are limited by constraints. + # but there is a possibility that the vehicle can't stop in front of the vehicle. + # "reliable": insert stop or slow down point with ignoring decel/jerk constraints. + # make it possible to increase chance to avoid but uncomfortable deceleration maybe happen. + deceleration: "best_effort" # [-] + # policy for avoidance lateral margin. select "best_effort" or "reliable". + # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal + # margin to avoid. + # "reliable": module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid + # with expected lateral margin. + lateral_margin: "best_effort" # [-] + # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. + use_shorten_margin_immediately: true # [-] + constraints: # lateral constraints lateral: - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] + velocity: [1.0, 1.38, 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] # longitudinal constraints longitudinal: nominal_deceleration: -1.0 # [m/ss] nominal_jerk: 0.5 # [m/sss] - max_deceleration: -2.0 # [m/ss] - max_jerk: 1.0 - # For prevention of large acceleration while avoidance - min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] - max_avoidance_acceleration: 0.5 # [m/ss] - - target_velocity_matrix: - col_size: 2 - matrix: - [2.78, 13.9, # velocity [m/s] - 0.50, 1.00] # margin [m] + max_deceleration: -1.5 # [m/ss] + max_jerk: 1.0 # [m/sss] + max_acceleration: 1.0 # [m/ss] shift_line_pipeline: trim: - quantize_filter_threshold: 0.2 + quantize_filter_threshold: 0.1 same_grad_filter_1_threshold: 0.1 same_grad_filter_2_threshold: 0.2 same_grad_filter_3_threshold: 0.5 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lane_change/avoidance_by_lane_change.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lane_change/avoidance_by_lane_change.param.yaml new file mode 100644 index 0000000000..74c6112c0e --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lane_change/avoidance_by_lane_change.param.yaml @@ -0,0 +1,87 @@ +/**: + ros__parameters: + avoidance_by_lane_change: + execute_object_longitudinal_margin: 80.0 + execute_only_when_lane_change_finish_before_object: false + + # avoidance is performed for the object type with true + target_object: + car: + execute_num: 2 # [-] + moving_speed_threshold: 1.0 # [m/s] + moving_time_threshold: 1.0 # [s] + max_expand_ratio: 0.0 # [-] + envelope_buffer_margin: 0.3 # [m] + avoid_margin_lateral: 0.0 # [m] + safety_buffer_lateral: 0.0 # [m] + truck: + execute_num: 2 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 0.0 + bus: + execute_num: 2 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 0.0 + trailer: + execute_num: 2 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 0.0 + unknown: + execute_num: 1 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.3 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 0.0 + bicycle: + execute_num: 2 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 1.0 + motorcycle: + execute_num: 2 + moving_speed_threshold: 1.0 # 3.6km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 1.0 + pedestrian: + execute_num: 2 + moving_speed_threshold: 0.28 # 1.0km/h + moving_time_threshold: 1.0 + max_expand_ratio: 0.0 + envelope_buffer_margin: 0.8 + avoid_margin_lateral: 0.0 + safety_buffer_lateral: 1.0 + lower_distance_for_polygon_expansion: 0.0 # [m] + upper_distance_for_polygon_expansion: 1.0 # [m] + + # For target object filtering + target_filtering: + # avoidance target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: true # [-] + bicycle: false # [-] + motorcycle: false # [-] + pedestrian: false # [-] 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 deleted file mode 100644 index a27841f28d..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance_by_lc/avoidance_by_lc.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - avoidance_by_lane_change: - execute_object_num: 1 - execute_object_longitudinal_margin: 0.0 - execute_only_when_lane_change_finish_before_object: true diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index f6c3d3a7ae..8d93a95b37 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: verbose: false + max_iteration_num: 100 - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 + traffic_light_signal_timeout: 1.0 planning_hz: 10.0 backward_path_length: 5.0 @@ -29,18 +29,6 @@ visualize_maximum_drivable_area: true - lateral_distance_max_threshold: 1.7 - longitudinal_distance_min_threshold: 3.0 - - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml deleted file mode 100644 index 4d8f1af9fa..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree.xml +++ /dev/null @@ -1,94 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - desc - - - desc - - - - - - desc - - - desc - - - - desc - - - desc - - - - - - - - desc - - - - - desc - - - desc - - - - - - - - diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml deleted file mode 100644 index 65c600d246..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_lane_change_only.xml +++ /dev/null @@ -1,86 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - desc - - - desc - - - - - - desc - - - desc - - - - desc - - - desc - - - - - - - - desc - - - - - desc - - - desc - - - - - - - - diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml deleted file mode 100644 index 70d6b37cb6..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_pull_out.xml +++ /dev/null @@ -1,100 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - desc - - - desc - - - - - - - - - desc - - - desc - - - - - desc - - - desc - - - - - - - - - - desc - - - - - - desc - - - desc - - - - - - - - - diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_with_external_request_LC.xml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_with_external_request_LC.xml deleted file mode 100644 index 80fe7ea8da..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner_tree_with_external_request_LC.xml +++ /dev/null @@ -1,74 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - desc - - - - - desc - - - - desc - - - - desc - - - - desc - - - - - - - - desc - - - - desc - - - - - diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml index 1609cdbc60..f3393ff5a4 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml @@ -5,33 +5,32 @@ drivable_area_left_bound_offset: 0.0 drivable_area_types_to_skip: [road_border] - # Dynamic expansion by projecting the ego footprint along the path + # Dynamic expansion by using the path curvature dynamic_expansion: - enabled: false + enabled: true + print_runtime: false + max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + smoothing: + curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average + max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length + arc_length_range: 2.0 # [m] arc length range where an expansion distance is initially applied ego: - extra_footprint_offset: - front: 0.5 # [m] extra length to add to the front of the ego footprint - rear: 0.5 # [m] extra length to add to the rear of the ego footprint - left: 0.5 # [m] extra length to add to the left of the ego footprint - right: 0.5 # [m] extra length to add to the rear of the ego footprint + extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase + extra_front_overhang: 0.5 # [m] extra length to add to the front overhang + extra_width: 1.0 # [m] extra length to add to the width dynamic_objects: - avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects + avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: front: 0.5 # [m] extra length to add to the front of the dynamic object footprint rear: 0.5 # [m] extra length to add to the rear of the dynamic object footprint left: 0.5 # [m] extra length to add to the left of the dynamic object footprint right: 0.5 # [m] extra length to add to the rear of the dynamic object footprint - expansion: - method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. - # 'lanelet': add lanelets overlapped by the ego footprints - # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area - max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - max_path_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) - extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + path_preprocessing: + max_arc_length: 100.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) + resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) + reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused. avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border + - curbstone distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid - compensate: - enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction - extra_distance: 3.0 # [m] extra distance to add to the compensation diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml index 39de93dab6..0a563498be 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -1,6 +1,10 @@ /**: ros__parameters: dynamic_avoidance: + common: + enable_debug_info: false + use_hatched_road_markings: true + # avoidance is performed for the object type with true target_object: car: true @@ -12,21 +16,62 @@ motorcycle: true pedestrian: false - min_obstacle_vel: 1.0 # [m/s] + max_obstacle_vel: 100.0 # [m/s] + min_obstacle_vel: 0.0 # [m/s] + + successive_num_to_entry_dynamic_avoidance_condition: 5 + successive_num_to_exit_dynamic_avoidance_condition: 1 + + min_obj_lat_offset_to_ego_path: 0.0 # [m] + max_obj_lat_offset_to_ego_path: 1.0 # [m] + + cut_in_object: + min_time_to_start_cut_in: 1.0 # [s] + min_lon_offset_ego_to_object: 0.0 # [m] + min_object_vel: 0.5 # [m/s] + + cut_out_object: + max_time_from_outside_ego_path: 2.0 # [s] + min_object_lat_vel: 0.3 # [m/s] + min_object_vel: 0.5 # [m/s] + + crossing_object: + min_overtaking_object_vel: 1.0 + max_overtaking_object_angle: 1.05 + min_oncoming_object_vel: 1.0 + max_oncoming_object_angle: 0.523 + + front_object: + max_object_angle: 0.785 + min_object_vel: -0.5 # [m/s] The value is negative considering the noisy velocity of the stopped vehicle. + max_ego_path_lat_cover_ratio: 0.3 # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value. + + stopped_object: + max_object_vel: 0.5 # [m/s] The object will be determined as stopped if the velocity is smaller than this value. drivable_area_generation: - lat_offset_from_obstacle: 0.8 # [m] + polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base" + object_path_base: + min_longitudinal_polygon_margin: 3.0 # [m] + + lat_offset_from_obstacle: 1.0 # [m] max_lat_offset_to_avoid: 0.5 # [m] + max_time_for_object_lat_shift: 0.0 # [s] + lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] + + max_ego_lat_acc: 0.3 # [m/ss] + max_ego_lat_jerk: 0.3 # [m/sss] + delay_time_ego_shift: 1.0 # [s] # for same directional object overtaking_object: - max_time_to_collision: 3.0 # [s] - start_duration_to_avoid: 4.0 # [s] - end_duration_to_avoid: 5.0 # [s] + max_time_to_collision: 40.0 # [s] + start_duration_to_avoid: 1.0 # [s] + end_duration_to_avoid: 1.0 # [s] duration_to_hold_avoidance: 3.0 # [s] # for opposite directional object oncoming_object: - max_time_to_collision: 3.0 # [s] - start_duration_to_avoid: 9.0 # [s] + max_time_to_collision: 40.0 # [s] This value should be the same as overtaking_object's one to suppress chattering of this value for parked vehicles + start_duration_to_avoid: 1.0 # [s] end_duration_to_avoid: 0.0 # [s] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml index 3c5846edf3..eea6d0acdc 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml @@ -2,28 +2,32 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. + center_line_path_interval: 1.0 # goal search goal_search: - search_priority: "efficient_path" # "efficient_path" or "close_goal" + goal_priority: "minimum_weighted_distance" # "minimum_weighted_distance" or "minimum_longitudinal_distance" + minimum_weighted_distance: + lateral_weight: 40.0 + prioritize_goals_before_objects: true parking_policy: "left_side" # "left_side" or "right_side" - forward_goal_search_length: 20.0 + forward_goal_search_length: 40.0 backward_goal_search_length: 20.0 goal_search_interval: 2.0 longitudinal_margin: 3.0 - max_lateral_offset: 0.5 - lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 - margin_from_boundary: 0.5 + max_lateral_offset: 1.0 + lateral_offset_interval: 0.5 + ignore_distance_from_lane_start: 0.0 + margin_from_boundary: 0.75 # occupancy grid map occupancy_grid: - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false + use_occupancy_grid_for_goal_search: true + use_occupancy_grid_for_goal_longitudinal_margin: false + use_occupancy_grid_for_path_collision_check: false occupancy_grid_collision_check_margin: 0.0 theta_size: 360 obstacle_threshold: 60 @@ -31,15 +35,25 @@ # object recognition object_recognition: use_object_recognition: true - object_recognition_collision_check_margin: 1.0 + collision_check_soft_margins: [2.0, 1.5, 1.0] + collision_check_hard_margins: [0.6] # this should be larger than `surround_check_distance` of surround_obstacle_checker + object_recognition_collision_check_max_extra_stopping_margin: 1.0 + th_moving_object_velocity: 1.0 + detection_bound_offset: 15.0 + outer_road_detection_offset: 1.0 + inner_road_detection_offset: 0.0 + # pull over pull_over: + minimum_request_length: 0.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 maximum_deceleration: 1.0 maximum_jerk: 1.0 + path_priority: "efficient_path" # "efficient_path" or "close_goal" + efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking) # shift parking shift_parking: @@ -103,6 +117,75 @@ neighbor_radius: 8.0 margin: 1.0 + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + min_velocity: 1.0 + min_acceleration: 1.0 + max_velocity: 1.0 + time_horizon_for_front_object: 10.0 + time_horizon_for_rear_object: 10.0 + time_resolution: 0.5 + delay_until_departure: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 10.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 100.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 1.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + method: "integral_predicted_polygon" + keep_unsafe_time: 3.0 + # collision check parameters + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + integral_predicted_polygon_params: + forward_margin: 1.0 + backward_margin: 1.0 + lat_margin: 1.0 + time_horizon: 10.0 + # hysteresis factor to expand/shrink polygon with the value + hysteresis_factor_expand_rate: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + # debug debug: print_debug_info: false diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml index fe24eca17d..a25234e100 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml @@ -5,25 +5,66 @@ prepare_duration: 4.0 # [s] backward_length_buffer_for_end_of_lane: 3.0 # [m] + backward_length_buffer_for_blocking_object: 3.0 # [m] lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] - lateral_acc_switching_velocity: 4.0 #[m/s] minimum_lane_changing_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 + # side walk parked vehicle + object_check_min_road_shoulder_width: 0.5 # [m] + object_shiftable_ratio_threshold: 0.6 + + # turn signal + min_length_for_turn_signal_activation: 10.0 # [m] + length_ratio_for_turn_signal_deactivation: 0.8 # ratio (desired end position) + # longitudinal acceleration min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 + # safety check + safety_check: + allow_loose_check_for_cancel: true + execution: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 + stuck: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + + # lane expansion for object filtering + lane_expansion: + left_offset: 1.0 # [m] + right_offset: 1.0 # [m] + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] - min_values: [0.15, 0.15, 0.15] - max_values: [0.5, 0.5, 0.5] + min_values: [0.4, 0.4, 0.4] + max_values: [0.65, 0.65, 0.65] # target object target_object: @@ -31,7 +72,7 @@ truck: true bus: true trailer: true - unknown: true + unknown: false bicycle: true motorcycle: true pedestrian: true @@ -39,16 +80,32 @@ # collision check enable_prepare_segment_collision_check: false prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false + check_objects_on_current_lanes: false + check_objects_on_other_lanes: false + use_all_predicted_path: true + + # lane change regulations + regulation: + crosswalk: true + intersection: true + traffic_light: true + + # ego vehicle stuck detection + stuck_detection: + velocity: 0.5 # [m/s] + stop_time: 3.0 # [s] - # abort - enable_cancel_lane_change: true - enable_abort_lane_change: false + # lane change cancel + cancel: + enable_on_prepare_phase: true + enable_on_lane_changing_phase: true + delta_time: 1.0 # [s] + duration: 5.0 # [s] + max_lateral_jerk: 100.0 # [m/s3] + overhang_tolerance: 0.0 # [m] + unsafe_hysteresis_threshold: 5 # [/] - abort_delta_time: 1.0 # [s] - aborting_time: 5.0 # [s] - abort_max_lateral_jerk: 100.0 # [m/s3] + finish_judge_lateral_threshold: 0.2 # [m] # debug - publish_debug_marker: false + publish_debug_marker: true diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml deleted file mode 100644 index 68000747b7..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml +++ /dev/null @@ -1,31 +0,0 @@ -/**: - ros__parameters: - pull_out: - th_arrived_distance: 1.0 - th_stopped_velocity: 0.01 - th_stopped_time: 1.0 - collision_check_margin: 1.0 - collision_check_distance_from_end: 1.0 - # shift pull out - enable_shift_pull_out: true - shift_pull_out_velocity: 2.0 - pull_out_sampling_num: 4 - minimum_shift_pull_out_distance: 20.0 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.5 - deceleration_interval: 15.0 - # geometric pull out - enable_geometric_pull_out: true - divide_pull_out_path: false - geometric_pull_out_velocity: 1.0 - arc_path_interval: 1.0 - lane_departure_margin: 0.2 - backward_velocity: -1.0 - pull_out_max_steer_angle: 0.26 # 15deg - # search start pose backward - enable_back: true - search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 30.0 - backward_search_resolution: 2.0 - backward_path_update_duration: 3.0 - ignore_distance_from_lane_end: 15.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml new file mode 100644 index 0000000000..9a6b30c219 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/sampling_planner/sampling_planner.param.yaml @@ -0,0 +1,40 @@ +/**: + ros__parameters: + common: + output_delta_arc_length: 0.5 # [m] delta arc length for output trajectory + + debug: + enable_calculation_time_info: false # flag to print calculation times + id: 0 # id of the candidate paths for which to print/show details (e.g., footprint in rviz) + + preprocessing: + force_zero_initial_deviation: True # if true, initial planning starts from the reference path + force_zero_initial_heading: True # if true, initial planning starts with a heading aligned with the reference path + smooth_reference_trajectory: False # if true, the reference trajectory is smoothed before being used for planning + constraints: + hard: + max_curvature: 3.0 # [m⁻¹] maximum curvature of a sampled path + min_curvature: -3.0 # [m⁻¹] minimum curvature of a sampled path + soft: + lateral_deviation_weight: 1.0 # cost weight for lateral deviation between the end of a sampled path and the reference path + length_weight: 1.0 # cost weight for the length of a sampled path + curvature_weight: 2000.0 # cost weight for the curvature of a sampled path + weights: [0.5,1.0,20.0] + sampling: + enable_frenet: True + enable_bezier: False + resolution: 0.5 # [m] target distance between sampled path points + previous_path_reuse_points_nb: 2 # number of points reused from the previously generated path (0:no reuse, 1:replan from end of prev path, 2: end and mid of prev path, etc) + target_lengths: [20.0, 40.0] # [m] target lengths of the sampled paths + nb_target_lateral_positions: 0 # number of lateral positions to use for sampling (in addition to 0.0 and the current lateral deviation) + target_lateral_positions: [-4.5,-2.5, 0.0, 2.5,4.5] # manual values that are only used if nb_target_lateral_positions = 0 + frenet: # target values for the sampled "lateral deviation over longitudinal position" polynomial + target_lateral_velocities: [-0.5, 0.0, 0.5] + target_lateral_accelerations: [0.0] + # bezier: + # nb_k: 3 # number of sampled curvature values + # mk_min: 0.0 # minimum curvature value + # mk_max: 10.0 # maximum curvature value + # nb_t: 5 # number of sampled acceleration values + # mt_min: 0.3 # minimum acceleration value + # mt_max: 1.7 # maximum acceleration value diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml index 76ae718c01..8d4fe057b4 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/scene_module_manager.param.yaml @@ -4,72 +4,90 @@ /**: ros__parameters: external_request_lane_change_left: - enable_module: false + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true - priority: 7 + keep_last: false + priority: 6 max_module_size: 1 external_request_lane_change_right: - enable_module: false + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true - priority: 7 + keep_last: false + priority: 6 max_module_size: 1 lane_change_left: - enable_module: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - priority: 6 + keep_last: false + priority: 5 max_module_size: 1 lane_change_right: - enable_module: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - priority: 6 + keep_last: false + priority: 5 max_module_size: 1 - pull_out: - enable_module: true + start_planner: + enable_rtc: false 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_module: true + enable_rtc: false 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_module: true - enable_simultaneous_execution_as_approved_module: false + enable_rtc: false + enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false + keep_last: true priority: 1 max_module_size: 1 avoidance: - enable_module: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false - priority: 5 + keep_last: false + priority: 4 max_module_size: 1 - # NOTE: This module is unstable. Deprecated for now. - avoidance_by_lc: - enable_module: false + avoidance_by_lane_change: + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false - priority: 4 + keep_last: false + priority: 3 max_module_size: 1 dynamic_avoidance: - enable_module: false + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true - priority: 3 + keep_last: true + priority: 7 + max_module_size: 1 + + sampling_planner: + enable_module: true + enable_rtc: false + enable_simultaneous_execution_as_approved_module: false + enable_simultaneous_execution_as_candidate_module: false + keep_last: false + priority: 16 max_module_size: 1 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml new file mode 100644 index 0000000000..2c55e9fad5 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/start_planner/start_planner.param.yaml @@ -0,0 +1,159 @@ +/**: + ros__parameters: + start_planner: + + th_arrived_distance: 1.0 + th_stopped_velocity: 0.01 + th_stopped_time: 1.0 + collision_check_margins: [2.0, 1.0, 0.5, 0.1] + collision_check_margin_from_front_object: 5.0 + th_moving_object_velocity: 1.0 + th_distance_to_middle_of_the_road: 0.5 + center_line_path_interval: 1.0 + # shift pull out + enable_shift_pull_out: true + check_shift_path_lane_departure: true + shift_collision_check_distance_from_end: -10.0 + minimum_shift_pull_out_distance: 0.0 + deceleration_interval: 15.0 + lateral_jerk: 0.5 + lateral_acceleration_sampling_num: 3 + minimum_lateral_acc: 0.15 + maximum_lateral_acc: 0.5 + maximum_curvature: 0.07 + # geometric pull out + enable_geometric_pull_out: true + geometric_collision_check_distance_from_end: 0.0 + divide_pull_out_path: true + geometric_pull_out_velocity: 1.0 + arc_path_interval: 1.0 + lane_departure_margin: 0.2 + backward_velocity: -1.0 + pull_out_max_steer_angle: 0.26 # 15deg + # search start pose backward + enable_back: true + search_priority: "efficient_path" # "efficient_path" or "short_back_distance" + max_back_distance: 20.0 + backward_search_resolution: 2.0 + backward_path_update_duration: 3.0 + ignore_distance_from_lane_end: 15.0 + # turns signal + prepare_time_before_start: 0.0 + th_turn_signal_on_lateral_offset: 1.0 + intersection_search_length: 30.0 + length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 + # freespace planner + freespace_planner: + enable_freespace_planner: true + end_pose_search_start_distance: 20.0 + end_pose_search_end_distance: 30.0 + end_pose_search_interval: 2.0 + freespace_planner_algorithm: "astar" # options: astar, rrtstar + velocity: 1.0 + vehicle_shape_margin: 1.0 + time_limit: 3000.0 + minimum_turning_radius: 5.0 + maximum_turning_radius: 5.0 + turning_radius_size: 1 + # search configs + search_configs: + theta_size: 144 + angle_goal_range: 6.0 + curve_weight: 1.2 + reverse_weight: 1.0 + lateral_goal_range: 0.5 + longitudinal_goal_range: 2.0 + # costmap configs + costmap_configs: + obstacle_threshold: 30 + # -- A* search Configurations -- + astar: + only_behind_solutions: false + use_back: false + distance_heuristic_weight: 1.0 + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 1.0 + + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + min_velocity: 0.0 + min_acceleration: 1.0 + time_horizon_for_front_object: 10.0 + time_horizon_for_rear_object: 10.0 + time_resolution: 0.5 + delay_until_departure: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 10.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 1.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + # hysteresis factor to expand/shrink polygon + hysteresis_factor_expand_rate: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + + surround_moving_obstacle_check: + search_radius: 10.0 + th_moving_obstacle_velocity: 1.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + + # debug + debug: + print_debug_info: false 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 6f1341cb02..b31506918a 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 @@ -1,19 +1,8 @@ /**: ros__parameters: - launch_stop_line: true - launch_crosswalk: true - launch_traffic_light: true - launch_intersection: true - launch_blind_spot: true - launch_detection_area: true - launch_virtual_traffic_light: false # disabled by default to not confuse newcomers - launch_occlusion_spot: false - launch_no_stopping_area: true - launch_run_out: false - launch_speed_bump: false - launch_out_of_lane: true forward_path_length: 1000.0 backward_path_length: 5.0 + behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 max_accel: -2.8 max_jerk: -5.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml index 7963766c03..424029300a 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml @@ -4,7 +4,9 @@ use_pass_judge_line: true stop_line_margin: 1.0 # [m] backward_length: 50.0 # [m] - ignore_width_from_center_line: 0.7 # [m] + ignore_width_from_center_line: 0.0 # [m] max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] + opposite_adjacent_extend_width: 1.5 # [m] + enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. 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 2bbc5d31fc..87140169b4 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 @@ -1,44 +1,87 @@ /**: ros__parameters: crosswalk: - show_processing_time: false # [-] whether to show processing time + common: + show_processing_time: false # [-] whether to show processing time + # param for input data + traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal + enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists - stop_line_margin: 10.0 # [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_margin meters before the object) - stop_margin: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin - stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk - - # param for ego velocity - min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) - max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake - max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake - no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) - - # param for stuck vehicle - stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck - max_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked - stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk - - # param for pass judge logic - ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) - 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) - 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) - max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. - ego_yield_query_stop_duration: 0.1 # [s] the amount of time which ego should be stopping to query whether it yields or not - - # param for input data - tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal - - # param for target area & object - crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk - target_object: - unknown: true # [-] whether to look and stop by UNKNOWN objects - bicycle: true # [-] whether to look and stop by BICYCLE objects - motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) - pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects - - walkway: - stop_duration_sec: 1.0 # [s] stop time at stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists + stop_position: + stop_position_threshold: 1.0 # [m] If the ego vehicle has stopped near the stop line than this value, this module assumes itself to have achieved yielding. + + # For the Lanelet2 map with no explicit stop lines + stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk + # For the case where the crosswalk width is very wide + far_object_threshold: 10.0 # [m] If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object). + # For the case where the stop position is determined according to the object position. + stop_distance_from_object: 3.0 # [m] the vehicle decelerates to be able to stop in front of object with margin + + # params for ego's slow down velocity. These params are not used for the case of "enable_rtc: false". + slow_down: + min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) + max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake + max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake + no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) + + # params to prevent stopping on crosswalks due to another vehicle ahead + stuck_vehicle: + enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection + stuck_vehicle_velocity: 1.0 # [m/s] threshold velocity whether other vehicles are assumed to be stuck or not. + max_stuck_vehicle_lateral_offset: 2.0 # [m] The feature does not handle the vehicles farther than this value to the ego's path. + required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + # params for the pass judge logic against the crosswalk users such as pedestrians or bicycles + pass_judge: + ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [3.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering + ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_margin_y: [13.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering + + no_stop_decision: # parameters to determine if it is safe to attempt stopping before the crosswalk + max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + + 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_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_set_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order + timeout_set_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: + crosswalk_attention_range: 2.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk + vehicle_object_cross_angle_threshold: 0.5 # [rad] threshold judging object is crossing walkway as a pedestrian or passing over as a vehicle + target_object: + unknown: true # [-] whether to look and stop by UNKNOWN objects + bicycle: true # [-] whether to look and stop by BICYCLE objects + motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) + pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects + + # param for occlusions + occlusion: + enable: true # if true, ego will slowdown around crosswalks that are occluded + occluded_object_velocity: 1.0 # [m/s] assumed velocity of objects that may come out of the occluded space + slow_down_velocity: 1.0 # [m/s] + time_buffer: 0.5 # [s] consecutive time with/without an occlusion to add/remove the slowdown + min_size: 0.5 # [m] minimum size of an occlusion (square side size) + free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid + occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid + ignore_with_red_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored + ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored + ignore_velocity_thresholds: + default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity + custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels) + custom_thresholds: [0.0] # velocities of the custom labels + extra_predicted_objects_size: 0.5 # [m] extra size added to the objects for masking the occlusions diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml index ad4e32cb4e..ddd8b934d2 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml @@ -8,3 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 + enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/dynamic_obstacle_stop.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/dynamic_obstacle_stop.param.yaml new file mode 100644 index 0000000000..14483093e8 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/dynamic_obstacle_stop.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + dynamic_obstacle_stop: # module to stop or before entering the immediate path of a moving object + extra_object_width: 1.0 # [m] extra width around detected objects + minimum_object_velocity: 0.5 # [m/s] objects with a velocity bellow this value are ignored + stop_distance_buffer: 0.5 # [m] extra distance to add between the stop point and the collision point + time_horizon: 5.0 # [s] time horizon used for collision checks + hysteresis: 1.0 # [m] once a collision has been detected, this hysteresis is used on the collision detection + decision_duration_buffer : 1.0 # [s] duration between no collision being detected and the stop decision being cancelled + minimum_object_distance_from_ego_path: 1.0 # [m] minimum distance between the footprints of ego and an object to consider for collision 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 0f176f1eaf..68d4070cbf 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 @@ -2,47 +2,121 @@ ros__parameters: intersection: common: - detection_area_margin: 0.5 # [m] - detection_area_right_margin: 0.5 # [m] - detection_area_left_margin: 0.5 # [m] - detection_area_length: 200.0 # [m] - detection_area_angle_threshold: 0.785 # [rad] - stop_line_margin: 3.0 - intersection_velocity: 2.778 # 2.778m/s = 10.0km/h - intersection_max_accel: 0.5 # m/ss - stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection + attention_area_length: 200.0 + attention_area_margin: 0.75 + attention_area_angle_threshold: 0.785 use_intersection_area: false + default_stopline_margin: 3.0 + stopline_overshoot_margin: 0.5 + path_interpolation_ds: 0.1 + max_accel: -2.8 + max_jerk: -5.0 + delay_response_time: 0.5 + enable_pass_judge_before_default_stopline: false stuck_vehicle: - use_stuck_stopline: false # stopline generate before the intersection lanelet when is_stuck - stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) - stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h - stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - enable_front_car_decel_prediction: false # By default this feature is disabled - assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning + target_type: + car: true + bus: true + truck: true + trailer: true + motorcycle: false + bicycle: false + unknown: false + turn_direction: + left: true + right: true + straight: true + use_stuck_stopline: true + stuck_vehicle_detect_dist: 5.0 + stuck_vehicle_velocity_threshold: 0.833 + # enable_front_car_decel_prediction: false + # assumed_front_car_decel: 1.0 + disable_against_private_lane: true + + yield_stuck: + target_type: + car: true + bus: true + truck: true + trailer: true + motorcycle: false + bicycle: false + unknown: false + turn_direction: + left: true + right: true + straight: false + distance_threshold: 5.0 collision_detection: - state_transit_margin_time: 1.0 + consider_wrong_direction_vehicle: false + collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 - minimum_ego_predicted_velocity: 1.388 # [m/s] - collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object - collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object - keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr + target_type: + car: true + bus: true + truck: true + trailer: true + motorcycle: true + bicycle: true + unknown: false + velocity_profile: + use_upstream: true + minimum_upstream_velocity: 0.01 + default_velocity: 2.778 + minimum_default_velocity: 1.388 + fully_prioritized: + collision_start_margin_time: 2.0 + collision_end_margin_time: 0.0 + partially_prioritized: + collision_start_margin_time: 3.0 + collision_end_margin_time: 2.0 + not_prioritized: + collision_start_margin_time: 3.0 + collision_end_margin_time: 2.0 + yield_on_green_traffic_light: + distance_to_assigned_lanelet_start: 10.0 + duration: 3.0 + object_dist_to_stopline: 10.0 + ignore_on_amber_traffic_light: + object_expected_deceleration: + car: 2.0 + bike: 5.0 + ignore_on_red_traffic_light: + object_margin_to_path: 2.0 + avoid_collision_by_acceleration: + object_time_margin_to_collision_point: 4.0 occlusion: enable: false - occlusion_detection_area_length: 70.0 # [m] - enable_creeping: false # flag to use the creep velocity when reaching occlusion limit stop line - occlusion_creep_velocity: 0.8333 # the creep velocity to occlusion limit stop line - peeking_offset: -0.5 # [m] offset for peeking into detection area + occlusion_attention_area_length: 70.0 free_space_max: 43 occupied_min: 58 - do_dp: true - before_creep_stop_time: 0.1 # [s] - min_vehicle_brake_for_rss: -2.5 # [m/s^2] - max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph - denoise_kernel: 1.0 # [m] - pub_debug_grid: false + denoise_kernel: 1.0 + attention_lane_crop_curvature_threshold: 0.25 + attention_lane_curvature_calculation_ds: 0.6 + creep_during_peeking: + enable: false + creep_velocity: 0.8333 + peeking_offset: -0.5 + occlusion_required_clearance_distance: 55.0 + possible_object_bbox: [1.5, 2.5] + ignore_parked_vehicle_speed_threshold: 0.8333 + occlusion_detection_hold_time: 1.5 + temporal_stop_time_before_peeking: 0.1 + temporal_stop_before_attention_area: false + creep_velocity_without_traffic_light: 1.388 + static_occlusion_with_traffic_light_timeout: 0.5 + + debug: + ttc: [0] + + enable_rtc: + intersection: false + intersection_to_occlusion: false merge_from_private: + stopline_margin: 3.0 stop_duration_sec: 1.0 + stop_distance_threshold: 1.0 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_drivable_lane.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_drivable_lane.param.yaml new file mode 100644 index 0000000000..d75acfc0b6 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_drivable_lane.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + no_drivable_lane: + stop_margin: 1.5 # Stop margin before the no drivable lane [m] + print_debug_info: false diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml index 32cd05a9cc..c84848f8cc 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml @@ -8,3 +8,4 @@ stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area detection_area_length: 200.0 # [m] used to create detection area polygon stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml index 0b93ea5308..089180aa66 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml @@ -13,9 +13,12 @@ is_show_cv_window: false # [-] whether to show open_cv debug window is_show_processing_time: false # [-] whether to show processing time threshold: + detection_area_offset: 30.0 detection_area_length: 100.0 # [m] the length of path to consider perception range stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision + search_dist: 10.0 + search_angle: 0.63 # [rad] PI/5.0 motion: safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety max_slow_down_jerk: -0.3 # [m/s^3] minimum jerk deceleration for safe brake. diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml index c501599b4a..b55c3b21de 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/out_of_lane.param.yaml @@ -2,7 +2,7 @@ ros__parameters: out_of_lane: # module to stop or slowdown before overlapping another lane with other objects mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" - skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane + skip_if_already_overlapping: false # do not run this module when ego already overlaps another lane threshold: time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time @@ -16,6 +16,8 @@ minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored use_predicted_paths: true # if true, use the predicted paths to estimate future positions. # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. + predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. + distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered @@ -23,9 +25,9 @@ action: # action to insert in the path if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed - strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego - # if false, ego stops just before entering a lane but may then be overlapping another lane. + precision: 0.1 # [m] precision when inserting a stop pose in the path distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap velocity: 2.0 # [m/s] slowdown velocity @@ -33,6 +35,7 @@ distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap ego: + min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego extra_front_offset: 0.0 # [m] extra front distance extra_rear_offset: 0.0 # [m] extra rear distance extra_right_offset: 0.0 # [m] extra right distance diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml index f9668549f2..196f7c6296 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -3,7 +3,8 @@ run_out: detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points use_partition_lanelet: true # [-] whether to use partition lanelet map data - specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates + suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet + specify_decel_jerk: true # [-] whether to specify jerk when ego decelerates stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles @@ -11,40 +12,56 @@ detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision - detection_area: - margin_behind: 0.5 # [m] ahead margin for detection area length - margin_ahead: 1.0 # [m] behind margin for detection area length + # Parameter to create abstracted dynamic obstacles + dynamic_obstacle: + use_mandatory_area: false # [-] whether to use mandatory detection area + assume_fixed_velocity: + enable: false # [-] If enabled, the obstacle's velocity is assumed to be within the minimum and maximum velocity values specified below + min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles + max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles + std_dev_multiplier: 1.96 # [-] min and max velocity of the obstacles are calculated from this value and covariance + diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points + height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points + max_prediction_time: 10.0 # [sec] create predicted path until this time + time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path + points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method - # This area uses points not filtered by vector map to ensure safety - mandatory_area: - decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area + # Parameter to prevent sudden stopping. + # If the deceleration jerk and acceleration exceed this value upon inserting a stop point, + # the deceleration will be moderated to stay under this value. + slow_down_limit: + enable: false + max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. + max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. - # parameter to create abstracted dynamic obstacles - dynamic_obstacle: - use_mandatory_area: false # [-] whether to use mandatory detection area - min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles - max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles - diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points - height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points - max_prediction_time: 10.0 # [sec] create predicted path until this time - time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path - points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method + # Parameter to prevent abrupt stops caused by false positives in perception + ignore_momentary_detection: + enable: true + time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration - # approach if ego has stopped in the front of the obstacle for a certain amount of time + # Typically used when the "detection_method" is set to ObjectWithoutPath or Points + # Approach if the ego has stopped in front of the obstacle for a certain period + # This avoids the issue of the ego continuously stopping in front of the obstacle approaching: enable: false margin: 0.0 # [m] distance on how close ego approaches the obstacle limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping + # Parameters for state change when "approaching" is enabled + state: + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value + keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition - # parameters for the change of state. used only when approaching is enabled - state: - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value - keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition + # Only used when "detection_method" is set to Points + # Filters points by the detection area polygon to reduce computational cost + # The detection area is calculated based on the current velocity and acceleration and deceleration jerk constraints + detection_area: + margin_behind: 0.5 # [m] ahead margin for detection area length + margin_ahead: 1.0 # [m] behind margin for detection area length - # parameter to avoid sudden stopping - slow_down_limit: - enable: true - max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. - max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. + # Only used when "detection_method" is set to Points + # Points in this area are detected even if it is in the no obstacle segmentation area + # The mandatory area is calculated based on the current velocity and acceleration and "decel_jerk" constraints + mandatory_area: + decel_jerk: -1.2 # [m/s^3] deceleration jerk for obstacles in this area 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 69e9241ba1..5fa183d73a 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 @@ -2,10 +2,9 @@ ros__parameters: stop_line: stop_margin: 0.0 - stop_check_dist: 2.0 stop_duration_sec: 1.0 use_initialization_stop_line_state: true hold_stop_margin_distance: 2.0 debug: - show_stopline_collision_check: false + show_stop_line_collision_check: false 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 444fa5ca65..23746a61b6 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 @@ -3,5 +3,7 @@ traffic_light: stop_margin: 0.0 tl_state_timeout: 1.0 + stop_time_hysteresis: 0.1 yellow_lamp_period: 2.75 enable_pass_judge: true + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval 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 new file mode 100644 index 0000000000..07f493edcd --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/walkway.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + walkway: + 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_auto_mode_manager/rtc_auto_mode_manager.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml similarity index 76% rename from autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml rename to autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml index af82a3461f..9d761b15ee 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/rtc_replayer/rtc_replayer.param.yaml @@ -7,14 +7,14 @@ - "intersection" - "no_stopping_area" - "traffic_light" - - "external_request_lane_change_left" - - "external_request_lane_change_right" - "lane_change_left" - "lane_change_right" - "avoidance_left" - "avoidance_right" + - "avoidance_by_lane_change_left" + - "avoidance_by_lane_change_right" - "goal_planner" - - "pull_out" + - "start_planner" - "intersection_occlusion" default_enable_list: @@ -28,6 +28,8 @@ - "lane_change_right" - "avoidance_left" - "avoidance_right" + - "avoidance_by_lane_change_left" + - "avoidance_by_lane_change_right" - "goal_planner" - - "pull_out" + - "start_planner" - "intersection_occlusion" diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml index 3f4655b782..db89a81e47 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: option: - enable_smoothing: true # enable path smoothing by elastic band enable_skip_optimization: false # skip elastic band and model predictive trajectory enable_reset_prev_optimization: false # If true, optimization has no fix constraint to the previous result. enable_outside_drivable_area_stop: true # stop if the ego's trajectory footprint is outside the drivable area @@ -9,7 +8,8 @@ debug: # flag to publish - enable_pub_debug_marker: true # publish debug marker + enable_pub_debug_marker: false # publish debug marker + enable_pub_extra_debug_marker: false # publish extra debug marker # flag to show enable_debug_info: false @@ -20,6 +20,9 @@ output_delta_arc_length: 0.5 # delta arc length for output trajectory [m] output_backward_traj_length: 5.0 # backward length for backward trajectory from base_link [m] + vehicle_stop_margin_outside_drivable_area: 0.0 # vehicle stop margin to let the vehicle stop before the calculated stop point if it is calculated outside the drivable area [m] . + # This margin will be realized with delta_arc_length_for_mpt_points m precision. + # replanning & trimming trajectory param outside algorithm replan: max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] @@ -28,36 +31,7 @@ max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] # make max_goal_moving_dist long to keep start point fixed for pull over max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] - max_delta_time_sec: 1.0 # threshold of delta time for replan [second] - - # eb param - eb: - option: - enable_warm_start: true - enable_optimization_validation: false - - common: - num_points: 100 # number of points for optimization [-] - delta_arc_length: 1.0 # delta arc length for optimization [m] - - clearance: - num_joint_points: 3 # number of joint points (joint means connecting fixing and smoothing) - - clearance_for_fix: 0.0 # maximum optimizing range when applying fixing - clearance_for_joint: 0.1 # maximum optimizing range when applying jointing - clearance_for_smooth: 0.1 # maximum optimizing range when applying smoothing - - weight: - smooth_weight: 1.0 - lat_error_weight: 0.001 - - qp: - max_iteration: 10000 # max iteration when solving QP - eps_abs: 1.0e-7 # eps abs when solving OSQP - eps_rel: 1.0e-7 # eps rel when solving OSQP - - validation: # if enable_optimization_validation is true - max_error: 3.0 # [m] + max_delta_time_sec: 0.0 # threshold of delta time for replan [second] # mpt param mpt: @@ -66,7 +40,7 @@ steer_limit_constraint: false visualize_sampling_num: 1 enable_manual_warm_start: false - enable_warm_start: true + enable_warm_start: false enable_optimization_validation: false common: @@ -88,7 +62,7 @@ # weight parameter for optimization weight: # collision free - soft_collision_free_weight: 1000.0 # soft weight for lateral error around the middle point + soft_collision_free_weight: 1.0 # soft weight for lateral error around the middle point # tracking error lat_error_weight: 1.0 # weight for lateral error @@ -104,7 +78,7 @@ # avoidance avoidance: - max_bound_fixing_time: 3.0 # [s] + max_bound_fixing_time: 1.0 # [s] max_longitudinal_margin_for_bound_violation: 1.0 # [m] max_avoidance_cost: 0.5 # [m] avoidance_cost_margin: 0.0 # [m] 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 e7a9a7fb27..2ff87e9b01 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 @@ -14,10 +14,17 @@ min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] + hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] + stop_on_curve: + enable_approaching: false + additional_safe_distance_margin: 3.0 # [m] + min_safe_distance_margin: 3.0 # [m] + suppress_sudden_obstacle_stop: true stop_obstacle_type: unknown: true @@ -51,7 +58,7 @@ pedestrian: false slow_down_obstacle_type: - unknown: true + unknown: false car: true truck: true bus: true @@ -63,9 +70,6 @@ behavior_determination: decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking - goal_extension_length: 5.0 # extension length for collision check around the goal - goal_extension_interval: 1.0 # extension interval for collision check around the goal - prediction_resampling_time_interval: 0.1 prediction_resampling_time_horizon: 10.0 @@ -81,21 +85,36 @@ 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 - + yield: + enable_yield: false + lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + max_obstacles_collision_time: 10.0 # how far the blocking obstacle + stopped_obstacle_velocity_threshold: 0.5 slow_down: max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 + successive_num_to_entry_slow_down_condition: 5 + successive_num_to_exit_slow_down_condition: 5 + + # consider the current ego pose (it is not the nearest pose on the reference trajectory) + # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" + # The both errors decrease with constant rates against the time. + consider_current_pose: + enable_to_consider_current_pose: true + time_to_convergence: 1.5 #[s] + cruise: pid_based_planner: use_velocity_limit_based_planner: true @@ -162,10 +181,22 @@ slow_down: # parameters to calculate slow down velocity by linear interpolation - min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 8.0 + labels: + - "default" + default: + moving: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + static: + min_lat_margin: 0.2 + max_lat_margin: 1.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] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml index c804cea577..8e215a1bcf 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml @@ -17,7 +17,7 @@ # params for stop position stop_position: max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] - max_longitudinal_margin_behind_goal: 3.0 # stop margin distance from obstacle behind goal on the path [m] + max_longitudinal_margin_behind_goal: 2.5 # stop margin distance from obstacle behind goal on the path [m] min_longitudinal_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_sampler/path_sampler.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_sampler/path_sampler.param.yaml index b50602b8e6..d22ce0e6b4 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_sampler/path_sampler.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_sampler/path_sampler.param.yaml @@ -16,7 +16,7 @@ max_curvature: 0.1 # [m⁻¹] maximum curvature of a sampled path min_curvature: -0.1 # [m⁻¹] minimum curvature of a sampled path soft: - lateral_deviation_weight: 0.1 # cost weight for lateral deviation between the end of a sampled path and the reference path + lateral_deviation_weight: 1.0 # cost weight for lateral deviation between the end of a sampled path and the reference path length_weight: 1.0 # cost weight for the length of a sampled path curvature_weight: 1.0 # cost weight for the curvature of a sampled path sampling: diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml new file mode 100644 index 0000000000..a9e71368e3 --- /dev/null +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml @@ -0,0 +1,47 @@ +/**: + ros__parameters: + common: + # output + output_delta_arc_length: 0.5 # delta arc length for output trajectory [m] + output_backward_traj_length: 5.0 # backward length for backward trajectory from base_link [m] + # elastic band + elastic_band: + option: + enable_warm_start: true + enable_optimization_validation: false + + common: + num_points: 100 # number of points for optimization [-] + delta_arc_length: 1.0 # delta arc length for optimization [m] + + clearance: + num_joint_points: 3 # number of joint points (joint means connecting fixing and smoothing) + + clearance_for_fix: 0.0 # maximum optimizing range when applying fixing + clearance_for_joint: 0.1 # maximum optimizing range when applying jointing + clearance_for_smooth: 0.1 # maximum optimizing range when applying smoothing + + weight: + smooth_weight: 1.0 + lat_error_weight: 0.001 + + qp: + max_iteration: 10000 # max iteration when solving QP + eps_abs: 1.0e-7 # eps abs when solving OSQP + eps_rel: 1.0e-7 # eps rel when solving OSQP + + validation: # if enable_optimization_validation is true + max_error: 3.0 # [m] + # nearest search + ego_nearest_dist_threshold: 3.0 # [m] + ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] + # replanning & trimming trajectory param outside algorithm + replan: + enable: true # if true, only perform smoothing when the input changes significantly + max_path_shape_around_ego_lat_dist: 2.0 # threshold of path shape change around ego [m] + max_path_shape_forward_lon_dist: 100.0 # forward point to check lateral distance difference [m] + max_path_shape_forward_lat_dist: 0.2 # threshold of path shape change around forward point [m] + max_ego_moving_dist: 5.0 # threshold of ego's moving distance for replan [m] + # make max_goal_moving_dist long to keep start point fixed for pull over + max_goal_moving_dist: 15.0 # threshold of goal's moving distance for replan [m] + max_delta_time_sec: 0.0 # threshold of delta time for replan [second] 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 6aa4e71774..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 @@ -1,15 +1,61 @@ /**: ros__parameters: - # obstacle check - use_pointcloud: true # use pointcloud as obstacle check - use_dynamic_object: true # use dynamic object as obstacle check - surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] - surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] - state_clear_time: 2.0 + # surround_check_*_distance: if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] + # surround_check_hysteresis_distance: if no object exists in this hysteresis distance added to the above distance, transit to "non-surrounding-obstacle" status [m] + pointcloud: + enable_check: false + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + unknown: + enable_check: false + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + car: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + 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.0 + bus: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + 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.0 + motorcycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.0 + surround_check_back_distance: 0.5 + bicycle: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + pedestrian: + enable_check: true + surround_check_front_distance: 0.5 + surround_check_side_distance: 0.5 + surround_check_back_distance: 0.5 + + surround_check_hysteresis_distance: 0.1 + + state_clear_time: 0.2 # ego stop state stop_state_ego_speed: 0.1 #[m/s] # debug publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets + debug_footprint_label: "car" diff --git a/autoware_launch/config/planning/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml index d6152c4f56..ffd99df533 100644 --- a/autoware_launch/config/planning/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml @@ -37,3 +37,11 @@ only_behind_solutions: false use_back: true distance_heuristic_weight: 1.0 + + # -- RRT* search Configurations -- + rrtstar: + enable_update: true + use_informed_sampling: true + max_planning_time: 150.0 + neighbor_radius: 8.0 + margin: 0.1 diff --git a/autoware_launch/config/simulator/fault_injection.param.yaml b/autoware_launch/config/simulator/fault_injection.param.yaml index 1a57b852f7..ac02442d70 100644 --- a/autoware_launch/config/simulator/fault_injection.param.yaml +++ b/autoware_launch/config/simulator/fault_injection.param.yaml @@ -4,7 +4,7 @@ vehicle_is_out_of_lane: "lane_departure" trajectory_deviation_is_high: "trajectory_deviation" localization_matching_score_is_low: "ndt_scan_matcher" - localization_accuracy_is_low: "localization_accuracy" + localization_accuracy_is_low: "localization_error_ellipse" map_version_is_different: "map_version" trajectory_is_invalid: "trajectory_point_validation" cpu_temperature_is_high: "CPU Temperature" diff --git a/autoware_launch/config/system/component_state_monitor/topics.yaml b/autoware_launch/config/system/component_state_monitor/topics.yaml index c00203dbb6..f8c6f9685f 100644 --- a/autoware_launch/config/system/component_state_monitor/topics.yaml +++ b/autoware_launch/config/system/component_state_monitor/topics.yaml @@ -50,6 +50,19 @@ error_rate: 1.0 timeout: 1.0 +- module: localization + mode: [online, logging_simulation] + type: autonomous + args: + node_name_suffix: pose_estimator_pose + topic: /localization/pose_estimator/pose_with_covariance + topic_type: geometry_msgs/msg/PoseWithCovarianceStamped + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + - module: perception mode: [online, logging_simulation] type: launch @@ -76,6 +89,19 @@ error_rate: 1.0 timeout: 1.0 +- module: perception + mode: [online, logging_simulation] + type: autonomous + args: + node_name_suffix: traffic_light_recognition_traffic_signals + topic: /perception/traffic_light_recognition/traffic_signals + topic_type: autoware_perception_msgs/msg/TrafficSignalArray + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + - module: planning mode: [online, logging_simulation, planning_simulation] type: autonomous diff --git a/autoware_launch/config/system/duplicated_node_checker/duplicated_node_checker.param.yaml b/autoware_launch/config/system/duplicated_node_checker/duplicated_node_checker.param.yaml new file mode 100644 index 0000000000..54b4f691b6 --- /dev/null +++ b/autoware_launch/config/system/duplicated_node_checker/duplicated_node_checker.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + update_rate: 10.0 + add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg 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 new file mode 100644 index 0000000000..a3c712d466 --- /dev/null +++ b/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml @@ -0,0 +1,54 @@ +# Description: +# name: diag name +# sf_at: diag level where it becomes Safe Fault +# lf_at: diag level where it becomes Latent Fault +# spf_at: diag level where it becomes Single Point Fault +# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. +# +# Note: +# empty-value for sf_at, lf_at and spf_at is "none" +# default values are: +# sf_at: "none" +# lf_at: "warn" +# spf_at: "error" +# auto_recovery: "true" +--- +/**: + ros__parameters: + required_modules: + autonomous_driving: + /autoware/control/autonomous_driving/node_alive_monitoring: default + /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default + /autoware/control/control_command_gate/node_alive_monitoring: 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_error_ellipse: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/localization/performance_monitoring/localization_stability: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/localization/performance_monitoring/sensor_fusion_status: { sf_at: "warn", lf_at: "none", spf_at: "none" } + + /autoware/map/node_alive_monitoring: default + + /autoware/perception/node_alive_monitoring: default + + /autoware/planning/node_alive_monitoring: default + /autoware/planning/performance_monitoring/trajectory_validation: default + + # /autoware/sensing/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/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } + + /autoware/vehicle/node_alive_monitoring: default + + 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 71dc2ac600..2826e9348c 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 @@ -23,7 +23,9 @@ /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_accuracy: default + /autoware/localization/performance_monitoring/localization_error_ellipse: default + /autoware/localization/performance_monitoring/localization_stability: default + /autoware/localization/performance_monitoring/sensor_fusion_status: { sf_at: "error", lf_at: "none", spf_at: "none" } /autoware/map/node_alive_monitoring: default @@ -37,7 +39,8 @@ /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: "error", spf_at: "none" } + /autoware/system/duplicated_node_checker: default /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 9708456df4..deddf33b34 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 @@ -19,11 +19,12 @@ 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/trajectory_deviation: default /autoware/control/control_command_gate/node_alive_monitoring: 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_accuracy: default + # /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default @@ -38,6 +39,7 @@ /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 diff --git a/autoware_launch/config/vehicle/raw_vehicle_cmd_converter/raw_vehicle_cmd_converter.param.yaml b/autoware_launch/config/vehicle/raw_vehicle_cmd_converter/raw_vehicle_cmd_converter.param.yaml new file mode 100644 index 0000000000..f6676bdbe7 --- /dev/null +++ b/autoware_launch/config/vehicle/raw_vehicle_cmd_converter/raw_vehicle_cmd_converter.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + convert_accel_cmd: true + convert_brake_cmd: true + convert_steer_cmd: false + use_steer_ff: true + use_steer_fb: true + is_debugging: false + max_throttle: 0.4 + max_brake: 0.8 + max_steer: 10.0 + min_steer: -10.0 + steer_pid: + kp: 150.0 + ki: 15.0 + kd: 0.0 + max: 8.0 + min: -8.0 + max_p: 8.0 + min_p: -8.0 + max_i: 8.0 + min_i: -8.0 + max_d: 0.0 + min_d: 0.0 + invalid_integration_decay: 0.97 diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 4a57cd1cb7..a5daf3dd16 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -2,10 +2,13 @@ - - - + + + + + + @@ -32,12 +35,16 @@ + + + + @@ -48,12 +55,10 @@ - - - - - - + + + + @@ -63,6 +68,7 @@ + @@ -84,17 +90,21 @@ - - + + + - + + + + @@ -109,6 +119,14 @@ - + diff --git a/autoware_launch/launch/components/map4_localization_component.launch.xml b/autoware_launch/launch/components/map4_localization_component.launch.xml deleted file mode 100644 index 4fd0fdf8bd..0000000000 --- a/autoware_launch/launch/components/map4_localization_component.launch.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/autoware_launch/launch/components/tier4_control_component.launch.xml b/autoware_launch/launch/components/tier4_control_component.launch.xml index 7b3169eacd..d15e050d7a 100644 --- a/autoware_launch/launch/components/tier4_control_component.launch.xml +++ b/autoware_launch/launch/components/tier4_control_component.launch.xml @@ -5,6 +5,7 @@ + @@ -35,11 +36,14 @@ + + + diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index 560fc8f557..3ccd98104d 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -1,23 +1,60 @@ - - + + + + - - - + + + + + + + - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/autoware_launch/launch/components/tier4_map_component.launch.xml b/autoware_launch/launch/components/tier4_map_component.launch.xml index 3062a6cedd..ba5611b6c0 100644 --- a/autoware_launch/launch/components/tier4_map_component.launch.xml +++ b/autoware_launch/launch/components/tier4_map_component.launch.xml @@ -1,10 +1,14 @@ - - + + + + + + diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index c0c2e4fc4c..3b0f8eca46 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -1,30 +1,142 @@ + + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml index 6db13f5259..3d57f6198e 100644 --- a/autoware_launch/launch/components/tier4_planning_component.launch.xml +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -1,142 +1,78 @@ - - - - - - + + + - - + - - - - - + + + + - - - - - - - - - - - + + + + + + + + + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - - - - - - - - - - - + + + + + + + + + - - + + - + diff --git a/autoware_launch/launch/components/tier4_sensing_component.launch.xml b/autoware_launch/launch/components/tier4_sensing_component.launch.xml index fe520e1fdc..4cd6eccbaa 100644 --- a/autoware_launch/launch/components/tier4_sensing_component.launch.xml +++ b/autoware_launch/launch/components/tier4_sensing_component.launch.xml @@ -5,7 +5,6 @@ - diff --git a/autoware_launch/launch/components/tier4_simulator_component.launch.xml b/autoware_launch/launch/components/tier4_simulator_component.launch.xml index 7c62bfbc34..a8503818cd 100644 --- a/autoware_launch/launch/components/tier4_simulator_component.launch.xml +++ b/autoware_launch/launch/components/tier4_simulator_component.launch.xml @@ -11,6 +11,7 @@ + @@ -26,6 +27,10 @@ + - - + + + + + + + + + + + diff --git a/autoware_launch/launch/components/tier4_system_component.launch.xml b/autoware_launch/launch/components/tier4_system_component.launch.xml index 3cfc6d8541..a90fdb89a5 100644 --- a/autoware_launch/launch/components/tier4_system_component.launch.xml +++ b/autoware_launch/launch/components/tier4_system_component.launch.xml @@ -1,5 +1,7 @@ + + @@ -8,9 +10,10 @@ + - + diff --git a/autoware_launch/launch/e2e_simulator.launch.xml b/autoware_launch/launch/e2e_simulator.launch.xml index a089da6397..4d978dc3fa 100644 --- a/autoware_launch/launch/e2e_simulator.launch.xml +++ b/autoware_launch/launch/e2e_simulator.launch.xml @@ -2,8 +2,12 @@ - - + + + + + + @@ -18,12 +22,19 @@ + + + @@ -34,6 +45,9 @@ + + + @@ -52,10 +66,12 @@ + + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 6621c15ea3..77d17d7897 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -2,9 +2,13 @@ - - + + + + + + @@ -17,7 +21,6 @@ - @@ -38,6 +41,8 @@ + + @@ -49,7 +54,6 @@ - diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 941399db7e..882ea9d9f9 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -2,19 +2,22 @@ - - + + + + + + + + - - - @@ -22,8 +25,15 @@ + + + + + + + @@ -31,16 +41,19 @@ + + + + + - - @@ -53,6 +66,9 @@ + + + diff --git a/autoware_launch/launch/pointcloud_container.launch.py b/autoware_launch/launch/pointcloud_container.launch.py index 87c46bce69..650e555e27 100644 --- a/autoware_launch/launch/pointcloud_container.launch.py +++ b/autoware_launch/launch/pointcloud_container.launch.py @@ -19,6 +19,7 @@ from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode def generate_launch_description(): @@ -37,13 +38,20 @@ def add_launch_arg(name: str, default_value=None): condition=IfCondition(LaunchConfiguration("use_multithread")), ) + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + namespace="pointcloud_container", + ) + pointcloud_container = ComposableNodeContainer( name=LaunchConfiguration("container_name"), namespace="/", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), - composable_node_descriptions=[], - output="screen", + composable_node_descriptions=[glog_component], + output="both", ) return LaunchDescription( diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 484ea7e8fd..9b998dc29b 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -16,8 +16,6 @@ Panels: Expanded: ~ Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: AutowareDateTimePanel Name: AutowareDateTimePanel - Class: rviz_plugins::AutowareStatePanel @@ -60,49 +58,6 @@ Visualization Manager: Value: false - Class: rviz_common/Group Displays: - - Class: rviz_plugins/SteeringAngle - Enabled: true - Name: SteeringAngle - Scale: 17 - Text Color: 25; 255; 240 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/steering_status - Value: true - Value height offset: 0 - - Class: rviz_plugins/ConsoleMeter - Enabled: true - Name: ConsoleMeter - Scale: 3 - Text Color: 25; 255; 240 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - Value height offset: 0 - - Alpha: 0.999 - 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 @@ -289,29 +244,40 @@ Visualization Manager: Value: true Wave Color: 255; 255; 255 Wave Velocity: 40 - - Class: rviz_plugins/MaxVelocity - Enabled: true - Name: MaxVelocity - Text Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/current_max_velocity - Value: true - - Class: rviz_plugins/TurnSignal + - Class: autoware_overlay_rviz_plugin/SignalDisplay Enabled: true - Name: TurnSignal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/turn_indicators_status + Gear Topic Test: /vehicle/status/gear_status + Hazard Lights Topic: /vehicle/status/hazard_lights_status + Height: 175 + Left: 10 + Name: SignalDisplay + Signal Color: 0; 230; 120 + Speed Limit Topic: /planning/scenario_planning/current_max_velocity + Speed Topic: /vehicle/status/velocity_status + Steering Topic: /vehicle/status/steering_status + Top: 10 + Traffic Topic: /perception/traffic_light_recognition/traffic_signals + Turn Signals Topic: /vehicle/status/turn_indicators_status Value: true + Width: 517 Enabled: true Name: Vehicle + - Class: rviz_plugins/MrmSummaryOverlayDisplay + Enabled: false + Font Size: 10 + Left: 512 + Max Letter Num: 100 + Name: MRM Summary + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /system/emergency/hazard_status + Value: false + Value height offset: 0 Enabled: true Name: System - Class: rviz_common/Group @@ -356,6 +322,7 @@ Visualization Manager: center_lane_line: false center_line_arrows: false crosswalk_lanelets: true + crosswalk_areas: false lane_start_bound: false lanelet direction: true lanelet_id: false @@ -373,10 +340,12 @@ Visualization Manager: shoulder_road_lanelets: false traffic_light: true traffic_light_id: false + traffic_light_reg_elem_id: false traffic_light_triangle: true walkway_lanelets: true hatched_road_markings_bound: true hatched_road_markings_area: false + intersection_area: false Topic: Depth: 5 Durability Policy: Transient Local @@ -878,7 +847,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/debug/rois + Value: /perception/traffic_light_recognition/traffic_light/debug/rois Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -1131,13 +1100,13 @@ Visualization Manager: - Class: rviz_plugins/Path Color Border Vel Max: 3 Enabled: false - Name: PathReference_PullOut + Name: PathReference_StartPlanner Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/path_reference/pull_out + Value: /planning/path_reference/start_planner Value: true View Path: Alpha: 0.3 @@ -1315,13 +1284,13 @@ Visualization Manager: - Class: rviz_plugins/Path Color Border Vel Max: 3 Enabled: true - Name: PathChangeCandidate_PullOut + Name: PathChangeCandidate_StartPlanner Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/path_candidate/pull_out + Value: /planning/path_candidate/start_planner Value: true View Path: Alpha: 0.30000001192092896 @@ -1448,7 +1417,7 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: VirtualWall (PullOver) + Name: VirtualWall (GoalPlanner) Namespaces: {} Topic: @@ -1456,11 +1425,11 @@ Visualization Manager: 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: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/goal_planner Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: VirtualWall (PullOut) + Name: VirtualWall (StartPlanner) Namespaces: {} Topic: @@ -1468,7 +1437,7 @@ Visualization Manager: 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: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/start_planner Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true @@ -1639,6 +1608,30 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/out_of_lane Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (NoDrivableLane) + 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_drivable_lane + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DynamicObstacleStop) + 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/dynamic_obstacle_stop + Value: true Enabled: true Name: VirtualWall - Class: rviz_common/Group @@ -1679,32 +1672,6 @@ Visualization Manager: 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 @@ -1815,7 +1782,19 @@ Visualization Manager: Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: LaneChange + Name: LeftLaneChange + 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_left + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: RightLaneChange Namespaces: {} Topic: @@ -1823,7 +1802,7 @@ Visualization Manager: 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: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_right Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -1851,7 +1830,7 @@ Visualization Manager: Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: PullOut + Name: StartPlanner Namespaces: {} Topic: @@ -1859,7 +1838,7 @@ Visualization Manager: 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: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -1909,6 +1888,30 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_avoidance Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: NoDrivableLane + 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_drivable_lane + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DynamicObstacleStop + 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/dynamic_obstacle_stop + Value: false Enabled: false Name: DebugMarker - Class: rviz_common/Group @@ -1992,7 +1995,7 @@ Visualization Manager: Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: Info (PullOver) + Name: Info (GoalPlanner) Namespaces: {} Topic: @@ -2000,11 +2003,11 @@ Visualization Manager: 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: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/goal_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false - Name: Info (PullOut) + Name: Info (StartPlanner) Namespaces: {} Topic: @@ -2012,7 +2015,7 @@ Visualization Manager: 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: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/start_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -2105,6 +2108,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (MotionVelocitySmoother) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/motion_velocity_smoother/virtual_wall + Value: true Enabled: true Name: VirtualWall - Class: rviz_common/Group @@ -2398,9 +2413,1207 @@ Visualization Manager: Value: false Enabled: true Name: Control + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: ~ + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: PointcloudOnCamera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/camera/camera6/image_raw + Value: true + Visibility: + Control: + Debug/AEB: true + Debug/MPC: true + Debug/PurePursuit: true + Predicted Trajectory: true + Value: false + Debug: + Control: true + Localization: + EKFPoseHistory: true + NDT pointclouds: true + NDTLoadedPCDMap: true + NDTPoseHistory: true + Value: true + Map: true + Perception: + CameraLidarFusion(purple): true + Centerpoint(red1): true + CenterpointROIFusion(red2): true + CenterpointValidator(red3): true + Detection(yellow): true + DetectionByTracker(cyan): true + PointPainting(light_green1): true + PointPaintingROIFusion(light_green2): true + PointPaintingValidator(light_green3): true + Prediction(light_blue): true + RadarFarObjects(white): true + Tracking(green): true + Value: true + Planning: true + Sensing: + ConcatenatePointCloud: true + PointcloudOnCamera: true + RadarRawObjects(white): true + Value: true + Value: true + Localization: + EKF: + PoseHistory: true + Value: true + NDT: + Aligned: true + Initial: true + MonteCarloInitialPose: true + PoseHistory: true + PoseWithCovAligned: true + PoseWithCovInitial: true + Value: true + Value: false + Map: + Lanelet2VectorMap: true + PointCloudMap: true + Value: false + Perception: + ObjectRecognition: + Detection: + DetectedObjects: true + Value: true + Prediction: + Maneuver: true + PredictedObjects: true + Value: true + Tracking: + TrackedObjects: true + Value: true + Value: true + OccupancyGrid: + Map: true + Value: true + Segmentation: + NoGroundPointCloud: true + Value: true + TrafficLight: + MapBasedDetectionResult: true + RecognitionResultOnImage: true + Value: true + Value: false + Planning: + Diagnostic: + PlanningErrorMarker: true + Value: true + MissionPlanning: + GoalPose: true + RouteArea: true + Value: true + ScenarioPlanning: + LaneDriving: + BehaviorPlanning: + (old)PathChangeCandidate_LaneChange: true + Bound: true + DebugMarker: + Arrow: true + Avoidance: true + Blind Spot: true + Crosswalk: true + DetectionArea: true + DynamicAvoidance: true + GoalPlanner: true + Intersection: true + LaneFollowing: true + LeftLaneChange: true + NoDrivableLane: true + NoStoppingArea: true + OcclusionSpot: true + OutOfLane: true + RightLaneChange: true + RunOut: true + SideShift: true + SpeedBump: true + StartPlanner: true + StopLine: true + TrafficLight: true + Value: true + VirtualTrafficLight: true + InfoMarker: + Info (Avoidance): true + Info (AvoidanceByLC): true + Info (DynamicAvoidance): true + Info (ExtLaneChangeLeft): true + Info (ExtLaneChangeRight): true + Info (GoalPlanner): true + Info (LaneChangeLeft): true + Info (LaneChangeRight): true + Info (StartPlanner): true + Value: true + Path: true + PathChangeCandidate_Avoidance: true + PathChangeCandidate_AvoidanceByLC: true + PathChangeCandidate_ExternalRequestLaneChangeLeft: true + PathChangeCandidate_ExternalRequestLaneChangeRight: true + PathChangeCandidate_GoalPlanner: true + PathChangeCandidate_LaneChangeLeft: true + PathChangeCandidate_LaneChangeRight: true + PathChangeCandidate_StartPlanner: true + PathReference_Avoidance: true + PathReference_AvoidanceByLC: true + PathReference_GoalPlanner: true + PathReference_LaneChangeLeft: true + PathReference_LaneChangeRight: true + PathReference_StartPlanner: true + Value: true + VirtualWall: + Value: true + VirtualWall (Avoidance): true + VirtualWall (AvoidanceByLC): true + VirtualWall (BlindSpot): true + VirtualWall (Crosswalk): true + VirtualWall (DetectionArea): true + VirtualWall (ExtLaneChangeLeft): true + VirtualWall (ExtLaneChangeRight): true + VirtualWall (GoalPlanner): true + VirtualWall (Intersection): true + VirtualWall (LaneChangeLeft): true + VirtualWall (LaneChangeRight): true + VirtualWall (MergeFromPrivateArea): true + VirtualWall (NoDrivableLane): true + VirtualWall (NoStoppingArea): true + VirtualWall (OcclusionSpot): true + VirtualWall (OutOfLane): true + VirtualWall (RunOut): true + VirtualWall (SpeedBump): true + VirtualWall (StartPlanner): true + VirtualWall (StopLine): true + VirtualWall (TrafficLight): true + VirtualWall (VirtualTrafficLight): true + VirtualWall (Walkway): true + MotionPlanning: + DebugMarker: + ObstacleAvoidance: true + ObstacleCruise: + CruiseVirtualWall: true + DebugMarker: true + SlowDownVirtualWall: true + Value: true + ObstacleStop: true + SurroundObstacleChecker: + Footprint: true + FootprintOffset: true + FootprintRecoverOffset: true + SurroundObstacleCheck: true + Value: true + Value: true + Trajectory: true + Value: true + VirtualWall: + Value: true + VirtualWall (ObstacleAvoidance): true + VirtualWall (ObstacleCruise): true + VirtualWall (ObstacleStop): true + VirtualWall (SurroundObstacle): true + Value: true + ModifiedGoal: true + Parking: + Costmap: true + PartialPoseArray: true + PoseArray: true + Value: true + ScenarioTrajectory: true + Value: true + Value: false + Sensing: + GNSS: + PoseWithCovariance: true + Value: true + LiDAR: + ConcatenatePointCloud: true + MeasurementRange: true + Value: true + Value: true + System: + Grid: true + MRM Summary: true + TF: true + Value: false + Vehicle: + PolarGridDisplay: true + Value: true + VehicleModel: true + SignalDisplay: true + Value: true + Zoom Factor: 1 + - Alpha: 1 + 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): 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: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - BUS: + Alpha: 0.5 + Color: 255; 255; 255 + CAR: + Alpha: 0.5 + Color: 255; 255; 255 + CYCLIST: + Alpha: 0.5 + Color: 255; 255; 255 + 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.5 + Color: 255; 255; 255 + Name: RadarRawObjects(white) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.5 + Color: 255; 255; 255 + Polygon Type: 3d + TRAILER: + Alpha: 0.5 + Color: 255; 255; 255 + TRUCK: + Alpha: 0.5 + Color: 255; 255; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/radar/detected_objects + UNKNOWN: + Alpha: 0.5 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Sensing + - Class: rviz_common/Group + Enabled: true + Name: Localization + Displays: + - 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: true + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: NDT pointclouds + 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: true + - Alpha: 1 + 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: 255; 255; 255 + Color Transformer: "" + 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: NDTLoadedPCDMap + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.1 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/debug/loaded_pointcloud_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: NDTPoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: true + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: EKFPoseHistory + 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 + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Name: Centerpoint(red1) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/centerpoint/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 138; 128 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Name: CenterpointROIFusion(red2) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/centerpoint/roi_fusion/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 82; 82 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + CAR: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Name: CenterpointValidator(red3) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + TRUCK: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/centerpoint/validation/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 213; 0; 0 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + CAR: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Name: PointPainting(light_green1) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + TRUCK: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/pointpainting/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 178; 255; 89 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + CAR: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Name: PointPaintingROIFusion(light_green2) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + TRUCK: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/pointpainting/roi_fusion/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 118; 255; 3 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + CAR: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Name: PointPaintingValidator(light_green3) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + TRUCK: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/pointpainting/validation/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 100; 221; 23 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Name: DetectionByTracker(orange) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/detection_by_tracker/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 145; 0 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + CAR: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Name: CameraLidarFusion(purple) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + TRUCK: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/clustering/camera_lidar_fusion/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 213; 0; 249 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Name: RadarFarObjects(white) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/radar/far_objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + CAR: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + Name: Detection(yellow) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + TRUCK: + Alpha: 0.9990000128746033 + Color: 255; 234; 0 + 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; 234; 0 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + CAR: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + Name: Tracking(green) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + TRUCK: + Alpha: 0.9990000128746033 + Color: 0; 230; 118 + 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: 0; 230; 118 + Value: false + Visualization Type: Normal + - BUS: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + 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.10000000149011612 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + Name: Prediction(light_blue) + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 0; 176; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 0; 176; 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: 0; 176; 255 + Value: false + Visualization Type: Normal + Enabled: true + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: LaneChangeLeft + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/lane_change_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: LaneChangeRight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/lane_change_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AvoidanceLeft + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/avoidance_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AvoidanceRight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/avoidance_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AvoidanceByLCLeft + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/avoidance_by_lc_left + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AvoidanceByLCRight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/avoidance_by_lc_right + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: StartPlanner + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/start_planner + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: GoalPlanner + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/goal_planner + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: BlindSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/debug/objects_of_interest/no_stopping_area + Value: true + Enabled: true + Name: Objects Of Interest + - Class: rviz_plugins/StringStampedOverlayDisplay + Enabled: true + Font Size: 15 + Left: 1024 + Max Letter Num: 100 + Name: StringStampedOverlayDisplay + Text Color: 25; 255; 240 + Top: 128 + 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/internal_state + Value: true + Value height offset: 0 + Enabled: true + Name: Planning + - Class: rviz_common/Group + Displays: ~ + Enabled: true + Name: Control + Enabled: false + Name: Debug Enabled: true Global Options: - Background Color: 10; 10; 10 + Background Color: 42; 42; 42 Default Light: true Fixed Frame: map Frame Rate: 30 diff --git a/autoware_launch/rviz/image/autoware.png b/autoware_launch/rviz/image/autoware.png index cf2d1e8eff..3b27ce5fbc 100644 Binary files a/autoware_launch/rviz/image/autoware.png and b/autoware_launch/rviz/image/autoware.png differ