From d26150e1a2f4a3a661bd2416b59e162185048838 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Thu, 27 Jun 2024 10:15:32 +0900 Subject: [PATCH 1/2] feat(behavior_path_planner): add yaw threshold param (#1040) add yaw threshold param Signed-off-by: Daniel Sanchez --- .../static_obstacle_avoidance.param.yaml | 1 + .../behavior_path_planner/goal_planner/goal_planner.param.yaml | 1 + .../behavior_path_planner/lane_change/lane_change.param.yaml | 1 + .../start_planner/start_planner.param.yaml | 2 ++ 4 files changed, 5 insertions(+) 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 dd2e13892d..c6c7f4ee5c 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 @@ -174,6 +174,7 @@ safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 1.5 # [-] hysteresis_factor_safe_count: 3 # [-] + collision_check_yaw_diff_threshold: 3.1416 # [rad] # predicted path parameters min_velocity: 1.38 # [m/s] max_velocity: 50.0 # [m/s] diff --git a/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/goal_planner/goal_planner.param.yaml index eea6d0acdc..adbda50864 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 @@ -182,6 +182,7 @@ time_horizon: 10.0 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 3.1416 # temporary backward_path_length: 30.0 forward_path_length: 100.0 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 10757691c9..d301e96deb 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 @@ -30,6 +30,7 @@ # safety check safety_check: allow_loose_check_for_cancel: true + collision_check_yaw_diff_threshold: 3.1416 execution: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.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 7d64d8c44a..13c43cd4d4 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 @@ -143,8 +143,10 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 + collision_check_yaw_diff_threshold: 1.578 # temporary backward_path_length: 30.0 forward_path_length: 100.0 From cb266c4287dcd5681a82043938dc52c43dc574b5 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 27 Jun 2024 12:47:19 +0900 Subject: [PATCH 2/2] refactor(static_obstacle_avoidance): organize params for drivable lane (#1042) Signed-off-by: satoshi-ota --- .../static_obstacle_avoidance.param.yaml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) 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 c6c7f4ee5c..cd5e2cb326 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 @@ -5,9 +5,13 @@ resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER resample_interval_for_output: 4.0 # [m] FOR DEVELOPER + # drivable lane setting. this module is able to use not only current lane but also right/left lane + # if the current lane(=lanelet::Lanelet) and the right/left lane share the boundary(=lanelet::Linestring) in HDMap. + # "current_lane" : use only current lane. this module doesn't use adjacent lane to avoid object. + # "same_direction_lane" : this module uses same direction lane to avoid object if need. + # "opposite_direction_lane": this module uses both same direction and opposite direction lane. + use_lane_type: "opposite_direction_lane" # drivable area setting - use_adjacent_lane: true - use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true use_freespace_areas: true