From fbd0f8e1231c851a49bd72ffbddb26d42c2f39bc Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sun, 17 Mar 2024 14:56:54 +0900 Subject: [PATCH] refactor(avoidance): parameterize Signed-off-by: satoshi-ota --- .../src/manager.cpp | 11 +++++++---- .../config/avoidance.param.yaml | 6 ++++-- .../behavior_path_avoidance_module/data_structs.hpp | 7 ++++--- .../behavior_path_avoidance_module/helper.hpp | 5 +++-- .../parameter_helper.hpp | 13 +++++++------ .../behavior_path_avoidance_module/src/utils.cpp | 6 +++--- 6 files changed, 28 insertions(+), 20 deletions(-) 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 19e03181e1dcb..799364437955c 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 @@ -133,10 +133,13 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + 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"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); 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 10e5c2a05d902..50b5aef92cf60 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -155,8 +155,10 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: enable: false # [-] - time_threshold: 1.0 # [s] - distance_threshold: 1.0 # [m] + closest_distance_to_wait_and_see: 10.0 # [m] + condition: + time_threshold: 1.0 # [s] + distance_threshold: 1.0 # [m] ignore_area: traffic_light: front_distance: 100.0 # [m] 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 a6ee85fbeae5c..ab79b49d67125 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 @@ -100,7 +100,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_force_avoidance_for_stopped_vehicle{false}; + bool enable_avoidance_for_ambiguous_vehicle{false}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -184,8 +184,9 @@ struct AvoidanceParameters double object_check_min_road_shoulder_width{0.0}; // force avoidance - double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; - double force_avoidance_distance_threshold{0.0}; + double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + double time_threshold_for_ambiguous_vehicle{0.0}; + double distance_threshold_for_ambiguous_vehicle{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction 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 c1c1f98e73cb4..6d2eb81b328f4 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 @@ -309,12 +309,13 @@ class AvoidanceHelper const auto desire_shift_length = getShiftLength(object, is_object_on_right, object.avoid_margin.value()); - constexpr double BUFFER = 10.0; const auto prepare_distance = getMinimumPrepareDistance(); const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); - return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + BUFFER; + return object.longitudinal < + prepare_distance + constant_distance + avoidance_distance + + parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; } bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) 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 82f1eddb03c01..d23f5c8729edd 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 @@ -141,12 +141,13 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); - p.force_avoidance_distance_threshold = - getOrDeclareParameter(*node, ns + "distance_threshold"); + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + 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"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); 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/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index ca8c6cfff477d..f83a07bafef48 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -680,7 +680,7 @@ bool isNeverAvoidanceTarget( const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto is_moving_distance_longer_than_threshold = tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > - parameters->force_avoidance_distance_threshold; + parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.reason = AvoidanceDebugFactor::MOVING_OBJECT; return true; @@ -860,13 +860,13 @@ bool isSatisfiedWithVehicleCondition( // from here, filtering for ambiguous vehicle. - if (!parameters->enable_force_avoidance_for_stopped_vehicle) { + if (!parameters->enable_avoidance_for_ambiguous_vehicle) { object.reason = "AmbiguousStoppedVehicle"; return false; } const auto stop_time_longer_than_threshold = - object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; + object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; if (!stop_time_longer_than_threshold) { object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; return false;