From ace3310a36b4c0142e8a11efcd43e95afd8e3553 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 12 Apr 2024 11:39:17 +0900 Subject: [PATCH] refactor(avoidance): rebuild parameter structure (#6786) (#1252) * refactor(avoidance): unify similar param * refactor(avoidance): rename misreading params * refactor(avoidance): update parameter namespace * refactor(avoidance): use prefix th instead of threshold * refactor(avoidance): rename params * refactor(AbLC): rename params * refactor(avoidance): update yaml * refactor(AbLC): update yaml --------- Signed-off-by: satoshi-ota --- .../avoidance_by_lane_change.param.yaml | 52 ++-- .../src/manager.cpp | 24 +- .../config/avoidance.param.yaml | 292 +++++++++--------- .../data_structs.hpp | 20 +- .../behavior_path_avoidance_module/helper.hpp | 7 +- .../parameter_helper.hpp | 78 ++--- .../src/manager.cpp | 64 ++-- .../src/scene.cpp | 8 +- .../src/shift_line_generator.cpp | 8 +- .../src/utils.cpp | 4 +- 10 files changed, 268 insertions(+), 289 deletions(-) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml index ad5fe0b123f1d..384ef7bd285e9 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml @@ -8,8 +8,8 @@ target_object: car: execute_num: 2 # [-] - moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] + th_moving_speed: 1.0 # [m/s] + th_moving_time: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] lateral_margin: @@ -18,8 +18,8 @@ hard_margin_for_parked_vehicle: 0.0 # [m] truck: execute_num: 2 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 # 3.6km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 lateral_margin: @@ -28,8 +28,8 @@ hard_margin_for_parked_vehicle: 0.0 # [m] bus: execute_num: 2 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 # 3.6km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 lateral_margin: @@ -38,8 +38,8 @@ hard_margin_for_parked_vehicle: 0.0 # [m] trailer: execute_num: 2 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 # 3.6km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 lateral_margin: @@ -48,8 +48,8 @@ hard_margin_for_parked_vehicle: 0.0 # [m] unknown: execute_num: 1 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 # 1.0km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 lateral_margin: @@ -58,34 +58,34 @@ hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: execute_num: 2 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 # 1.0km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 lateral_margin: soft_margin: 0.0 # [m] - hard_margin: 0.0 # [m] - hard_margin_for_parked_vehicle: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] motorcycle: execute_num: 2 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 # 3.6km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 lateral_margin: soft_margin: 0.0 # [m] - hard_margin: 0.0 # [m] - hard_margin_for_parked_vehicle: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] pedestrian: execute_num: 2 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 # 1.0km/h + th_moving_time: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 lateral_margin: soft_margin: 0.0 # [m] - hard_margin: 0.0 # [m] - hard_margin_for_parked_vehicle: 0.0 # [m] + hard_margin: 1.0 # [m] + hard_margin_for_parked_vehicle: 1.0 # [m] lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] @@ -101,11 +101,3 @@ bicycle: false # [-] motorcycle: false # [-] pedestrian: false # [-] - - constraints: - # lateral constraints - lateral: - velocity: [1.0, 1.38, 11.1] # [m/s] - max_accel_values: [0.5, 0.5, 0.5] # [m/ss] - min_jerk_values: [0.2, 0.2, 0.2] # [m/sss] - max_jerk_values: [1.0, 1.0, 1.0] # [m/sss] diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 799364437955c..a3b28b4d63d06 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -65,10 +65,8 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); + param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); @@ -121,14 +119,18 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_last_seen_threshold = + getOrDeclareParameter(*node, ns + "max_compensation_time"); + } + + { + const std::string ns = "avoidance.target_filtering.parked_vehicle."; p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); + getOrDeclareParameter(*node, ns + "th_offset_from_centerline"); p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); + getOrDeclareParameter(*node, ns + "th_shiftable_ratio"); p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); - p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + getOrDeclareParameter(*node, ns + "min_road_shoulder_width"); } { @@ -137,9 +139,9 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); p.time_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.time_threshold"); + getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.distance_threshold"); + getOrDeclareParameter(*node, ns + "condition.th_moving_distance"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index b7b8d8434b9c4..92f533bc63872 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -2,11 +2,11 @@ /**: ros__parameters: avoidance: - resample_interval_for_planning: 0.3 # [m] - resample_interval_for_output: 4.0 # [m] + resample_interval_for_planning: 0.3 # [m] FOR DEVELOPER + resample_interval_for_output: 4.0 # [m] FOR DEVELOPER + # avoidance module common setting enable_bound_clipping: false - enable_cancel_maneuver: true disable_path_update: false # drivable area setting @@ -16,215 +16,203 @@ use_hatched_road_markings: true use_freespace_areas: true - # for debug - publish_debug_marker: false - print_debug_info: false - # avoidance is performed for the object type with true target_object: car: - execute_num: 1 # [-] - moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 2.0 # [s] + th_moving_speed: 1.0 # [m/s] + th_moving_time: 2.0 # [s] + longitudinal_margin: 0.0 # [m] lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.3 # [m] - hard_margin_for_parked_vehicle: 0.3 # [m] - max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.5 # [m] - safety_buffer_longitudinal: 0.0 # [m] - use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. + soft_margin: 0.3 # [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 + use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: - execute_num: 1 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 2.0 + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bus: - execute_num: 1 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 2.0 + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true trailer: - execute_num: 1 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 2.0 + th_moving_speed: 1.0 + th_moving_time: 2.0 + longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.9 # [m] - hard_margin: 0.1 # [m] - hard_margin_for_parked_vehicle: 0.1 # [m] + soft_margin: 0.3 + hard_margin: 0.2 + hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true unknown: - execute_num: 1 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 0.0 lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: -0.2 # [m] - hard_margin_for_parked_vehicle: -0.2 # [m] + soft_margin: 0.7 + hard_margin: -0.2 + hard_margin_for_parked_vehicle: -0.2 max_expand_ratio: 0.0 envelope_buffer_margin: 0.1 - safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bicycle: - execute_num: 1 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 1.0 lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.5 # [m] - hard_margin_for_parked_vehicle: 0.5 # [m] + soft_margin: 0.7 + hard_margin: 0.5 + hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true motorcycle: - execute_num: 1 - moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + th_moving_speed: 1.0 + th_moving_time: 1.0 + longitudinal_margin: 1.0 lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.3 # [m] - hard_margin_for_parked_vehicle: 0.3 # [m] + soft_margin: 0.7 + hard_margin: 0.3 + hard_margin_for_parked_vehicle: 0.3 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true pedestrian: - execute_num: 1 - moving_speed_threshold: 0.28 # 1.0km/h - moving_time_threshold: 1.0 + th_moving_speed: 0.28 + th_moving_time: 1.0 + longitudinal_margin: 1.0 lateral_margin: - soft_margin: 0.7 # [m] - hard_margin: 0.5 # [m] - hard_margin_for_parked_vehicle: 0.5 # [m] + soft_margin: 0.7 + hard_margin: 0.5 + hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 - safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true - lower_distance_for_polygon_expansion: 30.0 # [m] - upper_distance_for_polygon_expansion: 100.0 # [m] + lower_distance_for_polygon_expansion: 30.0 # [m] FOR DEVELOPER + upper_distance_for_polygon_expansion: 100.0 # [m] FOR DEVELOPER # For target object filtering target_filtering: # avoidance target type target_type: - car: true # [-] - truck: true # [-] - bus: true # [-] - trailer: true # [-] - unknown: true # [-] - bicycle: true # [-] - motorcycle: true # [-] - pedestrian: true # [-] + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: true # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] # detection range - object_check_goal_distance: 20.0 # [m] - object_check_return_pose_distance: 20.0 # [m] - # filtering parking objects - threshold_distance_object_is_on_center: 1.0 # [m] - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] # lost object compensation - object_last_seen_threshold: 2.0 + max_compensation_time: 2.0 # detection area generation parameters detection_area: - static: false # [-] - min_forward_distance: 50.0 # [m] - max_forward_distance: 150.0 # [m] - backward_distance: 10.0 # [m] + static: false # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + + # filtering parking objects + parked_vehicle: + th_offset_from_centerline: 1.0 # [m] + th_shiftable_ratio: 0.8 # [-] + min_road_shoulder_width: 0.5 # [m] FOR DEVELOPER # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: - enable: false # [-] - closest_distance_to_wait_and_see: 10.0 # [m] + enable: true # [-] + closest_distance_to_wait_and_see: 10.0 # [m] condition: - time_threshold: 1.0 # [s] - distance_threshold: 1.0 # [m] + th_stopped_time: 3.0 # [s] + th_moving_distance: 1.0 # [m] ignore_area: traffic_light: - front_distance: 100.0 # [m] + front_distance: 100.0 # [m] crosswalk: - front_distance: 30.0 # [m] - behind_distance: 30.0 # [m] + front_distance: 30.0 # [m] + behind_distance: 30.0 # [m] # params for filtering objects that are in intersection intersection: - yaw_deviation: 0.349 # [rad] (default 20.0deg) + yaw_deviation: 0.349 # [rad] (default 20.0deg) # For safety check safety_check: # safety check target type target_type: - car: true # [-] - truck: true # [-] - bus: true # [-] - trailer: true # [-] - unknown: false # [-] - bicycle: true # [-] - motorcycle: true # [-] - pedestrian: true # [-] + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: false # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] # safety check configuration - enable: true # [-] - check_current_lane: false # [-] - check_shift_side_lane: true # [-] - check_other_side_lane: false # [-] - check_unavoidable_object: false # [-] - check_other_object: true # [-] + enable: true # [-] + check_current_lane: false # [-] + check_shift_side_lane: true # [-] + check_other_side_lane: false # [-] + check_unavoidable_object: false # [-] + check_other_object: true # [-] # collision check parameters - check_all_predicted_path: false # [-] - safety_check_backward_distance: 100.0 # [m] - hysteresis_factor_expand_rate: 1.5 # [-] - hysteresis_factor_safe_count: 10 # [-] + check_all_predicted_path: false # [-] + safety_check_backward_distance: 100.0 # [m] + hysteresis_factor_expand_rate: 1.5 # [-] + hysteresis_factor_safe_count: 3 # [-] # predicted path parameters - min_velocity: 1.38 # [m/s] - max_velocity: 50.0 # [m/s] - time_resolution: 0.5 # [s] - time_horizon_for_front_object: 3.0 # [s] - time_horizon_for_rear_object: 10.0 # [s] - delay_until_departure: 0.0 # [s] + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 3.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] # rss parameters - extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" - expected_front_deceleration: -1.0 # [m/ss] - expected_rear_deceleration: -1.0 # [m/ss] - rear_vehicle_reaction_time: 2.0 # [s] - rear_vehicle_safety_time_margin: 1.0 # [s] - lateral_distance_max_threshold: 0.75 # [m] - longitudinal_distance_min_threshold: 3.0 # [m] - longitudinal_velocity_delta_time: 0.8 # [s] + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.0 # [s] # For avoidance maneuver avoidance: # avoidance lateral parameters lateral: - lateral_execution_threshold: 0.09 # [m] - lateral_small_shift_threshold: 0.101 # [m] - lateral_avoid_check_threshold: 0.1 # [m] - soft_road_shoulder_margin: 0.3 # [m] - hard_road_shoulder_margin: 0.3 # [m] - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - max_deviation_from_lane: 0.5 # [m] + th_avoid_execution: 0.09 # [m] FOR DEVELOPER + th_small_shift_length: 0.101 # [m] FOR DEVELOPER + soft_drivable_bound_margin: 0.3 # [m] + hard_drivable_bound_margin: 0.3 # [m] + max_right_shift_length: 5.0 # [m] FOR DEVELOPER + max_left_shift_length: 5.0 # [m] FOR DEVELOPER + max_deviation_from_lane: 0.2 # [m] # approve the next shift line after reaching this percentage of the current shift line length. - # this parameter should be within range of 0.0 to 1.0. # 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 1.0. (in that case, avoidance module creates return shift as soon as possible.) + # this feature can be disabled by setting this parameter to 0.0. ratio_for_return_shift_approval: 0.5 # [-] # avoidance distance parameters longitudinal: @@ -232,17 +220,21 @@ max_prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] - buf_slow_down_speed: 0.56 # [m/s] - nominal_avoidance_speed: 8.33 # [m/s] + buf_slow_down_speed: 0.56 # [m/s] FOR DEVELOPER + nominal_avoidance_speed: 8.33 # [m/s] FOR DEVELOPER # return dead line return_dead_line: goal: enable: true # [-] - buffer: 30.0 # [m] + buffer: 3.0 # [m] traffic_light: enable: true # [-] buffer: 3.0 # [m] + # For cancel maneuver + cancel: + enable: true # [-] + # For yield maneuver yield: enable: true # [-] @@ -251,7 +243,7 @@ # For stop maneuver stop: max_distance: 20.0 # [m] - stop_buffer: 1.0 # [m] + stop_buffer: 1.0 # [m] FOR DEVELOPER policy: # policy for rtc request. select "per_shift_line" or "per_avoidance_maneuver". @@ -273,6 +265,10 @@ # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin. use_shorten_margin_immediately: true # [-] + # -------------------------------------- + # FOLLOWING PARAMETERS ARE FOR DEVELOPER + # -------------------------------------- + constraints: # lateral constraints lateral: @@ -292,8 +288,12 @@ shift_line_pipeline: trim: - quantize_filter_threshold: 0.1 - same_grad_filter_1_threshold: 0.1 - same_grad_filter_2_threshold: 0.2 - same_grad_filter_3_threshold: 0.5 - sharp_shift_filter_threshold: 0.2 + quantize_size: 0.1 + th_similar_grad_1: 0.1 + th_similar_grad_2: 0.2 + th_similar_grad_3: 0.5 + + # for debug + debug: + marker: false + console: false diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index fd841ede37e70..bc66598d3ee90 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -75,7 +75,7 @@ struct ObjectParameter double lateral_hard_margin_for_parked_vehicle{1.0}; - double safety_buffer_longitudinal{0.0}; + double longitudinal_margin{0.0}; bool use_conservative_buffer_longitudinal{true}; }; @@ -243,11 +243,11 @@ struct AvoidanceParameters // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. - double soft_road_shoulder_margin{1.0}; + double soft_drivable_bound_margin{1.0}; // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. - double hard_road_shoulder_margin{1.0}; + double hard_drivable_bound_margin{1.0}; // Even if the obstacle is very large, it will not avoid more than this length for right direction double max_right_shift_length{0.0}; @@ -276,26 +276,20 @@ struct AvoidanceParameters // line. double lateral_small_shift_threshold{0.0}; - // use for judge if the ego is shifting or not. - double lateral_avoid_check_threshold{0.0}; - // use for return shift approval. double ratio_for_return_shift_approval{0.0}; // For shift line generation process. The continuous shift length is quantized by this value. - double quantize_filter_threshold{0.0}; + double quantize_size{0.0}; // For shift line generation process. Merge small shift lines. (First step) - double same_grad_filter_1_threshold{0.0}; + double th_similar_grad_1{0.0}; // For shift line generation process. Merge small shift lines. (Second step) - double same_grad_filter_2_threshold{0.0}; + double th_similar_grad_2{0.0}; // For shift line generation process. Merge small shift lines. (Third step) - double same_grad_filter_3_threshold{0.0}; - - // For shift line generation process. Remove sharp(=jerky) shift line. - double sharp_shift_filter_threshold{0.0}; + double th_similar_grad_3{0.0}; // policy bool use_shorten_margin_immediately{false}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 4ff02df97bd89..2e481e7c98e44 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -151,15 +151,14 @@ class AvoidanceHelper const auto & additional_buffer_longitudinal = object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front : 0.0; - return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; + return object_parameter.longitudinal_margin + additional_buffer_longitudinal; } double getRearConstantDistance(const ObjectData & object) const { const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear + - object.length; + return object_parameter.longitudinal_margin + data_->parameters.base_link2rear + object.length; } double getEgoShift() const @@ -370,7 +369,7 @@ class AvoidanceHelper bool isShifted() const { - return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold; + return std::abs(getEgoShift()) > parameters_->lateral_execution_threshold; } bool isInitialized() const diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index fbfec58abe4da..5b8963c7ca22d 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -40,10 +40,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.resample_interval_for_output = getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); - p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); - p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); - p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); } // drivable area @@ -61,11 +58,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); - param.moving_speed_threshold = - getOrDeclareParameter(*node, ns + "moving_speed_threshold"); - param.moving_time_threshold = - getOrDeclareParameter(*node, ns + "moving_time_threshold"); + param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "th_moving_speed"); + param.moving_time_threshold = getOrDeclareParameter(*node, ns + "th_moving_time"); param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); @@ -75,8 +69,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); param.lateral_hard_margin_for_parked_vehicle = getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); - param.safety_buffer_longitudinal = - getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); + param.longitudinal_margin = getOrDeclareParameter(*node, ns + "longitudinal_margin"); param.use_conservative_buffer_longitudinal = getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); return param; @@ -124,16 +117,20 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.object_check_return_pose_distance = getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); - p.threshold_distance_object_is_on_center = - getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); - p.object_check_shiftable_ratio = - getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); - p.object_check_min_road_shoulder_width = - getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); p.object_check_yaw_deviation = getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); p.object_last_seen_threshold = - getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); + getOrDeclareParameter(*node, ns + "max_compensation_time"); + } + + { + const std::string ns = "avoidance.target_filtering.parked_vehicle."; + p.threshold_distance_object_is_on_center = + getOrDeclareParameter(*node, ns + "th_offset_from_centerline"); + p.object_check_shiftable_ratio = + getOrDeclareParameter(*node, ns + "th_shiftable_ratio"); + p.object_check_min_road_shoulder_width = + getOrDeclareParameter(*node, ns + "min_road_shoulder_width"); } { @@ -142,9 +139,9 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); p.time_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.time_threshold"); + getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = - getOrDeclareParameter(*node, ns + "condition.distance_threshold"); + getOrDeclareParameter(*node, ns + "condition.th_moving_distance"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = @@ -244,16 +241,13 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // avoidance maneuver (lateral) { const std::string ns = "avoidance.avoidance.lateral."; - p.soft_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); - p.hard_road_shoulder_margin = - getOrDeclareParameter(*node, ns + "hard_road_shoulder_margin"); - p.lateral_execution_threshold = - getOrDeclareParameter(*node, ns + "lateral_execution_threshold"); + p.soft_drivable_bound_margin = + getOrDeclareParameter(*node, ns + "soft_drivable_bound_margin"); + p.hard_drivable_bound_margin = + getOrDeclareParameter(*node, ns + "hard_drivable_bound_margin"); + p.lateral_execution_threshold = getOrDeclareParameter(*node, ns + "th_avoid_execution"); p.lateral_small_shift_threshold = - getOrDeclareParameter(*node, ns + "lateral_small_shift_threshold"); - p.lateral_avoid_check_threshold = - getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); + getOrDeclareParameter(*node, ns + "th_small_shift_length"); p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); p.max_deviation_from_lane = @@ -289,6 +283,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "traffic_light.buffer"); } + // cancel + { + const std::string ns = "avoidance.cancel."; + p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable"); + } + // yield { const std::string ns = "avoidance.yield."; @@ -369,17 +369,19 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // shift line pipeline { const std::string ns = "avoidance.shift_line_pipeline."; - p.quantize_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); - p.same_grad_filter_1_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_1_threshold"); - p.same_grad_filter_2_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_2_threshold"); - p.same_grad_filter_3_threshold = - getOrDeclareParameter(*node, ns + "trim.same_grad_filter_3_threshold"); - p.sharp_shift_filter_threshold = - getOrDeclareParameter(*node, ns + "trim.sharp_shift_filter_threshold"); + p.quantize_size = getOrDeclareParameter(*node, ns + "trim.quantize_size"); + p.th_similar_grad_1 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_1"); + p.th_similar_grad_2 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_2"); + p.th_similar_grad_3 = getOrDeclareParameter(*node, ns + "trim.th_similar_grad_3"); } + + // debug + { + const std::string ns = "avoidance.debug."; + p.publish_debug_marker = getOrDeclareParameter(*node, ns + "marker"); + p.print_debug_info = getOrDeclareParameter(*node, ns + "console"); + } + return p; } } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 5ce63987621ed..3261214d1b9b6 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -46,18 +46,11 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "enable_safety_check", p->enable_safety_check); - updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); - updateParam(parameters, ns + "print_debug_info", p->print_debug_info); - } - const auto update_object_param = [&p, ¶meters]( const auto & semantic, const std::string & ns) { auto & config = p->object_parameters.at(semantic); - updateParam(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold); - updateParam(parameters, ns + "moving_time_threshold", config.moving_time_threshold); + updateParam(parameters, ns + "th_moving_speed", config.moving_speed_threshold); + updateParam(parameters, ns + "th_moving_time", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); updateParam(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin); @@ -65,8 +58,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector( parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle", config.lateral_hard_margin_for_parked_vehicle); - updateParam( - parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); + updateParam(parameters, ns + "longitudinal_margin", config.longitudinal_margin); updateParam( parameters, ns + "use_conservative_buffer_longitudinal", config.use_conservative_buffer_longitudinal); @@ -113,16 +105,16 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorobject_check_goal_distance); updateParam( parameters, ns + "object_check_return_pose_distance", p->object_check_return_pose_distance); + updateParam(parameters, ns + "max_compensation_time", p->object_last_seen_threshold); + } + + { + const std::string ns = "avoidance.target_filtering.parked_vehicle."; updateParam( - parameters, ns + "threshold_distance_object_is_on_center", - p->threshold_distance_object_is_on_center); - updateParam( - parameters, ns + "object_check_shiftable_ratio", p->object_check_shiftable_ratio); - updateParam( - parameters, ns + "object_check_min_road_shoulder_width", - p->object_check_min_road_shoulder_width); + parameters, ns + "th_offset_from_centerline", p->threshold_distance_object_is_on_center); + updateParam(parameters, ns + "th_shiftable_ratio", p->object_check_shiftable_ratio); updateParam( - parameters, ns + "object_last_seen_threshold", p->object_last_seen_threshold); + parameters, ns + "min_road_shoulder_width", p->object_check_min_road_shoulder_width); } { @@ -142,9 +134,9 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorclosest_distance_to_wait_and_see_for_ambiguous_vehicle); updateParam( - parameters, ns + "condition.time_threshold", p->time_threshold_for_ambiguous_vehicle); + parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle); updateParam( - parameters, ns + "condition.distance_threshold", p->distance_threshold_for_ambiguous_vehicle); + parameters, ns + "condition.th_moving_distance", p->distance_threshold_for_ambiguous_vehicle); updateParam( parameters, ns + "ignore_area.traffic_light.front_distance", p->object_ignore_section_traffic_light_in_front_distance); @@ -163,14 +155,12 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "th_avoid_execution", p->lateral_execution_threshold); + updateParam(parameters, ns + "th_small_shift_length", p->lateral_small_shift_threshold); updateParam( - parameters, ns + "lateral_execution_threshold", p->lateral_execution_threshold); + parameters, ns + "soft_drivable_bound_margin", p->soft_drivable_bound_margin); updateParam( - parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold); - updateParam( - parameters, ns + "lateral_avoid_check_threshold", p->lateral_avoid_check_threshold); - updateParam(parameters, ns + "soft_road_shoulder_margin", p->soft_road_shoulder_margin); - updateParam(parameters, ns + "hard_road_shoulder_margin", p->hard_road_shoulder_margin); + parameters, ns + "hard_drivable_bound_margin", p->hard_drivable_bound_margin); } { @@ -259,16 +249,16 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector( - parameters, ns + "trim.quantize_filter_threshold", p->quantize_filter_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_1_threshold", p->same_grad_filter_1_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_2_threshold", p->same_grad_filter_2_threshold); - updateParam( - parameters, ns + "trim.same_grad_filter_3_threshold", p->same_grad_filter_3_threshold); - updateParam( - parameters, ns + "trim.sharp_shift_filter_threshold", p->sharp_shift_filter_threshold); + updateParam(parameters, ns + "trim.quantize_size", p->quantize_size); + updateParam(parameters, ns + "trim.th_similar_grad_1", p->th_similar_grad_1); + updateParam(parameters, ns + "trim.th_similar_grad_2", p->th_similar_grad_2); + updateParam(parameters, ns + "trim.th_similar_grad_3", p->th_similar_grad_3); + } + + { + const std::string ns = "avoidance.debug."; + updateParam(parameters, ns + "marker", p->publish_debug_marker); + updateParam(parameters, ns + "console", p->print_debug_info); } std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 33f58c6a08dfc..b16395f825385 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -162,7 +162,7 @@ bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & const bool has_shift_point = !path_shifter_.getShiftLines().empty(); const bool has_base_offset = - std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_execution_threshold; // Nothing to do. -> EXIT. if (!has_shift_point && !has_base_offset) { @@ -734,7 +734,7 @@ bool AvoidanceModule::isSafePath( for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { const auto length = shifted_path.shift_length.at(i); - if (parameters_->lateral_avoid_check_threshold < length) { + if (parameters_->lateral_execution_threshold < length) { return true; } } @@ -746,7 +746,7 @@ bool AvoidanceModule::isSafePath( for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { const auto length = shifted_path.shift_length.at(i); - if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { + if (parameters_->lateral_execution_threshold < -1.0 * length) { return true; } } @@ -1222,7 +1222,7 @@ bool AvoidanceModule::isValidShiftLine( // check if the vehicle is in road. (yaw angle is not considered) { const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + - parameters_->hard_road_shoulder_margin - + parameters_->hard_drivable_bound_margin - parameters_->max_deviation_from_lane; const size_t start_idx = shift_lines.front().start_idx; diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 5a8fd35f6d618..9137048945fa1 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -871,7 +871,7 @@ AvoidLineArray ShiftLineGenerator::applyTrimProcess( // - Combine avoid points that have almost same gradient. // this is to remove the noise. { - const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; + const auto THRESHOLD = parameters_->th_similar_grad_1; applySimilarGradFilter(sl_array_trimmed, THRESHOLD); debug.step3_grad_filtered_1st = sl_array_trimmed; } @@ -879,7 +879,7 @@ AvoidLineArray ShiftLineGenerator::applyTrimProcess( // - Quantize the shift length to reduce the shift point noise // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. { - const auto THRESHOLD = parameters_->quantize_filter_threshold; + const auto THRESHOLD = parameters_->quantize_size; applyQuantizeProcess(sl_array_trimmed, THRESHOLD); debug.step3_quantize_filtered = sl_array_trimmed; } @@ -893,14 +893,14 @@ AvoidLineArray ShiftLineGenerator::applyTrimProcess( // - Combine avoid points that have almost same gradient (again) { - const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; + const auto THRESHOLD = parameters_->th_similar_grad_2; applySimilarGradFilter(sl_array_trimmed, THRESHOLD); debug.step3_grad_filtered_2nd = sl_array_trimmed; } // - Combine avoid points that have almost same gradient (again) { - const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; + const auto THRESHOLD = parameters_->th_similar_grad_3; applySimilarGradFilter(sl_array_trimmed, THRESHOLD); debug.step3_grad_filtered_3rd = sl_array_trimmed; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 6c2b72db0237d..c97d21ca552eb 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -814,9 +814,9 @@ std::optional getAvoidMargin( object_parameter.lateral_soft_margin + 0.5 * vehicle_width; const auto min_avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; const auto soft_lateral_distance_limit = - object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; + object.to_road_shoulder_distance - parameters->soft_drivable_bound_margin - 0.5 * vehicle_width; const auto hard_lateral_distance_limit = - object.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width; + object.to_road_shoulder_distance - parameters->hard_drivable_bound_margin - 0.5 * vehicle_width; // Step1. check avoidable or not. if (hard_lateral_distance_limit < min_avoid_margin) {