fix(avoidance): don't ignore objects on straight lane in intersection

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Dec 23, 2023 · b44bb1c · b44bb1c
1 parent 2e2f184
commit b44bb1c
Showing 1 changed file with 11 additions and 14 deletions.
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -635,12 +635,6 @@ bool isSatisfiedWithVehicleCondition(
     return true;
   }
 
-  // always ignore all merging objects.
-  if (object.behavior == ObjectData::Behavior::MERGING) {
-    object.reason = "MergingToEgoLane";
-    return false;
-  }
-
   // Object is on center line -> ignore.
   if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) {
     object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE;
@@ -659,17 +653,20 @@ bool isSatisfiedWithVehicleCondition(
     }
   }
 
-  if (!object.is_within_intersection) {
-    return true;
-  }
+  if (object.is_within_intersection) {
+    std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else");
+    if (turn_direction == "straight") {
+      return true;
+    }
 
-  std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else");
-  if (turn_direction == "straight") {
-    return true;
+    if (object.behavior == ObjectData::Behavior::NONE) {
+      object.reason = "ParallelToEgoLane";
+      return false;
+    }
   }
 
-  if (object.behavior == ObjectData::Behavior::NONE) {
-    object.reason = "ParallelToEgoLane";
+  if (object.behavior == ObjectData::Behavior::MERGING) {
+    object.reason = "MergingToEgoLane";
     return false;
   }