From ec480fdb881d99402287e8f8d5fde41e3529309f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 28 Nov 2023 19:29:22 +0900 Subject: [PATCH] feat(avoidance): configurable object type for safety check (#5699) Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 28 ++++++++++----- .../avoidance_by_lc.param.yaml | 21 +++++++----- .../utils/avoidance/avoidance_module_data.hpp | 4 ++- .../src/scene_module/avoidance/manager.cpp | 34 +++++++++++++++++-- .../src/scene_module/lane_change/manager.cpp | 17 +++++++++- .../src/utils/avoidance/utils.cpp | 24 ++++++++++--- 6 files changed, 103 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index c6072e8a50ce3..20bdc7a064452 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -30,7 +30,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] @@ -40,7 +39,6 @@ safety_buffer_lateral: 0.7 # [m] safety_buffer_longitudinal: 0.0 # [m] truck: - is_target: true execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 @@ -50,7 +48,6 @@ safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bus: - is_target: true execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 @@ -60,7 +57,6 @@ safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 trailer: - is_target: true execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 @@ -70,7 +66,6 @@ safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 unknown: - is_target: false execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 @@ -80,7 +75,6 @@ safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bicycle: - is_target: false execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 @@ -90,7 +84,6 @@ safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 motorcycle: - is_target: false execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 @@ -100,7 +93,6 @@ safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 pedestrian: - is_target: false execute_num: 1 moving_speed_threshold: 1.0 moving_time_threshold: 1.0 @@ -114,6 +106,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 # [-] # params for avoidance of not-parked objects threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] object_ignore_section_traffic_light_in_front_distance: 30.0 # [m] @@ -132,6 +134,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 ec0ec4696a44d..84d3baa7cf155 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 @@ -52,7 +52,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 14f98d026c493..115d6ed1d446f 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -80,7 +80,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = get_parameter(node, ns + "is_target"); param.execute_num = get_parameter(node, ns + "execute_num"); param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); @@ -113,7 +112,23 @@ 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 = get_parameter(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.threshold_time_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); p.object_ignore_section_traffic_light_in_front_distance = @@ -138,7 +153,23 @@ 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 = get_parameter(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 = get_parameter(node, ns + "enable"); p.check_current_lane = get_parameter(node, ns + "check_current_lane"); p.check_shift_side_lane = get_parameter(node, ns + "check_shift_side_lane"); @@ -303,7 +334,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 7692cd624bffd..920a7dd9d7327 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 @@ -237,7 +237,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = get_parameter(node, ns + "is_target"); param.execute_num = get_parameter(node, ns + "execute_num"); param.moving_speed_threshold = get_parameter(node, ns + "moving_speed_threshold"); param.moving_time_threshold = get_parameter(node, ns + "moving_time_threshold"); @@ -268,7 +267,23 @@ 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 = get_parameter(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.threshold_time_force_avoidance_for_stopped_vehicle = get_parameter(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); p.object_ignore_section_traffic_light_in_front_distance = diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index cdd48d5b1509c..db729b0c502a3 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -76,7 +76,7 @@ bool isOnRight(const ObjectData & obj) return obj.lateral < 0.0; } -bool isTargetObjectType( +bool isAvoidanceTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters) { const auto object_type = utils::getHighestProbLabel(object.classification); @@ -85,7 +85,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) @@ -854,7 +866,7 @@ void filterTargetObjects( const auto object_type = utils::getHighestProbLabel(o.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); - if (!isTargetObjectType(o.object, parameters)) { + if (!isAvoidanceTargetObjectType(o.object, parameters)) { o.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; data.other_objects.push_back(o); continue; @@ -1458,8 +1470,10 @@ std::vector getSafetyCheckTargetObjects( const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - if (isCentroidWithinLanelets(object.object, check_lanes)) { - target_objects.push_back(utils::avoidance::transform(object.object, p)); + if (isSafetyCheckTargetObjectType(object.object, parameters)) { + if (isCentroidWithinLanelets(object.object, check_lanes)) { + target_objects.push_back(utils::avoidance::transform(object.object, p)); + } } }); };