fix(avoidance): consider avoidance prepare distance

Signed-off-by: satoshi-ota <[email protected]>

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -907,8 +907,8 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
   const auto & base_link2rear = planner_data_->parameters.base_link2rear;
 
   // Calculate feasible shift length
-  const auto get_shift_length =
-    [&](auto & object, const auto & desire_shift_length) -> boost::optional<double> {
+  const auto get_shift_profile = [&](auto & object, const auto & desire_shift_length)
+    -> boost::optional<std::pair<double, double>> {
     // use each object param
     const auto object_type = utils::getHighestProbLabel(object.object.classification);
     const auto object_parameter = parameters_->object_parameters.at(object_type);
@@ -918,44 +918,51 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
     const auto avoiding_shift = desire_shift_length - current_ego_shift;
     const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift);
 
+    // calculate remaining distance.
+    const auto prepare_distance = helper_.getNominalPrepareDistance();
+    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;
+    const auto avoidance_distance =
+      has_enough_distance ? nominal_avoid_distance : remaining_distance;
+
+    // nominal case. avoidable.
+    if (has_enough_distance) {
+      return std::make_pair(desire_shift_length, avoidance_distance);
+    }
+
     if (!isBestEffort(parameters_->policy_lateral_margin)) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // ego already has enough positive shift.
     const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3;
     if (is_object_on_right && has_enough_positive_shift) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // ego already has enough negative shift.
     const auto has_enough_negative_shift = avoiding_shift > 1e-3 && desire_shift_length < -1e-3;
     if (!is_object_on_right && has_enough_negative_shift) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // don't relax shift length since it can stop in front of the object.
     if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
-    // calculate remaining distance.
-    const auto prepare_distance = helper_.getNominalPrepareDistance();
-    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;
-
     // the avoidance path is already approved
     const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
     const auto is_approved = (helper_.getShift(object_pos) > 0.0 && is_object_on_right) ||
                              (helper_.getShift(object_pos) < 0.0 && !is_object_on_right);
     if (is_approved) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // prepare distance is not enough. unavoidable.
@@ -964,18 +971,13 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
       return boost::none;
     }
 
-    // nominal case. avoidable.
-    if (has_enough_distance) {
-      return desire_shift_length;
-    }
-
     // calculate lateral jerk.
     const auto required_jerk = PathShifter::calcJerkFromLatLonDistance(
       avoiding_shift, remaining_distance, helper_.getAvoidanceEgoSpeed());
 
     // relax lateral jerk limit. avoidable.
     if (required_jerk < helper_.getLateralMaxJerkLimit()) {
-      return desire_shift_length;
+      return std::make_pair(desire_shift_length, avoidance_distance);
     }
 
     // avoidance distance is not enough. unavoidable.
@@ -1013,7 +1015,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
         std::abs(avoiding_shift), feasible_relative_shift_length);
     }
 
-    return feasible_shift_length;
+    return std::make_pair(feasible_shift_length, avoidance_distance);
   };
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
@@ -1048,9 +1050,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
     // use each object param
     const auto object_type = utils::getHighestProbLabel(o.object.classification);
     const auto object_parameter = parameters_->object_parameters.at(object_type);
-    const auto feasible_shift_length = get_shift_length(o, desire_shift_length);
+    const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
 
-    if (!feasible_shift_length) {
+    if (!feasible_shift_profile) {
       if (o.avoid_required && is_forward_object(o)) {
         break;
       } else {
@@ -1059,10 +1061,8 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
     }
 
     // use absolute dist for return-to-center, relative dist from current for avoiding.
-    const auto feasible_avoid_distance =
-      helper_.getMaxAvoidanceDistance(feasible_shift_length.get() - current_ego_shift);
     const auto feasible_return_distance =
-      helper_.getMaxAvoidanceDistance(feasible_shift_length.get());
+      helper_.getMaxAvoidanceDistance(feasible_shift_profile.get().first);
 
     AvoidLine al_avoid;
     {
@@ -1078,14 +1078,15 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
 
       // start point (use previous linear shift length as start shift length.)
       al_avoid.start_longitudinal = [&]() {
-        const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3);
+        const auto nearest_avoid_distance =
+          std::max(to_shift_end - feasible_shift_profile.get().second, 1e-3);
 
         if (data.to_start_point > to_shift_end) {
           return nearest_avoid_distance;
         }
 
         const auto minimum_avoid_distance =
-          helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift);
+          helper_.getMinAvoidanceDistance(feasible_shift_profile.get().first - current_ego_shift);
         const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3);
 
         return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance);
@@ -1097,7 +1098,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
       al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position);
 
       // end point
-      al_avoid.end_shift_length = feasible_shift_length.get();
+      al_avoid.end_shift_length = feasible_shift_profile.get().first;
       al_avoid.end_longitudinal = to_shift_end;
 
       // misc
@@ -1112,7 +1113,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline(
       const auto to_shift_start = o.longitudinal + offset;
 
       // start point
-      al_return.start_shift_length = feasible_shift_length.get();
+      al_return.start_shift_length = feasible_shift_profile.get().first;
       al_return.start_longitudinal = to_shift_start;
 
       // end point