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 a3b28b4d63d06..78fc31eca304e 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 @@ -135,9 +135,11 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - 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.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index 324be9a73439e..2bd04f24d24a6 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -134,8 +134,11 @@ # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. avoidance_for_ambiguous_vehicle: - enable: true # [-] - closest_distance_to_wait_and_see: 10.0 # [m] + # policy for ego behavior for ambiguous vehicle. + # "auto" : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically. + # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode. + # "ignore" : never avoid it. + policy: "auto" # [-] condition: th_stopped_time: 3.0 # [s] th_moving_distance: 1.0 # [m] @@ -145,6 +148,9 @@ crosswalk: front_distance: 30.0 # [m] behind_distance: 30.0 # [m] + wait_and_see: + target_behaviors: ["MERGING", "DEVIATING"] # [-] + th_closest_distance: 10.0 # [m] # params for filtering objects that are in intersection intersection: 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 e67ab1c9af12e..cd35454b47f3a 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 @@ -112,7 +112,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_avoidance_for_ambiguous_vehicle{false}; + std::string policy_ambiguous_vehicle{"ignore"}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -196,7 +196,8 @@ struct AvoidanceParameters double object_check_min_road_shoulder_width{0.0}; // force avoidance - double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + std::vector wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"}; + double wait_and_see_th_closest_distance{0.0}; double time_threshold_for_ambiguous_vehicle{0.0}; double distance_threshold_for_ambiguous_vehicle{0.0}; 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 0e6592c6bdefd..ba07bd6bcad06 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 @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -316,10 +317,27 @@ class AvoidanceHelper const auto object = objects.front(); + // if the object is NOT ambiguous, this module doesn't wait operator approval if RTC is running + // as AUTO mode. if (!object.is_ambiguous) { return true; } + // check only front objects. + if (object.longitudinal < 0.0) { + return true; + } + + // if the policy is "manual", this module generates candidate path and waits approval. + if (parameters_->policy_ambiguous_vehicle == "manual") { + return false; + } + + // don't delay avoidance start position if it's not MERGING or DEVIATING vehicle. + if (!isWaitAndSeeTarget(object)) { + return true; + } + if (!object.avoid_margin.has_value()) { return true; } @@ -332,9 +350,32 @@ class AvoidanceHelper const auto constant_distance = getFrontConstantDistance(object); const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); - return object.longitudinal < - prepare_distance + constant_distance + avoidance_distance + - parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + + parameters_->wait_and_see_th_closest_distance; + } + + bool isWaitAndSeeTarget(const ObjectData & object) const + { + const auto & behaviors = parameters_->wait_and_see_target_behaviors; + if (object.behavior == ObjectData::Behavior::MERGING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "MERGING"; + }); + } + + if (object.behavior == ObjectData::Behavior::DEVIATING) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "DEVIATING"; + }); + } + + if (object.behavior == ObjectData::Behavior::NONE) { + return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) { + return behavior == "NONE"; + }); + } + + return false; } static bool isAbsolutelyNotAvoidable(const ObjectData & object) 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 1791d72764bbf..964d1e08cd010 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 @@ -138,9 +138,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node) { const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; - 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.policy_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "policy"); + p.wait_and_see_target_behaviors = + getOrDeclareParameter>(*node, ns + "wait_and_see.target_behaviors"); + p.wait_and_see_th_closest_distance = + getOrDeclareParameter(*node, ns + "wait_and_see.th_closest_distance"); p.time_threshold_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "condition.th_stopped_time"); p.distance_threshold_for_ambiguous_vehicle = diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json index ab032ed1c27a5..673b4a58bb80b 100644 --- a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json +++ b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json @@ -719,9 +719,10 @@ "avoidance_for_ambiguous_vehicle": { "type": "object", "properties": { - "enable": { - "type": "boolean", - "description": "Enable avoidance maneuver for ambiguous vehicles.", + "policy": { + "type": "string", + "enum": ["auto", "manual", "ignore"], + "description": "Ego behavior policy for ambiguous vehicle.", "default": "true" }, "closest_distance_to_wait_and_see": { @@ -781,14 +782,26 @@ }, "required": ["traffic_light", "crosswalk"], "additionalProperties": false + }, + "wait_and_see": { + "type": "object", + "properties": { + "target_behaviors": { + "type": "array", + "default": ["MERGING", "DEVIATING"], + "description": "This module doesn't avoid these behaviors vehicle until it gets closer than threshold." + }, + "th_closest_distance": { + "type": "number", + "default": 10.0, + "description": "Threshold to check whether the ego gets close enough the ambiguous vehicle." + } + }, + "required": ["target_behaviors", "th_closest_distance"], + "additionalProperties": false } }, - "required": [ - "enable", - "closest_distance_to_wait_and_see", - "condition", - "ignore_area" - ], + "required": ["policy", "condition", "ignore_area", "wait_and_see"], "additionalProperties": false }, "intersection": { diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index bdf6f4e822f7b..d96ca79f4a261 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -131,10 +131,9 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); + updateParam(parameters, ns + "policy", p->policy_ambiguous_vehicle); updateParam( - parameters, ns + "closest_distance_to_wait_and_see", - p->closest_distance_to_wait_and_see_for_ambiguous_vehicle); + parameters, ns + "wait_and_see.th_closest_distance", p->wait_and_see_th_closest_distance); updateParam( parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle); updateParam( diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 2447f6396004c..c4ea0f879bbfc 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -879,7 +879,7 @@ bool isSatisfiedWithVehicleCondition( // from here, filtering for ambiguous vehicle. - if (!parameters->enable_avoidance_for_ambiguous_vehicle) { + if (parameters->policy_ambiguous_vehicle == "ignore") { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; return false; } @@ -888,6 +888,7 @@ bool isSatisfiedWithVehicleCondition( object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; if (!stop_time_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -896,6 +897,7 @@ bool isSatisfiedWithVehicleCondition( parameters->distance_threshold_for_ambiguous_vehicle; if (is_moving_distance_longer_than_threshold) { object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = false; return false; } @@ -906,22 +908,9 @@ bool isSatisfiedWithVehicleCondition( return true; } } else { - if (object.behavior == ObjectData::Behavior::MERGING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::DEVIATING) { - object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; - object.is_ambiguous = true; - return true; - } - - if (object.behavior == ObjectData::Behavior::NONE) { - object.is_ambiguous = false; - return true; - } + object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE; + object.is_ambiguous = true; + return true; } object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;