Merge pull request #923 from tier4/hotfix/avoidance

fix(avoidance): cherry pick PRs for avoidance issue

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

@@ -166,7 +166,10 @@ bool AvoidanceModule::canTransitSuccessState()
     }
   }
 
-  const bool has_avoidance_target = !data.target_objects.empty();
+  const bool has_avoidance_target =
+    std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) {
+      return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
+    });
   const bool has_shift_point = !path_shifter_.getShiftLines().empty();
   const bool has_base_offset =
     std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold;
@@ -880,17 +883,35 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
     }
 
     // output avoidance path under lateral jerk constraints.
-    const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk(
+    const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk(
       remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed());
 
-    RCLCPP_WARN_THROTTLE(
-      getLogger(), *clock_, 1000,
-      "original shift length is not feasible. generate avoidance path under the constraints. "
-      "[original: (%.2f) actual: (%.2f)]",
-      std::abs(avoiding_shift), feasible_shift_length);
+    if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
+      object.reason = "LessThanExecutionThreshold";
+      return boost::none;
+    }
+
+    const auto feasible_shift_length =
+      desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift
+                                : -1.0 * feasible_relative_shift_length + current_ego_shift;
+
+    const auto feasible =
+      std::abs(feasible_shift_length - object.overhang_dist) <
+      0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
+    if (feasible) {
+      RCLCPP_WARN_THROTTLE(
+        getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. ");
+      object.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN;
+      return boost::none;
+    }
 
-    return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift
-                                     : -1.0 * feasible_shift_length + current_ego_shift;
+    {
+      RCLCPP_WARN_THROTTLE(
+        getLogger(), *clock_, 1000, "use feasible shift length. [original: (%.2f) actual: (%.2f)]",
+        std::abs(avoiding_shift), feasible_relative_shift_length);
+    }
+
+    return feasible_shift_length;
   };
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
@@ -3012,6 +3033,12 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
 
   const auto object = data.target_objects.front();
 
+  const auto enough_space =
+    object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
+  if (!enough_space) {
+    return;
+  }
+
   // calculate shift length for front object.
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
   const auto object_type = utils::getHighestProbLabel(object.object.classification);