diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index d3032ffdd3b99..27f6a0df63cec 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -27,7 +27,6 @@ # avoidance is performed for the object type with true target_object: car: - is_target: true # [-] execute_num: 1 # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] @@ -38,7 +37,6 @@ 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. truck: - is_target: true execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -49,7 +47,6 @@ safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bus: - is_target: true execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -60,7 +57,6 @@ safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true trailer: - is_target: true execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -71,7 +67,6 @@ safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true unknown: - is_target: true execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -82,7 +77,6 @@ safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bicycle: - is_target: true execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -93,7 +87,6 @@ safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true motorcycle: - is_target: true execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -104,7 +97,6 @@ safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true pedestrian: - is_target: true execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -119,6 +111,16 @@ # 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 # [-] # detection range object_check_goal_distance: 20.0 # [m] # filtering parking objects @@ -152,6 +154,16 @@ # 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 # [-] # safety check configuration enable: true # [-] check_current_lane: false # [-] diff --git a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml b/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml index 5ea50f90ab62c..9518185d30d63 100644 --- a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml +++ b/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml @@ -7,7 +7,6 @@ # avoidance is performed for the object type with true target_object: car: - is_target: true # [-] execute_num: 2 # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] @@ -16,7 +15,6 @@ avoid_margin_lateral: 1.0 # [m] safety_buffer_lateral: 0.7 # [m] truck: - is_target: true execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -25,7 +23,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 bus: - is_target: true execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -34,7 +31,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 trailer: - is_target: true execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -43,7 +39,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 unknown: - is_target: true execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -52,7 +47,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.0 bicycle: - is_target: true execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -61,7 +55,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 motorcycle: - is_target: true execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -70,7 +63,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 pedestrian: - is_target: true execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -80,3 +72,16 @@ safety_buffer_lateral: 1.0 lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] + + # For target object filtering + target_filtering: + # avoidance target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: true # [-] + bicycle: false # [-] + motorcycle: false # [-] + pedestrian: false # [-] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 6c223bff39ef0..191c3acf1bdf8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -54,7 +54,9 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; struct ObjectParameter { - bool is_target{false}; + bool is_avoidance_target{false}; + + bool is_safety_check_target{false}; size_t execute_num{1}; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index f4807223503fa..d82a105930b99 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -65,7 +65,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = getOrDeclareParameter(*node, ns + "is_target"); param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "moving_speed_threshold"); @@ -105,7 +104,24 @@ AvoidanceModuleManager::AvoidanceModuleManager( // target filtering { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -147,7 +163,24 @@ AvoidanceModuleManager::AvoidanceModuleManager( // safety check general params { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_safety_check_target = + getOrDeclareParameter(*node, ns); + }; + std::string ns = "avoidance.safety_check."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); @@ -346,7 +379,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorobject_parameters.at(semantic); - updateParam(parameters, ns + "is_target", config.is_target); updateParam(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold); updateParam(parameters, ns + "moving_time_threshold", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 3a0b7527e4184..56410dd8ba4ca 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -285,7 +285,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = getOrDeclareParameter(*node, ns + "is_target"); param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "moving_speed_threshold"); @@ -321,7 +320,24 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // target filtering { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index cee60f08ca79e..f47ee90d0ad52 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -235,7 +235,7 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) namespace filtering_utils { -bool isTargetObjectType( +bool isAvoidanceTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters) { const auto object_type = utils::getHighestProbLabel(object.classification); @@ -244,7 +244,19 @@ bool isTargetObjectType( return false; } - return parameters->object_parameters.at(object_type).is_target; + return parameters->object_parameters.at(object_type).is_avoidance_target; +} + +bool isSafetyCheckTargetObjectType( + const PredictedObject & object, const std::shared_ptr & parameters) +{ + const auto object_type = utils::getHighestProbLabel(object.classification); + + if (parameters->object_parameters.count(object_type) == 0) { + return false; + } + + return parameters->object_parameters.at(object_type).is_safety_check_target; } bool isVehicleTypeObject(const ObjectData & object) @@ -500,7 +512,7 @@ bool isSatisfiedWithCommonCondition( const std::shared_ptr & parameters) { // Step1. filtered by target object type. - if (!isTargetObjectType(object.object, parameters)) { + if (!isAvoidanceTargetObjectType(object.object, parameters)) { object.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; return false; } @@ -1705,10 +1717,12 @@ std::vector getSafetyCheckTargetObjects( }); }; - const auto to_predicted_objects = [&p](const auto & objects) { + const auto to_predicted_objects = [&p, ¶meters](const auto & objects) { PredictedObjects ret{}; - std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) { - ret.objects.push_back(object.object); + std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](const auto & object) { + if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) { + ret.objects.push_back(object.object); + } }); return ret; };