From 8c52fd27824131f1a5fa4a401c118ad8dcbd82b7 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 26 Jun 2024 13:26:12 +0900 Subject: [PATCH] Rebase all x2 changes to tier4/main fix parameter namespace remove old parameter file remove old style parameter file remove unused parameter remove duplicated parameter remove old style parameter remove old style parameter pick new style parameter remove duplicated parameter remove unnecessary parameter remove old parameter Revert "feat: sp tuning for low obstacle (#578)" This reverts commit f00363f012cfcfe8c244d153e281796c25a9d7cf. Revert "chore(ground_segmentation): reduce ground_segmentation range (#649)" This reverts commit 3aacc89e5cb7fced34ea0c404c24366934ec8925. fix ekf parameter remove temporary parameter disable pull over of mrm change initial_selector_mode to local reset analytical smoother parameter remove hazard lights selector remove l4toolkit enable stuck vehicle detection in left turn chore diagnostic_graph_aggregator params fix module name chore x2_preset.yaml fix(static_obstacle_avoidance): parameter tuning Signed-off-by: satoshi-ota fix(slow_down): parameter tuning Signed-off-by: satoshi-ota fix typo remove duplicated parameter remove old node revert common planning param for safety reason. use mrm_handler as default relax emergency stop acceleration for safety reason modify rviz setting remove l4toolkit from simulation fix lane change priority fix behavior velocity planner parameter make same mpt param as main update mlmodel parameter style fix old parameter name --- .../control_validator.param.yaml | 1 - .../external_cmd_selector.param.yaml | 2 +- .../longitudinal/pid.param.yaml | 3 +- .../localization/ekf_localizer.param.yaml | 16 +- .../localization/ndt_scan_matcher.param.yaml | 84 ------- ...el_grid_based_euclidean_cluster.param.yaml | 2 +- .../lidar_model/centerpoint_x2.param.yaml | 20 ++ .../pointcloud_map_filter.param.yaml | 4 +- .../data_association_matrix.param.yaml | 2 +- .../ground_segmentation.param.yaml | 66 ++--- .../config/planning/preset/x2_preset.yaml | 30 ++- .../Analytical.param.yaml | 4 +- .../common/common.param.yaml | 8 +- .../static_obstacle_avoidance.param.yaml | 28 +-- .../avoidance/avoidance.param.yaml | 222 ----------------- .../drivable_area_expansion.param.yaml | 18 -- .../lane_change/lane_change.param.yaml | 20 +- .../scene_module_manager.param.yaml | 4 +- .../start_planner/start_planner.param.yaml | 3 +- .../behavior_velocity_planner.param.yaml | 2 +- .../crosswalk.param.yaml | 6 +- .../intersection.param.yaml | 24 +- .../run_out.param.yaml | 6 - .../path_optimizer.param.yaml | 2 +- .../obstacle_cruise_planner.param.yaml | 11 +- ...cruise_planner_pseudo_occlusion.param.yaml | 227 ------------------ .../component_state_monitor/topics.yaml | 13 - .../hazard_lights_selector.param.yaml | 3 - .../mrm_emergency_stop_operator.param.yaml | 4 +- .../system/mrm_handler/mrm_handler.param.yaml | 2 +- autoware_launch/launch/autoware.launch.xml | 16 -- .../tier4_localization_component.launch.xml | 2 +- .../tier4_perception_component.launch.xml | 3 +- .../tier4_system_component.launch.xml | 17 +- .../launch/logging_simulator.launch.xml | 2 - .../launch/planning_simulator.launch.xml | 2 - autoware_launch/rviz/autoware.rviz | 4 +- 37 files changed, 137 insertions(+), 746 deletions(-) delete mode 100644 autoware_launch/config/localization/ndt_scan_matcher.param.yaml create mode 100644 autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_x2.param.yaml delete mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml delete mode 100644 autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml delete mode 100644 autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml diff --git a/autoware_launch/config/control/control_validator/control_validator.param.yaml b/autoware_launch/config/control/control_validator/control_validator.param.yaml index 3910360424..ffd259e5e5 100644 --- a/autoware_launch/config/control/control_validator/control_validator.param.yaml +++ b/autoware_launch/config/control/control_validator/control_validator.param.yaml @@ -10,4 +10,3 @@ thresholds: max_distance_deviation: 1.0 - min_velocity_for_checking: 1.0 # m/s diff --git a/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml b/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml index 3de702aa00..6556656cc8 100644 --- a/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml +++ b/autoware_launch/config/control/external_cmd_selector/external_cmd_selector.param.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: update_rate: 10.0 - initial_selector_mode: "remote" # ["local", "remote"] + initial_selector_mode: "local" # ["local", "remote"] 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 fd83953182..688e650de1 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -71,9 +71,8 @@ # slope compensation # pitch - use_trajectory_for_pitch_calculation: true lpf_pitch_gain: 0.95 - slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + slope_source: "trajectory_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/localization/ekf_localizer.param.yaml b/autoware_launch/config/localization/ekf_localizer.param.yaml index 096db82bdd..6df24435cc 100644 --- a/autoware_launch/config/localization/ekf_localizer.param.yaml +++ b/autoware_launch/config/localization/ekf_localizer.param.yaml @@ -1,10 +1,12 @@ /**: ros__parameters: - show_debug_info: false - enable_yaw_bias_estimation: false - predict_frequency: 50.0 - tf_rate: 50.0 - extend_state_step: 50 + node: + show_debug_info: false + enable_yaw_bias_estimation: false + predict_frequency: 50.0 + tf_rate: 50.0 + publish_tf: true + extend_state_step: 50 pose_measurement: # for Pose measurement @@ -40,7 +42,5 @@ misc: # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.0 # [m/s] + threshold_observable_velocity_mps: 0.5 # [m/s] pose_frame_id: "map" - # for velocity measurement limitation (Set 0.0 if you want to ignore) - threshold_observable_velocity_mps: 0.5 # [m/s] 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/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml index b99612ab8f..26b027f007 100644 --- a/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/clustering/voxel_grid_based_euclidean_cluster.param.yaml @@ -3,7 +3,7 @@ tolerance: 0.7 voxel_leaf_size: 0.3 min_points_number_per_voxel: 1 - min_cluster_size: 3 + min_cluster_size: 10 max_cluster_size: 3000 use_height: false input_frame: "base_link" diff --git a/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_x2.param.yaml b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_x2.param.yaml new file mode 100644 index 0000000000..0777ddb92b --- /dev/null +++ b/autoware_launch/config/perception/object_recognition/detection/lidar_model/centerpoint_x2.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + + # 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" + trt_precision: fp16 + post_process_params: + # 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 + densification_params: + world_frame_id: map + num_past_frames: 1 diff --git a/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml b/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml index 5b9ad199fa..62b3074c15 100644 --- a/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml +++ b/autoware_launch/config/perception/object_recognition/detection/pointcloud_filter/pointcloud_map_filter.param.yaml @@ -2,13 +2,13 @@ ros__parameters: # voxel size for downsample filter - down_sample_voxel_size: 0.05 + down_sample_voxel_size: 0.1 # 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.3 + 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/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 9743a9ce59..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 @@ -56,7 +56,7 @@ min_iou_matrix: # If value is negative, it will be ignored. #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [0.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + [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 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 689ada5e47..ec966ecc45 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 @@ -1,6 +1,6 @@ /**: ros__parameters: - additional_lidars: ["front_lower"] + additional_lidars: [] ransac_input_topics: [] use_single_frame_filter: False use_time_series_filter: True @@ -8,11 +8,11 @@ common_crop_box_filter: parameters: min_x: -70.0 - max_x: 100.0 + max_x: 120.0 min_y: -75.0 max_y: 75.0 - max_z: 3.2 - min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 3.2 + margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False common_ground_filter: @@ -20,10 +20,10 @@ parameters: global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 25.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.15 + split_points_distance_tolerance: 0.2 use_virtual_ground_point: True split_height_distance: 0.2 - non_ground_height_threshold: 0.12 + non_ground_height_threshold: 0.20 grid_size_m: 0.2 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 5 @@ -44,12 +44,12 @@ front_upper_crop_box_filter: parameters: - min_x: 5.8 + min_x: -50.0 max_x: 100.0 min_y: -50.0 max_y: 50.0 - max_z: 3.2 # recommended 2.5 for non elevation_grid_mode - min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 3.2 # recommended 2.5 for non elevation_grid_mode + margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False front_upper_ground_filter: @@ -58,7 +58,7 @@ global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 18.0 # recommended 30.0 for non elevation_grid_mode split_points_distance_tolerance: 0.20 # recommended 0.045 for non elevation_grid_mode - split_height_distance: 0.15 # recommended 0.15 for non elevation_grid_mode + split_height_distance: 0.2 # recommended 0.15 for non elevation_grid_mode use_virtual_ground_point: False non_ground_height_threshold: 0.1 grid_size_m: 0.1 @@ -74,12 +74,12 @@ front_lower_crop_box_filter: parameters: - min_x: 5.8 + min_x: -50.0 max_x: 100.0 min_y: -50.0 max_y: 50.0 - max_z: 3.2 # recommended 2.5 for non elevation_grid_mode - min_z: -2.5 # recommended 0.0 for non elevation_grid_mode + margin_max_z: 3.2 # recommended 2.5 for non elevation_grid_mode + margin_min_z: -2.5 # recommended 0.0 for non elevation_grid_mode negative: False front_lower_ground_filter: @@ -87,46 +87,16 @@ parameters: global_slope_max_angle_deg: 10.0 local_slope_max_angle_deg: 18.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.10 # recommended 0.1 for non elevation_grid_mode - split_height_distance: 0.05 # recommended 0.05 for non elevation_grid_mode - use_virtual_ground_point: true + split_points_distance_tolerance: 0.20 # recommended 0.1 for non elevation_grid_mode + split_height_distance: 0.2 # recommended 0.05 for non elevation_grid_mode + use_virtual_ground_point: False non_ground_height_threshold: 0.1 grid_size_m: 0.1 grid_mode_switch_radius: 20.0 gnd_grid_buffer_size: 4 detection_range_z_max: 3.2 - elevation_grid_mode: true - use_recheck_ground_cluster: true - use_lowest_point: true - low_priority_region_x: -20.0 - center_pcl_shift: 0.0 - radial_divider_angle_deg: 1.0 - - left_upper_crop_box_filter: - parameters: - min_x: 5.8 - max_x: 40.0 - min_y: -10.0 - max_y: 10.0 - max_z: 1. # recommended 2.5 for non elevation_grid_mode - min_z: -1.0 # recommended 0.0 for non elevation_grid_mode - negative: False - - left_upper_ground_filter: - plugin: "ground_segmentation::ScanGroundFilterComponent" - parameters: - global_slope_max_angle_deg: 6.0 - local_slope_max_angle_deg: 6.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.20 # recommended 0.045 for non elevation_grid_mode - split_height_distance: 0.2 # recommended 0.15 for non elevation_grid_mode - use_virtual_ground_point: False - non_ground_height_threshold: 0.05 - grid_size_m: 0.2 - grid_mode_switch_radius: 10.0 - gnd_grid_buffer_size: 3 - detection_range_z_max: 3.2 - elevation_grid_mode: true - use_recheck_ground_cluster: true + elevation_grid_mode: false + use_recheck_ground_cluster: false use_lowest_point: true low_priority_region_x: -20.0 center_pcl_shift: 0.0 diff --git a/autoware_launch/config/planning/preset/x2_preset.yaml b/autoware_launch/config/planning/preset/x2_preset.yaml index 21378f9d97..12c1bcadf5 100644 --- a/autoware_launch/config/planning/preset/x2_preset.yaml +++ b/autoware_launch/config/planning/preset/x2_preset.yaml @@ -1,14 +1,17 @@ launch: # behavior path modules - arg: - name: launch_avoidance_module + name: launch_static_obstacle_avoidance default: "true" - arg: name: launch_avoidance_by_lane_change_module default: "false" - arg: - name: launch_dynamic_avoidance_module + name: launch_dynamic_obstacle_avoidance 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" @@ -71,9 +74,6 @@ launch: - 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" @@ -87,21 +87,31 @@ launch: - arg: name: motion_path_planner_type - default: obstacle_avoidance_planner - # option: obstacle_avoidance_planner + default: path_optimizer + # option: path_optimizer # path_sampler # none + # motion velocity planner modules + - arg: + name: launch_dynamic_obstacle_stop_module + default: "false" + - arg: + name: launch_out_of_lane_module + default: "true" + - arg: + name: launch_obstacle_velocity_limiter_module + default: "true" + - arg: name: motion_stop_planner_type - default: obstacle_cruise_planner_with_pseudo_occlusion + default: obstacle_cruise_planner # option: obstacle_stop_planner # obstacle_cruise_planner - # obstacle_cruise_planner_with_pseudo_occlusion # none - arg: - name: motion_velocity_smoother_type + name: velocity_smoother_type default: JerkFiltered # option: JerkFiltered # L2 diff --git a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml index 232610713f..329714e3d3 100644 --- a/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/autoware_velocity_smoother/Analytical.param.yaml @@ -17,8 +17,8 @@ kp: 0.3 backward: - start_jerk: -0.3 - min_jerk_mild_stop: -0.5 + start_jerk: -0.1 + min_jerk_mild_stop: -0.3 min_jerk: -1.5 min_acc_mild_stop: -1.0 min_acc: -2.5 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 857c40cdf2..9f1c261562 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -6,12 +6,12 @@ normal: min_acc: -1.0 # min deceleration [m/ss] max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -0.6 # min jerk [m/sss] + min_jerk: -0.3 # min jerk [m/sss] max_jerk: 0.6 # max jerk [m/sss] # constraints to be observed limit: - min_acc: -6.0 # min deceleration limit [m/ss] + min_acc: -2.5 # min deceleration limit [m/ss] max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -20.0 # min jerk limit [m/sss] - max_jerk: 20.0 # max jerk limit [m/sss] + min_jerk: -1.5 # min jerk limit [m/sss] + max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml index 24e88c8ed6..7e03b1c45f 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/static_obstacle_avoidance.param.yaml @@ -23,41 +23,41 @@ th_moving_time: 2.0 # [s] longitudinal_margin: 0.0 # [m] lateral_margin: - soft_margin: 0.3 # [m] + soft_margin: 0.2 # [m] hard_margin: 0.2 # [m] hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 # [-] FOR DEVELOPER - envelope_buffer_margin: 0.5 # [m] FOR DEVELOPER + envelope_buffer_margin: 0.1 # [m] FOR DEVELOPER truck: th_moving_speed: 1.0 th_moving_time: 2.0 longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.3 + soft_margin: 0.2 hard_margin: 0.2 hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.5 + envelope_buffer_margin: 0.1 bus: th_moving_speed: 1.0 th_moving_time: 2.0 longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.3 + soft_margin: 0.2 hard_margin: 0.2 hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.5 + envelope_buffer_margin: 0.1 trailer: th_moving_speed: 1.0 th_moving_time: 2.0 longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.3 + soft_margin: 0.2 hard_margin: 0.2 hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 - envelope_buffer_margin: 0.5 + envelope_buffer_margin: 0.1 unknown: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -214,7 +214,7 @@ longitudinal: min_prepare_time: 1.0 # [s] max_prepare_time: 2.0 # [s] - min_prepare_distance: 1.0 # [m] + min_prepare_distance: 4.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] FOR DEVELOPER nominal_avoidance_speed: 8.33 # [m/s] FOR DEVELOPER @@ -253,7 +253,7 @@ # 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" # [-] + deceleration: "reliable" # [-] # 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. @@ -261,7 +261,7 @@ # 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 # [-] + use_shorten_margin_immediately: false # [-] # -------------------------------------- # FOLLOWING PARAMETERS ARE FOR DEVELOPER @@ -270,10 +270,10 @@ constraints: # lateral constraints lateral: - velocity: [1.39, 4.17, 11.1] # [m/s] + velocity: [2.78, 8.33, 11.1] # [m/s] max_accel_values: [0.5, 0.5, 0.5] # [m/ss] min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] - max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] + max_jerk_values: [0.5, 1.0, 1.0] # [m/sss] # longitudinal constraints longitudinal: @@ -281,7 +281,7 @@ nominal_jerk: 0.5 # [m/sss] max_deceleration: -1.5 # [m/ss] max_jerk: 1.0 # [m/sss] - max_acceleration: 0.5 # [m/ss] + max_acceleration: 0.3 # [m/ss] min_velocity_to_limit_max_acceleration: 2.78 # [m/ss] shift_line_pipeline: 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 deleted file mode 100644 index b131e60dd0..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ /dev/null @@ -1,222 +0,0 @@ -# see AvoidanceParameters description in avoidance_module_data.hpp for description. -/**: - ros__parameters: - 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 - disable_path_update: false - - # drivable area setting - use_adjacent_lane: true - use_opposite_lane: true - use_intersection_areas: true - use_hatched_road_markings: true - - # for debug - publish_debug_marker: false - print_debug_info: false - - # avoidance is performed for the object type with true - target_object: - car: - enable: true # [-] - moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] - lateral_margin: - soft_margin: 0.2 # [m] - hard_margin: 0.2 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.1 # [m] - safety_buffer_longitudinal: 0.0 # [m] - use_conservative_buffer_longitudinal: false # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. - truck: - enable: true - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.2 # [m] - hard_margin: 0.2 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 0.0 - safety_buffer_lateral: 0.8 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: false - bus: - enable: true - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.2 # [m] - hard_margin: 0.2 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.1 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: false - trailer: - enable: true - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.2 # [m] - hard_margin: 0.2 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.1 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: false - unknown: - enable: false - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 1.0 # [m] - hard_margin: 0.7 # [m] - hard_margin_for_parked_vehicle: 0.7 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - safety_buffer_lateral: 0.7 - safety_buffer_longitudinal: 0.0 - use_conservative_buffer_longitudinal: false - bicycle: - enable: false - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.0 # [m] - hard_margin: 1.0 # [m] - hard_margin_for_parked_vehicle: 1.0 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: false - motorcycle: - enable: false - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.0 # [m] - hard_margin: 1.0 # [m] - hard_margin_for_parked_vehicle: 1.0 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: false - pedestrian: - enable: false - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 - lateral_margin: - soft_margin: 0.0 # [m] - hard_margin: 1.0 # [m] - hard_margin_for_parked_vehicle: 1.0 # [m] - max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - safety_buffer_longitudinal: 1.0 - use_conservative_buffer_longitudinal: false - 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_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] - # 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: 10.0 # [m] - object_check_goal_distance: 0.0 # [m] - # filtering parking objects - threshold_distance_object_is_on_center: 0.5 # [m] - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] - # lost object compensation - object_last_seen_threshold: 2.0 - - # For safety check - safety_check: - 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 # [-] - - # For avoidance maneuver - avoidance: - # avoidance lateral parameters - lateral: - lateral_collision_margin: 1.0 # [m] - lateral_execution_threshold: 0.499 # [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.5 # [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_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] - - # For yield maneuver - yield: - yield_velocity: 2.78 # [m/s] - - # For stop maneuver - stop: - min_distance: 10.0 # [m] - max_distance: 20.0 # [m] - - constraints: - # vehicle slows down under longitudinal constraints - use_constraints_for_decel: false # [-] - - # lateral constraints - lateral: - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] - - # 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] - - shift_line_pipeline: - trim: - quantize_filter_threshold: 0.2 - same_grad_filter_1_threshold: 0.1 - same_grad_filter_2_threshold: 0.2 - same_grad_filter_3_threshold: 0.5 - sharp_shift_filter_threshold: 0.2 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 7226876c75..46082e2659 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 @@ -18,11 +18,6 @@ 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 - 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 dynamic_objects: avoid: false # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: @@ -34,22 +29,9 @@ 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. - 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) - extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border - curbstone - drivable_area distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid - compensate: - enable: false # 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 - replan_checker: - enable: true # if true, only recalculate the expanded drivable area when the path or its original drivable area change significantly - # not compatible with dynamic_objects.avoid - max_deviation: 1.0 # [m] full replan is only done if the path changes by more than this distance 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 2605bfd563..7adb3fe776 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 @@ -46,14 +46,6 @@ lateral_distance_max_threshold: 0.75 longitudinal_distance_min_threshold: 2.0 longitudinal_velocity_delta_time: 0.3 - stuck: - 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 @@ -68,11 +60,6 @@ left_offset: 1.0 # [m] right_offset: 1.0 # [m] - # 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] @@ -88,7 +75,7 @@ unknown: false bicycle: true motorcycle: true - pedestrian: false + pedestrian: true # collision check enable_collision_check_for_prepare_phase: @@ -111,11 +98,6 @@ velocity: 0.5 # [m/s] stop_time: 3.0 # [s] - # ego vehicle stuck detection - stuck_detection: - velocity: 0.5 # [m/s] - stop_time: 3.0 # [s] - # lane change cancel cancel: enable_on_prepare_phase: true 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 d54c1b210f..c62c866492 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 @@ -24,7 +24,7 @@ enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false keep_last: false - priority: 4 + priority: 5 max_module_size: 1 lane_change_right: @@ -32,7 +32,7 @@ enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false keep_last: false - priority: 4 + priority: 5 max_module_size: 1 start_planner: 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 index 073daba72b..9d7054487f 100644 --- 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 @@ -26,11 +26,12 @@ allow_check_shift_path_lane_departure_override: false shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 - deceleration_interval: 15.0 + maximum_longitudinal_deviation: 1.0 # Note, should be the same or less than planning validator's "longitudinal_distance_deviation" 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 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 cb4e8acdbd..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 @@ -4,7 +4,7 @@ backward_path_length: 5.0 behavior_output_path_interval: 1.0 stop_line_extend_length: 5.0 - max_accel: -4.0 + max_accel: -2.8 max_jerk: -5.0 system_delay: 0.5 delay_response_time: 0.5 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml index 54ef4ff402..88d730f654 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 @@ -30,8 +30,8 @@ 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. - stuck_vehicle_attention_range: 10.0 # [m] Ego does not enter the crosswalk area if a stuck vehicle exists within this distance from the end of the crosswalk on the ego's path. - min_acc: -4.0 # min acceleration [m/ss] + required_clearance: 6.0 # [m] clearance to be secured between the ego and the ahead vehicle + min_acc: -2.8 # min acceleration [m/ss] min_jerk: -5.0 # min jerk [m/sss] max_jerk: 5.0 # max jerk [m/sss] @@ -47,7 +47,7 @@ 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: -4.0 # min acceleration [m/ss] + min_acc: -2.8 # min acceleration [m/ss] min_jerk: -5.0 # min jerk [m/sss] max_jerk: 5.0 # max jerk [m/sss] 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 08bed1714c..7820a5db31 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 @@ -9,7 +9,7 @@ default_stopline_margin: 3.0 stopline_overshoot_margin: 0.5 path_interpolation_ds: 0.1 - max_accel: -4.0 + max_accel: -2.8 max_jerk: -5.0 delay_response_time: 0.5 enable_pass_judge_before_default_stopline: false @@ -24,7 +24,7 @@ bicycle: false unknown: false turn_direction: - left: false + left: true right: true straight: true use_stuck_stopline: true @@ -75,17 +75,18 @@ not_prioritized: 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 - use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module - minimum_upstream_velocity: 1.38 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 10.0 duration: 8.0 object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: - object_expected_deceleration: 2.0 # [m/ss] + 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: true @@ -105,11 +106,12 @@ occlusion_detection_hold_time: 1.5 temporal_stop_time_before_peeking: 0.1 temporal_stop_before_attention_area: false - absence_traffic_light: - creep_velocity: 1.388 # [m/s] - maximum_peeking_distance: -0.5 # [m] - attention_lane_crop_curvature_threshold: 0.25 - attention_lane_curvature_calculation_ds: 0.5 + creep_velocity_without_traffic_light: 1.388 + static_occlusion_with_traffic_light_timeout: 0.5 + + debug: + ttc: [0] + enable_rtc: intersection: false 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 e5fdabef3a..c8905f66da 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 @@ -67,12 +67,6 @@ 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: 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. - # 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 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml index 1f78eecb02..93cc644040 100644 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/autoware_path_optimizer/path_optimizer.param.yaml @@ -44,7 +44,7 @@ enable_optimization_validation: false common: - num_points: 50 # number of points for optimization [-] + num_points: 100 # number of points for optimization [-] delta_arc_length: 1.0 # delta arc length for optimization [m] kinematics: 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 57322fd487..70dfbaec9c 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 @@ -107,11 +107,6 @@ slow_down: max_lat_margin: 0.8 # lateral margin between obstacle and trajectory band with ego's width lat_hysteresis_margin: 0.2 - pseudo_occlusion: - enable_function: false # If true, the slow down function behave as pseudo_occlusion - max_obj_vel: 1.0 # Objects which is slow than this speed are focused by the pseudo_occlusion function - focus_intersections: [0] # Specify the polygon's ID of lanelet map. If a object is on this ID's area the function treat the object even if it's right-hand beside object. ID "0" is required to parse as ROS params. - successive_num_to_entry_slow_down_condition: 5 successive_num_to_exit_slow_down_condition: 5 @@ -198,9 +193,9 @@ max_ego_velocity: 5.56 static: min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 4.0 - max_ego_velocity: 8.0 + max_lat_margin: 0.7 + min_ego_velocity: 2.0 + max_ego_velocity: 5.56 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 diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml deleted file mode 100644 index e01cd3c99d..0000000000 --- a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner_pseudo_occlusion.param.yaml +++ /dev/null @@ -1,227 +0,0 @@ -/**: - ros__parameters: - common: - planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - - enable_debug_info: false - enable_calculation_time_info: false - - enable_slow_down_planning: true - - # longitudinal info - idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] - min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] - 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 : 6.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: 6.0 # [m] - stop_on_curve: - enable_approaching: true - additional_safe_distance_margin: 2.0 # [m] - min_safe_distance_margin: 3.5 # [m] - suppress_sudden_obstacle_stop: true - - stop_obstacle_type: - unknown: false - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: false - pedestrian: false - - cruise_obstacle_type: - inside: - unknown: false - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: false - pedestrian: false - - outside: - unknown: false - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: false - pedestrian: false - - slow_down_obstacle_type: - unknown: false - car: true - truck: true - bus: true - trailer: true - motorcycle: false - bicycle: false - pedestrian: false - - behavior_determination: - decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking - - prediction_resampling_time_interval: 0.1 - prediction_resampling_time_horizon: 10.0 - - stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle - - # hysteresis for cruise and stop - obstacle_velocity_threshold_from_cruise_to_stop : 4.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - obstacle_velocity_threshold_from_stop_to_cruise : 4.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] - - # if crossing vehicle is determined as target obstacles or not - crossing_obstacle: - obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] - 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 - 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] - max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego - - slow_down: - max_lat_margin: 1.5 # lateral margin between obstacle and trajectory band with ego's width - lat_hysteresis_margin: 0.2 - pseudo_occlusion: - enable_function: true # If true, the slow down function behave as pseudo_occlusion - max_obj_vel: 1.0 # Objects which is slow than this speed are focused by the pseudo_occlusion function - focus_intersections: [0, 452] # Specify the polygon's ID of lanelet map. If a object is on this ID's area the function treat the object even if it's right-hand beside object. ID "0" is required to parse as ROS params. - - 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 - error_function_type: quadratic # choose from linear, quadratic - - velocity_limit_based_planner: - # PID gains to keep safe distance with the front vehicle - kp: 10.0 - ki: 0.0 - kd: 2.0 - - output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] - - enable_jerk_limit_to_output_acc: false - - disable_target_acceleration: true - - velocity_insertion_based_planner: - kp_acc: 6.0 - ki_acc: 0.0 - kd_acc: 2.0 - - kp_jerk: 20.0 - ki_jerk: 0.0 - kd_jerk: 0.0 - - output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - - enable_jerk_limit_to_output_acc: true - - min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] - min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] - time_to_evaluate_rss: 0.0 - - lpf_normalized_error_cruise_dist_gain: 0.2 - - optimization_based_planner: - dense_resampling_time_interval: 0.2 - sparse_resampling_time_interval: 2.0 - dense_time_horizon: 5.0 - max_time_horizon: 25.0 - velocity_margin: 0.2 #[m/s] - - # Parameters for safe distance - t_dangerous: 0.5 - - # For initial Motion - replan_vel_deviation: 5.0 # 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_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] - - # Weights for optimization - max_s_weight: 100.0 - max_v_weight: 1.0 - over_s_safety_weight: 1000000.0 - over_s_ideal_weight: 50.0 - over_v_weight: 500000.0 - over_a_weight: 5000.0 - over_j_weight: 10000.0 - - slow_down: - # parameters to calculate slow down velocity by linear interpolation - labels: - - "default" - - "pedestrian" - default: - moving: - min_lat_margin: 0.7 - max_lat_margin: 1.5 - min_ego_velocity: 2.78 - max_ego_velocity: 9.72 - static: - min_lat_margin: 0.7 - max_lat_margin: 1.5 - min_ego_velocity: 2.78 - max_ego_velocity: 9.72 - pedestrian: - moving: - min_lat_margin: 0.5 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 4.0 - static: - min_lat_margin: 0.5 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 4.0 - moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" - moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold - - time_margin_on_target_velocity: 1.5 # [s] - - lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity - lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path - lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start - stop: - type_specified_params: - labels: # For the listed types, the node try to read the following type specified values - - "default" - - "unknown" - # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. - # limit_min_acc: common_param.yaml/limit.min_acc - unknown: - limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. - sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". - sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". - abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. diff --git a/autoware_launch/config/system/component_state_monitor/topics.yaml b/autoware_launch/config/system/component_state_monitor/topics.yaml index 9e81adf3e3..a9b18001d7 100644 --- a/autoware_launch/config/system/component_state_monitor/topics.yaml +++ b/autoware_launch/config/system/component_state_monitor/topics.yaml @@ -50,19 +50,6 @@ 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 diff --git a/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml b/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml deleted file mode 100644 index 68273fd990..0000000000 --- a/autoware_launch/config/system/hazard_lights_selector/hazard_lights_selector.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - update_rate: 10 # hazard lights command publish rate [Hz] diff --git a/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml b/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml index d3f0d41fd7..1ee2699a23 100644 --- a/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml +++ b/autoware_launch/config/system/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml @@ -1,5 +1,5 @@ /**: ros__parameters: update_rate: 30 - target_acceleration: -4.0 - target_jerk: -5.0 + target_acceleration: -2.5 + target_jerk: -1.5 diff --git a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml index f4fda15af6..2370259cbe 100644 --- a/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml +++ b/autoware_launch/config/system/mrm_handler/mrm_handler.param.yaml @@ -9,7 +9,7 @@ use_emergency_holding: false timeout_emergency_recovery: 5.0 use_parking_after_stopped: false - use_pull_over: true + use_pull_over: false use_comfortable_stop: true # setting whether to turn hazard lamp on for each situation diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index b96bfdcbc0..5cf27f274f 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -23,7 +23,6 @@ - @@ -47,9 +46,6 @@ - - - @@ -136,11 +132,6 @@ - - - - - - - - - - - - diff --git a/autoware_launch/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index b6e2f8f18a..1b92977330 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/autoware_launch/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 5d9ecebe1f..fe24e836b0 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -25,8 +25,7 @@ - - + diff --git a/autoware_launch/launch/components/tier4_system_component.launch.xml b/autoware_launch/launch/components/tier4_system_component.launch.xml index eee2bf2f0d..027837196d 100644 --- a/autoware_launch/launch/components/tier4_system_component.launch.xml +++ b/autoware_launch/launch/components/tier4_system_component.launch.xml @@ -15,10 +15,10 @@ - + + - @@ -29,10 +29,17 @@ - + - - + + + + + + + + + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 12f9cab3be..fa33147824 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -75,8 +75,6 @@ - - diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index 366f1d8ef8..88712916b1 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -76,8 +76,6 @@ - - diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 96b17995c2..fc757b8156 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -2215,7 +2215,7 @@ Visualization Manager: Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: false + Enabled: true Name: DebugMarker Namespaces: {} @@ -2226,7 +2226,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker Value: true - Enabled: true + Enabled: false Name: ObstacleCruise - Class: rviz_default_plugins/MarkerArray Enabled: false