From 569922cd7a010fe5cc619049db66af07e60d1275 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 15 Jan 2024 00:25:32 +0900 Subject: [PATCH 01/37] feat: use obstacle_cruise_planner and change safe_distance_margin Signed-off-by: Takayuki Murooka --- autoware_launch/config/planning/preset/default_preset.yaml | 2 +- .../obstacle_cruise_planner/obstacle_cruise_planner.param.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml index 7bd3dd26b3..8954026761 100644 --- a/autoware_launch/config/planning/preset/default_preset.yaml +++ b/autoware_launch/config/planning/preset/default_preset.yaml @@ -94,7 +94,7 @@ launch: - arg: name: motion_stop_planner_type - default: obstacle_stop_planner + default: obstacle_cruise_planner # option: obstacle_stop_planner # obstacle_cruise_planner # none diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 90e897fda3..4d38226c26 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 @@ -12,7 +12,7 @@ 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] + safe_distance_margin : 5.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] From 40935760292c21974bd17e70438dec137c1ace54 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 15 Jan 2024 00:26:09 +0900 Subject: [PATCH 02/37] feat: set max_vel to 40km/h Signed-off-by: Takayuki Murooka --- .../motion_velocity_smoother.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml index 235c76a5c1..44450b1848 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,7 +1,7 @@ /**: ros__parameters: # motion state constraints - max_velocity: 20.0 # max velocity limit [m/s] + max_velocity: 11.1 # max velocity limit [m/s] stop_decel: 0.0 # deceleration at a stop point[m/ss] # external velocity limit parameter From df2779015b1b807ce70a4a62487d885c4156be6a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 16 Jan 2024 11:31:15 +0900 Subject: [PATCH 03/37] feat: enable surround_obstacle_checker Signed-off-by: Takayuki Murooka --- .../launch/components/tier4_planning_component.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml index 06740146f5..a6b5744f1b 100644 --- a/autoware_launch/launch/components/tier4_planning_component.launch.xml +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -8,7 +8,7 @@ - + From 39191f2dc0b589c61a592303f1cca8fd73a19200 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 16 Jan 2024 16:13:18 +0900 Subject: [PATCH 04/37] feat: enable surround_obstacle_checker Signed-off-by: Takayuki Murooka --- autoware_launch/config/planning/preset/default_preset.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml index b57696d324..7c4b452bbf 100644 --- a/autoware_launch/config/planning/preset/default_preset.yaml +++ b/autoware_launch/config/planning/preset/default_preset.yaml @@ -112,7 +112,7 @@ launch: - arg: name: launch_surround_obstacle_checker - default: "false" + default: "true" # parking modules - arg: From 1e6387a7bdfd1c00afa9a727d68d122d1d766ba6 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 22 Jan 2024 02:04:51 +0900 Subject: [PATCH 05/37] feat: enable dynamic_avoidance and disable outside_drivable_area_stop Signed-off-by: Takayuki Murooka --- autoware_launch/config/planning/preset/default_preset.yaml | 2 +- .../obstacle_avoidance_planner.param.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/config/planning/preset/default_preset.yaml b/autoware_launch/config/planning/preset/default_preset.yaml index 7c4b452bbf..72921af441 100644 --- a/autoware_launch/config/planning/preset/default_preset.yaml +++ b/autoware_launch/config/planning/preset/default_preset.yaml @@ -8,7 +8,7 @@ launch: default: "true" - arg: name: launch_dynamic_avoidance_module - default: "false" + default: "true" - arg: name: launch_lane_change_right_module default: "true" 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 db89a81e47..17a044fb67 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 @@ -3,7 +3,7 @@ option: 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 + enable_outside_drivable_area_stop: false # stop if the ego's trajectory footprint is outside the drivable area use_footprint_polygon_for_outside_drivable_area_check: false # If false, only the footprint's corner points are considered. debug: From b19141504820197400750757e06cb63e34590a0f Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 7 Feb 2024 10:49:20 +0900 Subject: [PATCH 06/37] fix(lidar_centerpoint): remove build_only param from param.yaml (#856) Signed-off-by: kminoda --- .../detection/lidar_model/centerpoint_tiny.param.yaml | 1 - 1 file changed, 1 deletion(-) 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 index 5eb560dd7b..474d0e2b69 100644 --- 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 @@ -14,7 +14,6 @@ iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] score_threshold: 0.35 - build_only: false # shutdown node after TensorRT engine file is built has_twist: false trt_precision: fp16 densification_num_past_frames: 1 From 668c9b014eabf6f4a5cc79f6fddf521a9b4233ec Mon Sep 17 00:00:00 2001 From: Anh Nguyen Date: Wed, 7 Feb 2024 11:16:00 +0700 Subject: [PATCH 07/37] chore(map): rework parameters of map (#843) * Added reference to launch parameters to yaml files of map/ Signed-off-by: anhnv3991 * style(pre-commit): autofix --------- Signed-off-by: anhnv3991 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- autoware_launch/config/map/lanelet2_map_loader.param.yaml | 1 + autoware_launch/config/map/pointcloud_map_loader.param.yaml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/autoware_launch/config/map/lanelet2_map_loader.param.yaml b/autoware_launch/config/map/lanelet2_map_loader.param.yaml index 24d2b0e8ed..b830e038f1 100755 --- a/autoware_launch/config/map/lanelet2_map_loader.param.yaml +++ b/autoware_launch/config/map/lanelet2_map_loader.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: center_line_resolution: 5.0 # [m] + lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/autoware_launch/config/map/pointcloud_map_loader.param.yaml b/autoware_launch/config/map/pointcloud_map_loader.param.yaml index b4efbec970..48d10944cb 100644 --- a/autoware_launch/config/map/pointcloud_map_loader.param.yaml +++ b/autoware_launch/config/map/pointcloud_map_loader.param.yaml @@ -7,3 +7,5 @@ # 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 From b1b1423eefcb821f48728851a4e2febbe498d95c Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 7 Feb 2024 14:41:30 +0900 Subject: [PATCH 08/37] chore(planning/control/vehicle): declare ROS params in yaml files (#833) * update yaml Signed-off-by: Yuki Takagi --- .../config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index 7ad685217f..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,7 +3,7 @@ update_rate: 10.0 system_emergency_heartbeat_timeout: 0.5 use_emergency_handling: false - check_external_emergency_heartbeat: false + check_external_emergency_heartbeat: $(var check_external_emergency_heartbeat) use_start_request: false enable_cmd_limit_filter: true filter_activated_count_threshold: 5 From 1ad37efbf853908e701da58a3d615f3a01a650f2 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 7 Feb 2024 16:42:46 +0900 Subject: [PATCH 09/37] feat(intersection): use different expected deceleration for bike/car (#852) Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/intersection.param.yaml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 86bcf801f4..8e50a451f3 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 @@ -56,7 +56,9 @@ duration: 3.0 object_dist_to_stopline: 10.0 ignore_on_amber_traffic_light: - object_expected_deceleration: 2.0 + object_expected_deceleration: + car: 2.0 + bike: 5.0 ignore_on_red_traffic_light: object_margin_to_path: 2.0 avoid_collision_by_acceleration: From 86c9f00d90aec18610e6b3570c27471f3af67dfb Mon Sep 17 00:00:00 2001 From: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Date: Wed, 7 Feb 2024 12:58:23 +0300 Subject: [PATCH 10/37] refactor(rviz): update the class name and turn signal color (#855) Signed-off-by: KhalilSelyan --- autoware_launch/rviz/autoware.rviz | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autoware_launch/rviz/autoware.rviz b/autoware_launch/rviz/autoware.rviz index 4301034e74..f73b720b5d 100644 --- a/autoware_launch/rviz/autoware.rviz +++ b/autoware_launch/rviz/autoware.rviz @@ -244,14 +244,14 @@ Visualization Manager: Value: true Wave Color: 255; 255; 255 Wave Velocity: 40 - - Class: awf_2d_overlay_vehicle/SignalDisplay + - Class: autoware_overlay_rviz_plugin/SignalDisplay Enabled: true Gear Topic Test: /vehicle/status/gear_status Hazard Lights Topic: /vehicle/status/hazard_lights_status Height: 175 Left: 10 Name: SignalDisplay - Signal Color: 94; 130; 255 + 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 From 93fc83e02e2090bc68ea426059c273cf3bec8457 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Wed, 7 Feb 2024 19:02:35 +0900 Subject: [PATCH 11/37] chore(ndt_scan_matcher): rename config path (#854) * chore(ndt_scan_matcher): rename config path Signed-off-by: Yamato Ando * rename path Signed-off-by: Yamato Ando * style(pre-commit): autofix --------- Signed-off-by: Yamato Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ndt_scan_matcher.param.yaml | 0 ...crop_box_filter_measurement_range.param.yaml | 0 .../random_downsample_filter.param.yaml | 0 .../voxel_grid_filter.param.yaml | 0 .../tier4_localization_component.launch.xml | 17 +++++++++++++---- 5 files changed, 13 insertions(+), 4 deletions(-) rename autoware_launch/config/localization/{ => ndt_scan_matcher}/ndt_scan_matcher.param.yaml (100%) rename autoware_launch/config/localization/{ => ndt_scan_matcher/pointcloud_preprocessor}/crop_box_filter_measurement_range.param.yaml (100%) rename autoware_launch/config/localization/{ => ndt_scan_matcher/pointcloud_preprocessor}/random_downsample_filter.param.yaml (100%) rename autoware_launch/config/localization/{ => ndt_scan_matcher/pointcloud_preprocessor}/voxel_grid_filter.param.yaml (100%) diff --git a/autoware_launch/config/localization/ndt_scan_matcher.param.yaml b/autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml similarity index 100% rename from autoware_launch/config/localization/ndt_scan_matcher.param.yaml rename to autoware_launch/config/localization/ndt_scan_matcher/ndt_scan_matcher.param.yaml 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/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index 70170a5fef..c5117009d2 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -19,16 +19,25 @@ - - - - + + + + From 7623f6fea0da7ad2a949b81b5b85b16ebd714a1e Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 8 Feb 2024 01:25:07 +0900 Subject: [PATCH 12/37] feat(start/goal_planner): remove unused param and update time horizon for goal planner's safety check (#863) * remove unused param Signed-off-by: kyoichi-sugahara * update safety check time horizon Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 3 +-- .../start_planner/start_planner.param.yaml | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) 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 d03efa405f..6d82e7790d 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 @@ -128,7 +128,7 @@ delay_until_departure: 1.0 # For target object filtering target_filtering: - safety_check_time_horizon: 5.0 + safety_check_time_horizon: 10.0 safety_check_time_resolution: 1.0 # detection range object_check_forward_distance: 100.0 @@ -164,7 +164,6 @@ method: "integral_predicted_polygon" keep_unsafe_time: 3.0 # collision check parameters - check_all_predicted_path: true publish_debug_marker: false rss_params: rear_vehicle_reaction_time: 2.0 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 154c4b1e0b..5edf7d468b 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 @@ -127,7 +127,6 @@ # safety check configuration enable_safety_check: true # collision check parameters - check_all_predicted_path: true publish_debug_marker: false rss_params: rear_vehicle_reaction_time: 2.0 From 0ce46ea580d6e782136157512079da9b0b30a56c Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 8 Feb 2024 09:02:15 +0900 Subject: [PATCH 13/37] feat: remove use_pointcloud_container (#806) * feat!: remove use_pointcloud_container Signed-off-by: kminoda * style(pre-commit): autofix * remove unnecessary files Signed-off-by: kminoda * revert: revert change in declaration of sample vehicle and sensor_kit Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- autoware_launch/launch/autoware.launch.xml | 10 ++++------ .../components/tier4_planning_component.launch.xml | 1 - .../components/tier4_sensing_component.launch.xml | 1 - autoware_launch/launch/logging_simulator.launch.xml | 2 -- autoware_launch/launch/planning_simulator.launch.xml | 2 -- 5 files changed, 4 insertions(+), 12 deletions(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 7de7741be2..91a12aebfe 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -56,12 +56,10 @@ - - - - - - + + + + diff --git a/autoware_launch/launch/components/tier4_planning_component.launch.xml b/autoware_launch/launch/components/tier4_planning_component.launch.xml index cbc2349a6e..3d57f6198e 100644 --- a/autoware_launch/launch/components/tier4_planning_component.launch.xml +++ b/autoware_launch/launch/components/tier4_planning_component.launch.xml @@ -7,7 +7,6 @@ - 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/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index 57cbc38af6..77d17d7897 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -21,7 +21,6 @@ - @@ -55,7 +54,6 @@ - diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index d19c7308f0..cf56413121 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -50,8 +50,6 @@ - - From a027fa9a30487684d68b83d7a52835b5e645a610 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 8 Feb 2024 10:03:31 +0900 Subject: [PATCH 14/37] chore(intersection): target type param (#851) Signed-off-by: Mamoru Sobue --- .../intersection.param.yaml | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) 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 8e50a451f3..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 @@ -15,6 +15,14 @@ enable_pass_judge_before_default_stopline: false stuck_vehicle: + target_type: + car: true + bus: true + truck: true + trailer: true + motorcycle: false + bicycle: false + unknown: false turn_direction: left: true right: true @@ -27,6 +35,14 @@ 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 @@ -37,6 +53,14 @@ consider_wrong_direction_vehicle: false collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 + 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 From f2f298acd76c5c4d5105c8ef5415395e94198c8e Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 9 Feb 2024 06:52:44 +0900 Subject: [PATCH 15/37] fix(autoware_launch): remove use_pointcloud_container flag completely (#864) --- autoware_launch/launch/autoware.launch.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 91a12aebfe..a5daf3dd16 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -4,7 +4,6 @@ - From 6969c8890570ba7b52de0c40b8761dbfd246d9d0 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 9 Feb 2024 13:30:54 +0900 Subject: [PATCH 16/37] feat(map based prediction, crosswalk)!: transplantation of pedestrians' behavior prediction against green signal (#860) pedestrians' intention estimation feature against the green signal Signed-off-by: Yuki Takagi --- .../prediction/map_based_prediction.param.yaml | 7 ++++++- .../behavior_velocity_planner/crosswalk.param.yaml | 5 ++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml index ed1193f1c4..f8ad371ab9 100644 --- a/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml +++ b/autoware_launch/config/perception/object_recognition/prediction/map_based_prediction.param.yaml @@ -19,7 +19,12 @@ 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 - use_crosswalk_signal: true + 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 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 921b728a34..8e42255f67 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 @@ -53,11 +53,10 @@ stop_object_velocity_threshold: 0.25 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.25 m/s = 0.9 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: true # for the crosswalks with traffic signal disable_yield_for_new_stopped_object: true # for the crosswalks with traffic signal # If the pedestrian does not move for X seconds after the ego has stopped in front the crosswalk, the module judge that the pedestrian has no intention to walk and allows the ego to proceed. - distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order - timeout_map_for_no_intention_to_walk: [1.0, 0.0] # [s] + 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 From 4afc06f12ec0ba398a165595dc4a95285bfd3827 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Fri, 9 Feb 2024 13:26:50 +0300 Subject: [PATCH 17/37] feat(pid_longitudinal_controller): adjust slope compensation parameters (#585) Signed-off-by: Berkay Karaman --- .../control/trajectory_follower/longitudinal/pid.param.yaml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 c39088753f..ad6217663f 100644 --- a/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/longitudinal/pid.param.yaml @@ -69,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 From fe35f03b0c1383a0825c619c38c47e75340f0cf1 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Sat, 10 Feb 2024 23:21:09 +0900 Subject: [PATCH 18/37] chore(radar_object_tracker): move radar object tracker param to yaml (#838) chore: move radar object tracker param to yaml Signed-off-by: yoshiri --- .../radar_object_tracker/radar_object_tracker.param.yaml | 3 +++ .../launch/components/tier4_perception_component.launch.xml | 4 ---- 2 files changed, 3 insertions(+), 4 deletions(-) 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 index d2c0841cf3..3a1e9f02c3 100644 --- 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 @@ -24,3 +24,6 @@ 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/launch/components/tier4_perception_component.launch.xml b/autoware_launch/launch/components/tier4_perception_component.launch.xml index 3519d5780d..3b0f8eca46 100644 --- a/autoware_launch/launch/components/tier4_perception_component.launch.xml +++ b/autoware_launch/launch/components/tier4_perception_component.launch.xml @@ -127,10 +127,6 @@ name="object_recognition_tracking_radar_object_tracker_node_param_path" value="$(find-pkg-share autoware_launch)/config/perception/object_recognition/tracking/radar_object_tracker/radar_object_tracker.param.yaml" /> - Date: Sun, 11 Feb 2024 11:13:42 +0900 Subject: [PATCH 19/37] fix(avoidance): tuning shiftable ratio & deviation param (#869) Signed-off-by: satoshi-ota --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 52724cdda0..49978d07d2 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 @@ -127,7 +127,7 @@ 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 @@ -206,7 +206,7 @@ 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] + max_deviation_from_lane: 0.2 # [m] # avoidance distance parameters longitudinal: min_prepare_time: 1.0 # [s] From 1a31b4b7013e31e4772c01711ae1bf2426c51414 Mon Sep 17 00:00:00 2001 From: Ahmed Ebrahim <36835765+ahmeddesokyebrahim@users.noreply.github.com> Date: Mon, 12 Feb 2024 19:42:20 +0200 Subject: [PATCH 20/37] fix(log-messages): reduce excessive log messages (#760) Signed-off-by: AhmedEbrahim --- .../control/control_validator/control_validator.param.yaml | 2 -- 1 file changed, 2 deletions(-) diff --git a/autoware_launch/config/control/control_validator/control_validator.param.yaml b/autoware_launch/config/control/control_validator/control_validator.param.yaml index c51cbafba2..ffd259e5e5 100644 --- a/autoware_launch/config/control/control_validator/control_validator.param.yaml +++ b/autoware_launch/config/control/control_validator/control_validator.param.yaml @@ -1,8 +1,6 @@ /**: ros__parameters: - publish_diag: false # if true, diagnostic msg is published - # If the number of consecutive invalid predicted_path exceeds this threshold, the Diag will be set to ERROR. # (For example, threshold = 1 means, even if the predicted_path is invalid, Diag will not be ERROR if # the next predicted_path is valid.) From 2db8354615d88dc5bf6b6188f7a90ef229d3a3f5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 13 Feb 2024 13:37:48 +0900 Subject: [PATCH 21/37] feat(goal_planne): check objects within the area between ego edge and boudary of pull_over_lanes (#867) Signed-off-by: Mamoru Sobue --- .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 3 +++ 1 file changed, 3 insertions(+) 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 6d82e7790d..66542e1bf1 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 @@ -39,6 +39,9 @@ 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: From f5ca49c53f3151e6442023721f3745f9914c2652 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 13 Feb 2024 14:25:35 +0900 Subject: [PATCH 22/37] feat(planning_simulator): use fit_target=vector_map in planning_simulator (#859) * Added fit_target Signed-off-by: Shintaro Sakoda * Fixed arg name Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda --- autoware_launch/launch/planning_simulator.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index cf56413121..63443c928f 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -13,6 +13,8 @@ + + From 1cf2e8f8a11c98d13b4b7c0188d0d0fbaaf46269 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 13 Feb 2024 14:29:50 +0900 Subject: [PATCH 23/37] chore(crosswalk): change LATER param (#868) crosswalk/change-LATER-param Signed-off-by: Yuki Takagi --- .../behavior_velocity_planner/crosswalk.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8e42255f67..6de22a67dc 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 @@ -41,7 +41,7 @@ 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: [10.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_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 From 6a2320e0709a898de64a83d1f44d5e4b73a75765 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 13 Feb 2024 17:38:31 +0900 Subject: [PATCH 24/37] fix(localization): add ar tag based localizer param (#871) Added ar_tag_based_localizer.param.yaml Signed-off-by: Shintaro Sakoda --- .../ar_tag_based_localizer.param.yaml | 40 +++++++++++++++++++ .../tier4_localization_component.launch.xml | 3 ++ 2 files changed, 43 insertions(+) create mode 100644 autoware_launch/config/localization/ar_tag_based_localizer.param.yaml 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/launch/components/tier4_localization_component.launch.xml b/autoware_launch/launch/components/tier4_localization_component.launch.xml index c5117009d2..3ccd98104d 100644 --- a/autoware_launch/launch/components/tier4_localization_component.launch.xml +++ b/autoware_launch/launch/components/tier4_localization_component.launch.xml @@ -52,6 +52,9 @@ + + + From 07d12301e03857416a77ce2beb13774aa62d1cfc Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Wed, 14 Feb 2024 13:26:20 +0900 Subject: [PATCH 25/37] feat(motion_velocity_smoother): increase engage_acceleration (#736) * feat(motion_velocity_smoother): increase engage_acceleration * Update autoware_launch/config/planning/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml --- .../motion_velocity_smoother.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 4b6693f150..cfe03102a1 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 @@ -26,7 +26,7 @@ # 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] From 1a6dbe2ea610263c12936b9323c5bfe9e2b6f852 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 14 Feb 2024 21:32:27 +0900 Subject: [PATCH 26/37] feat: define common max_vel (#870) Signed-off-by: Takayuki Murooka --- .../config/planning/scenario_planning/common/common.param.yaml | 2 ++ .../motion_velocity_smoother.param.yaml | 1 - 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index 03958fda9a..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] 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 cfe03102a1..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,7 +1,6 @@ /**: 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 From 39892b18358bb024ea68db611e33c377419e66d2 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 15 Feb 2024 00:57:22 +0900 Subject: [PATCH 27/37] refactor(blind_spot): find first_conflicting_lane just as intersection module (#873) temp Signed-off-by: Mamoru Sobue --- .../behavior_velocity_planner/blind_spot.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7bd7d88230..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,7 @@ 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] From 6c6e8640e6725e137a0650c389af2cfceb0cd25a Mon Sep 17 00:00:00 2001 From: Zhe Shen <80969277+HansOersted@users.noreply.github.com> Date: Thu, 15 Feb 2024 08:40:17 +0900 Subject: [PATCH 28/37] feat(tier4_control_launch): disable the trajectory extension (#866) disable the trajectory extending for terminal yaw control Signed-off-by: Zhe Shen --- .../config/control/trajectory_follower/lateral/mpc.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9998b6aadf..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) From bb8e579f2aad82982e38308ee7d92905970bc099 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 15 Feb 2024 09:10:41 +0900 Subject: [PATCH 29/37] fix(planning_validator): add missing params (#876) Signed-off-by: satoshi-ota --- .../planning_validator/planning_validator.param.yaml | 11 +++++++++++ 1 file changed, 11 insertions(+) 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 From 3fff913415566244fde4b7890d9c2179e61f6824 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Thu, 15 Feb 2024 13:35:33 +0900 Subject: [PATCH 30/37] chore(start_planner): remove unused parameter (#878) Signed-off-by: Shumpei Wakabayashi --- .../behavior_path_planner/start_planner/start_planner.param.yaml | 1 - 1 file changed, 1 deletion(-) 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 5edf7d468b..cfa630b24b 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 @@ -6,7 +6,6 @@ th_stopped_velocity: 0.01 th_stopped_time: 1.0 collision_check_margins: [2.0, 1.0, 0.5, 0.1] - collision_check_distance_from_end: -10.0 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 From a6141ab4e75a00b3a7663160d77540f6a8b60a59 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 15 Feb 2024 14:10:51 +0900 Subject: [PATCH 31/37] fix(start_planner): fix safety_check_time_horizon (#875) Signed-off-by: kosuke55 --- .../start_planner/start_planner.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 cfa630b24b..3fed8bd726 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 @@ -92,7 +92,7 @@ delay_until_departure: 1.0 # For target object filtering target_filtering: - safety_check_time_horizon: 5.0 + safety_check_time_horizon: 10.0 safety_check_time_resolution: 1.0 # detection range object_check_forward_distance: 10.0 From 59556db9794164f3c54a1f29c0b708115d25db57 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 15 Feb 2024 18:11:28 +0900 Subject: [PATCH 32/37] fix(system_error_monitor): changed settings of /autoware/localization/performance_monitoring (#877) Fixed settings of /autoware/localization/performance_monitoring Signed-off-by: Shintaro Sakoda --- .../system_error_monitor/system_error_monitor.awsim.param.yaml | 2 ++ .../system/system_error_monitor/system_error_monitor.param.yaml | 1 + 2 files changed, 3 insertions(+) diff --git a/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml b/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml index 69e3831591..a3c712d466 100644 --- a/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml +++ b/autoware_launch/config/system/system_error_monitor/system_error_monitor.awsim.param.yaml @@ -24,6 +24,8 @@ /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 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 d8229425af..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 @@ -24,6 +24,7 @@ /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: 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 From a2532da0279beb1da7401941b321d26c7b8007bb Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 16 Feb 2024 17:56:31 +0900 Subject: [PATCH 33/37] feat(avoidance): use free steer policy for safety check (#865) Signed-off-by: satoshi-ota --- .../behavior_path_planner/avoidance/avoidance.param.yaml | 1 + 1 file changed, 1 insertion(+) 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 49978d07d2..2ac846ed25 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 @@ -187,6 +187,7 @@ 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] From 39fbda1e496a50c2ec5683e8775d08bb504269c6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 20 Feb 2024 13:52:48 +0900 Subject: [PATCH 34/37] feat(strat_planner): add a prepare time for blinker before taking action for approval (#881) Signed-off-by: Mamoru Sobue --- .../behavior_path_planner/start_planner/start_planner.param.yaml | 1 + 1 file changed, 1 insertion(+) 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 3fed8bd726..2c55e9fad5 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 @@ -38,6 +38,7 @@ 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 From 9039471f84b634fc57962180e054c8d5b8211998 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 20 Feb 2024 14:13:30 +0900 Subject: [PATCH 35/37] feat(traffic_light_arbiter): add parameter of signal match validator (#879) Signed-off-by: Tomohito Ando --- .../traffic_light_arbiter/traffic_light_arbiter.param.yaml | 1 + 1 file changed, 1 insertion(+) 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 index 5dc2b62eaa..dfe12ff352 100644 --- 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 @@ -3,3 +3,4 @@ external_time_tolerance: 5.0 perception_time_tolerance: 1.0 external_priority: false + enable_signal_matching: false From b23c5f948421037d2bf81dd340015c4b2fd1602c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 20 Feb 2024 20:13:31 +0900 Subject: [PATCH 36/37] feat(goal_planner): change pull over path candidate priority with soft and hard margins (#874) Signed-off-by: kosuke55 --- .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 66542e1bf1..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 @@ -35,7 +35,8 @@ # object recognition object_recognition: use_object_recognition: true - object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker + 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 From c86e9a2d34b088e5759e94d0800ed4de18b20acc Mon Sep 17 00:00:00 2001 From: Shintaro Tomie <58775300+Shin-kyoto@users.noreply.github.com> Date: Tue, 20 Feb 2024 21:30:20 +0900 Subject: [PATCH 37/37] fix: recovery default parameter (#882) Signed-off-by: Shin-kyoto --- .../detection/lidar_model/pointpainting.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 8ccd469786..3abaffb243 100755 --- 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 @@ -26,7 +26,7 @@ iou_nms_target_class_names: ["CAR"] iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 - score_threshold: 0.4 + score_threshold: 0.35 omp_params: # omp params num_threads: 1