feat(avoidance): add use_conservative_buffer_longitudinal

Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Oct 25, 2023 · 3c2bf6a · 3c2bf6a
1 parent 7aed87e
commit 3c2bf6a
Show file tree

Hide file tree

Showing 4 changed files with 40 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
@@ -37,6 +37,7 @@
           avoid_margin_lateral: 1.0                    # [m]
           safety_buffer_lateral: 0.7                   # [m]
           safety_buffer_longitudinal: 0.0              # [m]
+          use_conservative_buffer_longitudinal: true   # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance.
         truck:
           is_target: true
           execute_num: 1
@@ -47,6 +48,7 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
+          use_conservative_buffer_longitudinal: true
         bus:
           is_target: true
           execute_num: 1
@@ -57,6 +59,7 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
+          use_conservative_buffer_longitudinal: true
         trailer:
           is_target: true
           execute_num: 1
@@ -67,6 +70,7 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
+          use_conservative_buffer_longitudinal: true
         unknown:
           is_target: false
           execute_num: 1
@@ -77,6 +81,7 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
+          use_conservative_buffer_longitudinal: true
         bicycle:
           is_target: false
           execute_num: 1
@@ -87,6 +92,7 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
+          use_conservative_buffer_longitudinal: true
         motorcycle:
           is_target: false
           execute_num: 1
@@ -97,6 +103,7 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
+          use_conservative_buffer_longitudinal: true
         pedestrian:
           is_target: false
           execute_num: 1
@@ -107,6 +114,7 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
+          use_conservative_buffer_longitudinal: true
         lower_distance_for_polygon_expansion: 30.0      # [m]
         upper_distance_for_polygon_expansion: 100.0     # [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
@@ -69,6 +69,8 @@ struct ObjectParameter
   double safety_buffer_lateral{1.0};
 
   double safety_buffer_longitudinal{0.0};
+
+  bool use_conservative_buffer_longitudinal{true};
 };
 
 struct AvoidanceParameters

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
@@ -542,7 +542,10 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
   const auto o_front = data.stop_target_object.get();
   const auto object_type = utils::getHighestProbLabel(o_front.object.classification);
   const auto object_parameter = parameters_->object_parameters.at(object_type);
-  const auto & base_link2front = planner_data_->parameters.base_link2front;
+  const auto & additional_buffer_longitudinal =
+    object_parameter.use_conservative_buffer_longitudinal
+      ? planner_data_->parameters.base_link2front
+      : 0.0;
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
   const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor +
@@ -551,7 +554,8 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
   const auto variable = helper_.getSharpAvoidanceDistance(
     helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
   const auto constant = helper_.getNominalPrepareDistance() +
-                        object_parameter.safety_buffer_longitudinal + base_link2front;
+                        object_parameter.safety_buffer_longitudinal +
+                        additional_buffer_longitudinal;
   const auto total_avoid_distance = variable + constant;
 
   dead_pose_ = calcLongitudinalOffsetPose(
@@ -796,7 +800,6 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
 {
   // To be consistent with changes in the ego position, the current shift length is considered.
   const auto current_ego_shift = helper_.getEgoShift();
-  const auto & base_link2front = planner_data_->parameters.base_link2front;
   const auto & base_link2rear = planner_data_->parameters.base_link2rear;
 
   // Calculate feasible shift length
@@ -834,8 +837,12 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
 
     // calculate remaining distance.
     const auto prepare_distance = helper_.getNominalPrepareDistance();
-    const auto constant =
-      object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance;
+    const auto & additional_buffer_longitudinal =
+      object_parameter.use_conservative_buffer_longitudinal
+        ? planner_data_->parameters.base_link2front
+        : 0.0;
+    const auto constant = object_parameter.safety_buffer_longitudinal +
+                          additional_buffer_longitudinal + prepare_distance;
     const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
     const auto remaining_distance = object.longitudinal - constant;
 
@@ -955,7 +962,12 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
 
     AvoidLine al_avoid;
     {
-      const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front;
+      const auto & additional_buffer_longitudinal =
+        object_parameter.use_conservative_buffer_longitudinal
+          ? planner_data_->parameters.base_link2front
+          : 0.0;
+      const auto offset =
+        object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
       const auto path_front_to_ego =
         avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index);
 
@@ -2727,7 +2739,6 @@ void AvoidanceModule::updateAvoidanceDebugData(
 double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
 {
   const auto & p = parameters_;
-  const auto & base_link2front = planner_data_->parameters.base_link2front;
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
   //         D5
@@ -2743,7 +2754,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
   // D2: min_avoid_distance
   // D3: longitudinal_avoid_margin_front (margin + D5)
   // D4: o_front.longitudinal
-  // D5: base_link2front
+  // D5: additional_buffer_longitudinal (base_link2front or 0 depending on the
+  // use_conservative_buffer_longitudinal)
 
   const auto object_type = utils::getHighestProbLabel(object.object.classification);
   const auto object_parameter = parameters_->object_parameters.at(object_type);
@@ -2752,8 +2764,12 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
                             object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
   const auto variable = helper_.getMinAvoidanceDistance(
     helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
+  const auto & additional_buffer_longitudinal =
+    object_parameter.use_conservative_buffer_longitudinal
+      ? planner_data_->parameters.base_link2front
+      : 0.0;
   const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal +
-                        base_link2front + p->stop_buffer;
+                        additional_buffer_longitudinal + p->stop_buffer;
 
   return object.longitudinal - std::min(variable + constant, p->stop_max_distance);
 }

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp
@@ -79,6 +79,8 @@ AvoidanceModuleManager::AvoidanceModuleManager(
         getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
       param.safety_buffer_longitudinal =
         getOrDeclareParameter<double>(*node, ns + "safety_buffer_longitudinal");
+      param.use_conservative_buffer_longitudinal =
+        getOrDeclareParameter<bool>(*node, ns + "use_conservative_buffer_longitudinal");
       return param;
     };
 
@@ -312,6 +314,9 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     updateParam<double>(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral);
     updateParam<double>(
       parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal);
+    updateParam<bool>(
+      parameters, ns + "use_conservative_buffer_longitudinal",
+      config.use_conservative_buffer_longitudinal);
   };
 
   {