feat(behavior_path_planner): add avoid_margin for over distance case (#…

…2198)

* add avoid margin for over distance case

Signed-off-by: beyza <bnk@leodrive.ai>

* ci(pre-commit): autofix

* fix: let avoid_margin vary between min and max value

Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

Signed-off-by: beyza <bnk@leodrive.ai>
Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com>
Co-authored-by: beyza <bnk@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Mehmet Dogru <42mehmetdogru42@gmail.com>

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

@@ -559,8 +559,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
   const auto & road_shoulder_safety_margin = parameters_->road_shoulder_safety_margin;
 
-  const auto avoid_margin =
-    lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width;
+  auto avoid_margin = lat_collision_safety_buffer + lat_collision_margin + 0.5 * vehicle_width;
 
   AvoidLineArray avoid_lines;
   std::vector<AvoidanceDebugMsg> avoidance_debug_msg_array;
@@ -583,9 +582,16 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr
     avoidance_debug_msg.to_furthest_linestring_distance = o.to_road_shoulder_distance;
     avoidance_debug_msg.max_shift_length = max_allowable_lateral_distance;
 
-    if (max_allowable_lateral_distance <= avoid_margin) {
-      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN);
-      continue;
+    auto const min_safety_lateral_distance = lat_collision_safety_buffer + 0.5 * vehicle_width;
+
+    if (max_allowable_lateral_distance < avoid_margin) {
+      if (max_allowable_lateral_distance >= min_safety_lateral_distance) {
+        avoid_margin = max_allowable_lateral_distance;
+      } else {
+        avoidance_debug_array_false_and_push_back(
+          AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN);
+        continue;
+      }
     }
 
     const auto is_object_on_right = isOnRight(o);