diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index a4399c41bc0ef..ed7063c9fecba 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -635,18 +635,7 @@ 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; - return false; - } - + // check vehicle shift ratio lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); const auto on_ego_driving_lane = within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); @@ -659,17 +648,26 @@ bool isSatisfiedWithVehicleCondition( } } - if (!object.is_within_intersection) { - return true; + // 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; + return false; } - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "straight") { - return true; + if (object.is_within_intersection) { + 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; }