From 569922cd7a010fe5cc619049db66af07e60d1275 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 15 Jan 2024 00:25:32 +0900 Subject: [PATCH 1/8] 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 2/8] 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 3/8] 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 4/8] 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 5/8] 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 7969af4ba1d0d996aa38b392f0bc930f5c1de7a2 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 23 Jan 2024 14:27:21 +0900 Subject: [PATCH 6/8] Update collision check margins in start planner configuration Signed-off-by: kyoichi-sugahara --- .../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 5edf7d468b..9a2800c0ba 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,7 @@ 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 + back_objects_collision_check_margin: 3.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 176f2981cf9393ad80e23c77cef4fbd9fcff1b3b Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 23 Jan 2024 15:42:25 +0900 Subject: [PATCH 7/8] Update collision check margins in start planner configuration Signed-off-by: kyoichi-sugahara --- .../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 9a2800c0ba..60b81fe5d4 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 @@ -5,7 +5,7 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margins: [2.0, 1.0, 0.5, 0.1] + objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1] back_objects_collision_check_margin: 3.0 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 From 3300addf8feb38cc4d47ba555b5bf1b7e39024c5 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sun, 11 Feb 2024 14:44:03 +0900 Subject: [PATCH 8/8] update param of back_objects_collision_check_margin Signed-off-by: kyoichi-sugahara --- .../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 60b81fe5d4..2ff013ec95 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,7 @@ th_stopped_velocity: 0.01 th_stopped_time: 1.0 objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1] - back_objects_collision_check_margin: 3.0 + back_objects_collision_check_margin: 0.5 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5