feat(dynamic_avoidance): minimum object polygon longitudinal margin (a…

…utowarefoundation#4758) * feat(dynamic_avoidance): minimum object polygon longitudinal margin Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * update config Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> --------- Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
tier4 · Aug 30, 2023 · cc71c7f · cc71c7f
1 parent 6e07430
commit cc71c7f
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 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
@@ -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/...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/...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/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -766,6 +766,10 @@ DynamicAvoidanceParameters BehaviorPathPlannerNode::getDynamicAvoidanceParam()
 
   {  // drivable_area_generation
     std::string ns = "dynamic_avoidance.drivable_area_generation.";
+    p.polygon_generation_method =
+      declare_parameter<std::string>(ns + "polygon_generation_method");
+    p.min_obj_path_based_lon_polygon_margin =
+      declare_parameter<double>(ns + "object_path_base.min_longitudinal_polygon_margin");
     p.lat_offset_from_obstacle = declare_parameter<double>(ns + "lat_offset_from_obstacle");
     p.max_lat_offset_to_avoid = declare_parameter<double>(ns + "max_lat_offset_to_avoid");
     p.max_time_for_lat_shift =

diff --git a/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -963,11 +963,19 @@ DynamicAvoidanceModule::calcObjectPathBasedDynamicObstaclePolygon(
   // calculate left and right bound
   std::vector<geometry_msgs::msg::Point> obj_left_bound_points;
   std::vector<geometry_msgs::msg::Point> 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
@@ -102,6 +102,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
 
     updateParam<std::string>(
       parameters, ns + "polygon_generation_method", p->polygon_generation_method);
+    updateParam<double>(
+      parameters, ns + "object_path_base.min_longitudinal_polygon_margin",
+      p->min_obj_path_based_lon_polygon_margin);
     updateParam<double>(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle);
     updateParam<double>(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid);
     updateParam<double>(