fix(avoidance_module): ignore object instead of creating zero shift (t…

…ier4#731) * fix: ignore object instead of creating zero shift instead of creating zero shift point, the object will be ignored. no behavior changes should be observed. Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * refactor: sync continue with upstream Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix: fix debug message for insufficient lateral margin Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
boyali · Oct 3, 2022 · 5ad7156 · 5ad7156
1 parent 12d494b
commit 5ad7156
Showing 1 changed file with 7 additions and 12 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -490,24 +490,19 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
     avoidance_debug_msg.max_shift_length = max_allowable_lateral_distance;
 
     if (!(o.to_road_shoulder_distance > max_allowable_lateral_distance)) {
-      avoidance_debug_msg.allow_avoidance = false;
-      avoidance_debug_msg.failed_reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN;
+      avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN);
+      continue;
     }
 
     const auto max_shift_length =
       o.to_road_shoulder_distance - road_shoulder_safety_margin - 0.5 * vehicle_width;
-    const auto max_left_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length,
-                                       this]() noexcept {
-      const auto left_shift_constraint = std::min(getLeftShiftBound(), max_shift_length);
-      return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? left_shift_constraint
-                                                                            : 0.0;
+
+    const auto max_left_shift_limit = [&max_shift_length, this]() noexcept {
+      return std::min(getLeftShiftBound(), max_shift_length);
     };
 
-    const auto max_right_shift_limit = [&o, &max_allowable_lateral_distance, &max_shift_length,
-                                        this]() noexcept {
-      const auto right_shift_constraint = std::max(getRightShiftBound(), -max_shift_length);
-      return (o.to_road_shoulder_distance > max_allowable_lateral_distance) ? right_shift_constraint
-                                                                            : 0.0;
+    const auto max_right_shift_limit = [&max_shift_length, this]() noexcept {
+      return std::max(getRightShiftBound(), -max_shift_length);
     };
 
     const auto shift_length = isOnRight(o)