refactor(avoidance): add helper function

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -17,6 +17,7 @@
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/utils.hpp"
+#include "behavior_path_planner_common/utils/utils.hpp"
 
 #include <motion_utils/distance/distance.hpp>
 
@@ -134,6 +135,24 @@ class AvoidanceHelper
       shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed());
   }
 
+  double getFrontConstantDistance(const ObjectData & object) const
+  {
+    const auto object_type = utils::getHighestProbLabel(object.object.classification);
+    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    const auto & additional_buffer_longitudinal =
+      object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
+                                                            : 0.0;
+    return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
+  }
+
+  double getRearConstantDistance(const ObjectData & object) const
+  {
+    const auto object_type = utils::getHighestProbLabel(object.object.classification);
+    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear +
+           object.length;
+  }
+
   double getEgoShift() const
   {
     validate();

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -638,21 +638,16 @@ void AvoidanceModule::fillDebugData(
   const auto o_front = data.stop_target_object.value();
   const auto object_type = utils::getHighestProbLabel(o_front.object.classification);
   const auto object_parameter = parameters_->object_parameters.at(object_type);
-  const auto & additional_buffer_longitudinal =
-    object_parameter.use_conservative_buffer_longitudinal
-      ? planner_data_->parameters.base_link2front
-      : 0.0;
+  const auto constant_distance = helper_->getFrontConstantDistance(o_front);
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
   const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor +
                                 object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
 
-  const auto variable = helper_->getSharpAvoidanceDistance(
+  const auto avoidance_distance = helper_->getSharpAvoidanceDistance(
     helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
-  const auto constant = helper_->getNominalPrepareDistance() +
-                        object_parameter.safety_buffer_longitudinal +
-                        additional_buffer_longitudinal;
-  const auto total_avoid_distance = variable + constant;
+  const auto prepare_distance = helper_->getNominalPrepareDistance();
+  const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance;
 
   dead_pose_ = calcLongitudinalOffsetPose(
     data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance);
@@ -1434,16 +1429,14 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
 
   const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
                             object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
-  const auto variable = helper_->getMinAvoidanceDistance(
+  const auto avoidance_distance = 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 +
-                        additional_buffer_longitudinal + p->stop_buffer;
-
-  return object.longitudinal - std::min(variable + constant, p->stop_max_distance);
+  const auto constant_distance = helper_->getFrontConstantDistance(object);
+
+  return object.longitudinal -
+         std::min(
+           avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer,
+           p->stop_max_distance);
 }
 
 void AvoidanceModule::insertReturnDeadLine(

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

@@ -123,7 +123,6 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 {
   // 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_link2rear = data_->parameters.base_link2rear;
 
   // Calculate feasible shift length
   const auto get_shift_profile =
@@ -140,13 +139,10 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 
     // calculate remaining distance.
     const auto prepare_distance = helper_->getNominalPrepareDistance();
-    const auto & additional_buffer_longitudinal =
-      object_parameter.use_conservative_buffer_longitudinal ? 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;
+    const auto constant_distance = helper_->getFrontConstantDistance(object);
+    const auto has_enough_distance =
+      object.longitudinal > constant_distance + prepare_distance + nominal_avoid_distance;
+    const auto remaining_distance = object.longitudinal - constant_distance - prepare_distance;
     const auto avoidance_distance =
       has_enough_distance ? nominal_avoid_distance : remaining_distance;
 
@@ -278,11 +274,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       }
     }
 
-    // use each object param
-    const auto object_type = utils::getHighestProbLabel(o.object.classification);
-    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    // calculate feasible shift length based on behavior policy
     const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
-
     if (!feasible_shift_profile.has_value()) {
       if (o.avoid_required && is_forward_object(o)) {
         break;
@@ -297,12 +290,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 
     AvoidLine al_avoid;
     {
-      const auto & additional_buffer_longitudinal =
-        object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
-                                                              : 0.0;
-      const auto offset =
-        object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
-      const auto to_shift_end = o.longitudinal - offset;
+      const auto constant_distance = helper_->getFrontConstantDistance(o);
+      const auto to_shift_end = o.longitudinal - constant_distance;
       const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index);
 
       // start point (use previous linear shift length as start shift length.)
@@ -338,8 +327,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 
     AvoidLine al_return;
     {
-      const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length;
-      const auto to_shift_start = o.longitudinal + offset;
+      const auto constant_distance = helper_->getRearConstantDistance(o);
+      const auto to_shift_start = o.longitudinal + constant_distance;
 
       // start point
       al_return.start_shift_length = feasible_shift_profile.value().first;