feat(avoidance): wait next shift approval until the ego reaches shift…

… length threshold (autowarefoundation#6501)

Signed-off-by: satoshi-ota <[email protected]>
Co-authored-by: Shumpei Wakabayashi <[email protected]>

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -207,6 +207,11 @@
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
           max_deviation_from_lane: 0.5                  # [m]
+          # approve the next shift line after reaching this percentage of the current shift line length.
+          # this parameter should be within range of 0.0 to 1.0.
+          # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear.
+          # this feature can be disabled by setting this parameter to 1.0. (in that case, avoidance module creates return shift as soon as possible.)
+          ratio_for_return_shift_approval: 0.5          # [-]
         # avoidance distance parameters
         longitudinal:
           min_prepare_time: 1.0                         # [s]

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -276,6 +276,9 @@ struct AvoidanceParameters
   // use for judge if the ego is shifting or not.
   double lateral_avoid_check_threshold{0.0};
 
+  // use for return shift approval.
+  double ratio_for_return_shift_approval{0.0};
+
   // For shift line generation process. The continuous shift length is quantized by this value.
   double quantize_filter_threshold{0.0};
 
@@ -542,6 +545,8 @@ struct AvoidancePlanningData
 
   bool valid{false};
 
+  bool ready{false};
+
   bool success{false};
 
   bool comfortable{false};

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -270,6 +270,33 @@ class AvoidanceHelper
     });
   }
 
+  bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const
+  {
+    if (std::abs(current_shift_length) < 1e-3) {
+      return true;
+    }
+
+    if (new_shift_lines.empty()) {
+      return true;
+    }
+
+    const auto front_shift_relative_length = new_shift_lines.front().getRelativeLength();
+
+    // same direction shift.
+    if (current_shift_length > 0.0 && front_shift_relative_length > 0.0) {
+      return true;
+    }
+
+    // same direction shift.
+    if (current_shift_length < 0.0 && front_shift_relative_length < 0.0) {
+      return true;
+    }
+
+    // keep waiting the other side shift approval until the ego reaches shift length threshold.
+    const auto ego_shift_ratio = (current_shift_length - getEgoShift()) / current_shift_length;
+    return ego_shift_ratio < parameters_->ratio_for_return_shift_approval + 1e-3;
+  }
+
   bool isShifted() const
   {
     return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -256,6 +256,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.max_left_shift_length = getOrDeclareParameter<double>(*node, ns + "max_left_shift_length");
     p.max_deviation_from_lane =
       getOrDeclareParameter<double>(*node, ns + "max_deviation_from_lane");
+    p.ratio_for_return_shift_approval =
+      getOrDeclareParameter<double>(*node, ns + "ratio_for_return_shift_approval");
+    if (p.ratio_for_return_shift_approval < 0.0 || p.ratio_for_return_shift_approval > 1.0) {
+      throw std::domain_error(
+        "ratio_for_return_shift_approval should be within range of 0.0 to 1.0");
+    }
   }
 
   // avoidance maneuver (longitudinal)

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -133,7 +133,7 @@ bool AvoidanceModule::isExecutionRequested() const
 bool AvoidanceModule::isExecutionReady() const
 {
   DEBUG_PRINT("AVOIDANCE isExecutionReady");
-  return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid;
+  return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready;
 }
 
 bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const
@@ -513,6 +513,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de
    */
   data.comfortable = helper_->isComfortable(data.new_shift_line);
   data.safe = isSafePath(data.candidate_path, debug);
+  data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength());
 }
 
 void AvoidanceModule::fillEgoStatus(