feat(avoidance): set additional buffer margin independently

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Jun 22, 2023 · d7bbc4f · d7bbc4f
1 parent f6f3bab
commit d7bbc4f
Show file tree

Hide file tree

Showing 6 changed files with 17 additions and 9 deletions.
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -33,6 +33,7 @@
           moving_time_threshold: 1.0                   # [s]
           max_expand_ratio: 0.0                        # [-]
           envelope_buffer_margin: 0.3                  # [m]
+          additional_buffer_lateral: 1.0               # [m]
           safety_buffer_lateral: 0.7                   # [m]
           safety_buffer_longitudinal: 0.0              # [m]
         truck:
@@ -41,6 +42,7 @@
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
+          additional_buffer_lateral: 1.0
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         bus:
@@ -49,6 +51,7 @@
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
+          additional_buffer_lateral: 1.0
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         trailer:
@@ -57,6 +60,7 @@
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
+          additional_buffer_lateral: 1.0
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         unknown:
@@ -65,6 +69,7 @@
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
+          additional_buffer_lateral: 1.0
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         bicycle:
@@ -73,6 +78,7 @@
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
+          additional_buffer_lateral: 1.0
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
         motorcycle:
@@ -81,6 +87,7 @@
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
+          additional_buffer_lateral: 1.0
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
         pedestrian:
@@ -89,6 +96,7 @@
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
+          additional_buffer_lateral: 1.0
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
         lower_distance_for_polygon_expansion: 30.0      # [m]
@@ -124,7 +132,6 @@
       avoidance:
         # avoidance lateral parameters
         lateral:
-          lateral_collision_margin: 1.0                 # [m]
           lateral_execution_threshold: 0.499            # [m]
           lateral_small_shift_threshold: 0.101          # [m]
           road_shoulder_safety_margin: 0.3              # [m]

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp
@@ -58,6 +58,8 @@ struct ObjectParameter
 
   double envelope_buffer_margin{0.0};
 
+  double additional_buffer_lateral{1.0};
+
   double safety_buffer_lateral{1.0};
 
   double safety_buffer_longitudinal{0.0};
@@ -159,9 +161,6 @@ struct AvoidanceParameters
   // force avoidance
   double threshold_time_force_avoidance_for_stopped_vehicle;
 
-  // we want to keep this lateral margin when avoiding
-  double lateral_collision_margin;
-
   // when complete avoidance motion, there is a distance margin with the object
   // for longitudinal direction
   double longitudinal_collision_margin_min_distance;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -584,6 +584,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
       param.moving_time_threshold = declare_parameter<double>(ns + "moving_time_threshold");
       param.max_expand_ratio = declare_parameter<double>(ns + "max_expand_ratio");
       param.envelope_buffer_margin = declare_parameter<double>(ns + "envelope_buffer_margin");
+      param.additional_buffer_lateral = declare_parameter<double>(ns + "additional_buffer_lateral");
       param.safety_buffer_lateral = declare_parameter<double>(ns + "safety_buffer_lateral");
       param.safety_buffer_longitudinal =
         declare_parameter<double>(ns + "safety_buffer_longitudinal");
@@ -647,7 +648,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   // avoidance maneuver (lateral)
   {
     std::string ns = "avoidance.avoidance.lateral.";
-    p.lateral_collision_margin = declare_parameter<double>(ns + "lateral_collision_margin");
     p.road_shoulder_safety_margin = declare_parameter<double>(ns + "road_shoulder_safety_margin");
     p.lateral_execution_threshold = declare_parameter<double>(ns + "lateral_execution_threshold");
     p.lateral_small_shift_threshold =

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
@@ -628,7 +628,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
   const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor +
-                                parameters_->lateral_collision_margin + 0.5 * vehicle_width;
+                                object_parameter.additional_buffer_lateral + 0.5 * vehicle_width;
 
   const auto variable = helper_.getSharpAvoidanceDistance(
     helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
@@ -3290,7 +3290,7 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
   const auto object_parameter = parameters_->object_parameters.at(t);
 
   const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
-                            p->lateral_collision_margin + 0.5 * vehicle_width;
+                            object_parameter.additional_buffer_lateral + 0.5 * vehicle_width;
   const auto variable = helper_.getMinimumAvoidanceDistance(
     helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
   const auto constant =

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
@@ -52,6 +52,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     updateParam<double>(parameters, ns + "moving_time_threshold", config.moving_time_threshold);
     updateParam<double>(parameters, ns + "max_expand_ratio", config.max_expand_ratio);
     updateParam<double>(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin);
+    updateParam<double>(
+      parameters, ns + "additional_buffer_lateral", config.additional_buffer_lateral);
     updateParam<double>(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral);
     updateParam<double>(
       parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal);
@@ -82,7 +84,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
       parameters, ns + "lateral_execution_threshold", p->lateral_execution_threshold);
     updateParam<double>(
       parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold);
-    updateParam<double>(parameters, ns + "lateral_collision_margin", p->lateral_collision_margin);
     updateParam<double>(
       parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin);
   }

diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp
@@ -845,7 +845,8 @@ void filterTargetObjects(
     // calculate avoid_margin dynamically
     // NOTE: This calculation must be after calculating to_road_shoulder_distance.
     const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor +
-                                    parameters->lateral_collision_margin + 0.5 * vehicle_width;
+                                    object_parameter.additional_buffer_lateral +
+                                    0.5 * vehicle_width;
     const double min_safety_lateral_distance =
       object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
     const auto max_allowable_lateral_distance =