From fbd5b6f375675d425a1e760a84a9b1e263431625 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 30 Aug 2023 01:49:14 +0900 Subject: [PATCH] feat(dynamic_avoidance): minimum object polygon longitudinal margin (#4758) * feat(dynamic_avoidance): minimum object polygon longitudinal margin Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../dynamic_avoidance/dynamic_avoidance.param.yaml | 8 ++++++-- .../dynamic_avoidance/dynamic_avoidance_module.hpp | 1 + .../dynamic_avoidance/dynamic_avoidance_module.cpp | 12 ++++++++++-- .../src/scene_module/dynamic_avoidance/manager.cpp | 3 +++ 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 701f05eb89ba1..fe30397683494 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -2,7 +2,7 @@ ros__parameters: dynamic_avoidance: common: - enable_debug_info: true + enable_debug_info: false use_hatched_road_markings: true # avoidance is performed for the object type with true @@ -42,9 +42,13 @@ max_object_angle: 0.785 drivable_area_generation: + polygon_generation_method: "object_path_base" # choose "ego_path_base" and "object_path_base" + object_path_base: + min_longitudinal_polygon_margin: 3.0 # [m] + lat_offset_from_obstacle: 0.8 # [m] max_lat_offset_to_avoid: 0.5 # [m] - max_time_for_object_lat_shift: 2.0 # [s] + max_time_for_object_lat_shift: 0.0 # [s] lpf_gain_for_lat_avoid_to_offset: 0.9 # [-] # for same directional object diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 07ddfca39e5bf..057a19a809484 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -75,6 +75,7 @@ struct DynamicAvoidanceParameters // drivable area generation std::string polygon_generation_method{}; + double min_obj_path_based_lon_polygon_margin{0.0}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; double max_time_for_lat_shift{0.0}; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 7590d1ef71896..22b4f674c8341 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -947,11 +947,19 @@ DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon( // calculate left and right bound std::vector obj_left_bound_points; std::vector obj_right_bound_points; + const double obj_path_length = motion_utils::calcArcLength(obj_path.path); for (size_t i = 0; i < obj_path.path.size(); ++i) { const double lon_offset = [&]() { - if (i == 0) return -object.shape.dimensions.x / 2.0 - parameters_->lat_offset_from_obstacle; + if (i == 0) + return -object.shape.dimensions.x / 2.0 - + std::max( + parameters_->min_obj_path_based_lon_polygon_margin, + parameters_->lat_offset_from_obstacle); if (i == obj_path.path.size() - 1) - return object.shape.dimensions.x / 2.0 + parameters_->lat_offset_from_obstacle; + return object.shape.dimensions.x / 2.0 + + std::max( + parameters_->min_obj_path_based_lon_polygon_margin - obj_path_length, + parameters_->lat_offset_from_obstacle); return 0.0; }(); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 2024384349b48..cdb78d9ea3b37 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -101,6 +101,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "polygon_generation_method", p->polygon_generation_method); + updateParam( + parameters, ns + "object_path_base.min_longitudinal_polygon_margin", + p->min_obj_path_based_lon_polygon_margin); updateParam(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle); updateParam(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid); updateParam(