From 982263a78e1079f054b428818636de1287801109 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 28 Mar 2022 19:44:24 +0900 Subject: [PATCH] feat(behavior_path_planner): parametrize avoidance target type (#574) * parametrize avoidance target type Signed-off-by: Takamasa Horibe * add target type parameter in yaml Signed-off-by: Takamasa Horibe * mototbike -> motorcycle Signed-off-by: Takamasa Horibe * apply clang Signed-off-by: Takamasa Horibe --- .../avoidance/avoidance.param.yaml | 11 +++++++++++ .../config/avoidance/avoidance.param.yaml | 11 +++++++++++ .../scene_module/avoidance/avoidance_module_data.hpp | 12 ++++++++++++ .../src/behavior_path_planner_node.cpp | 9 +++++++++ .../src/scene_module/avoidance/avoidance_module.cpp | 10 ++++++++-- 5 files changed, 51 insertions(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 3e799bea132ab..d691228589bfa 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -46,5 +46,16 @@ longitudinal_collision_margin_min_distance: 0.0 # [m] longitudinal_collision_margin_time: 0.0 + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: false + pedestrian: false + # ---------- advanced parameters ---------- avoidance_execution_lateral_threshold: 0.499 diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 5b2ec3d3b7b27..63f3e1438cdd5 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -46,5 +46,16 @@ longitudinal_collision_margin_min_distance: 0.0 # [m] longitudinal_collision_margin_time: 0.0 + # avoidance is performed for the object type with true + target_object: + car: true + truck: true + bus: true + trailer: true + unknown: false + bicycle: false + motorcycle: false + pedestrian: false + # ---------- advanced parameters ---------- avoidance_execution_lateral_threshold: 0.499 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 3c01937543627..e335540ee004f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -146,6 +146,18 @@ struct AvoidanceParameters // avoidance path will be generated unless their lateral margin difference exceeds this value. double avoidance_execution_lateral_threshold; + // true by default + bool avoid_car{true}; // avoidance is performed for type object car + bool avoid_truck{true}; // avoidance is performed for type object truck + bool avoid_bus{true}; // avoidance is performed for type object bus + bool avoid_trailer{true}; // avoidance is performed for type object trailer + + // false by default + bool avoid_unknown{false}; // avoidance is performed for type object unknown + bool avoid_bicycle{false}; // avoidance is performed for type object bicycle + bool avoid_motorcycle{false}; // avoidance is performed for type object motorbike + bool avoid_pedestrian{false}; // avoidance is performed for type object pedestrian + // debug bool publish_debug_marker = false; bool print_debug_info = false; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 0d25f25988aa0..96ed094d31a6f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -252,6 +252,15 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.publish_debug_marker = dp("publish_debug_marker", false); p.print_debug_info = dp("print_debug_info", false); + p.avoid_car = dp("target_object.car", true); + p.avoid_truck = dp("target_object.truck", true); + p.avoid_bus = dp("target_object.bus", true); + p.avoid_trailer = dp("target_object.trailer", true); + p.avoid_unknown = dp("target_object.unknown", false); + p.avoid_bicycle = dp("target_object.bicycle", false); + p.avoid_motorcycle = dp("target_object.motorcycle", false); + p.avoid_pedestrian = dp("target_object.pedestrian", false); + p.avoidance_execution_lateral_threshold = dp("avoidance_execution_lateral_threshold", 0.499); return p; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index d9239591149de..eb972783422de 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2528,8 +2528,14 @@ bool AvoidanceModule::isTargetObjectType(const PredictedObject & object) const using autoware_auto_perception_msgs::msg::ObjectClassification; const auto t = util::getHighestProbLabel(object.classification); const auto is_object_type = - (t == ObjectClassification::CAR || t == ObjectClassification::TRUCK || - t == ObjectClassification::BUS); + ((t == ObjectClassification::CAR && parameters_.avoid_car) || + (t == ObjectClassification::TRUCK && parameters_.avoid_truck) || + (t == ObjectClassification::BUS && parameters_.avoid_bus) || + (t == ObjectClassification::TRAILER && parameters_.avoid_trailer) || + (t == ObjectClassification::UNKNOWN && parameters_.avoid_unknown) || + (t == ObjectClassification::BICYCLE && parameters_.avoid_bicycle) || + (t == ObjectClassification::MOTORCYCLE && parameters_.avoid_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && parameters_.avoid_pedestrian)); return is_object_type; }