From b44bb1c21e90c5d3445680c728a88fbdebbe6ad8 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sat, 23 Dec 2023 16:06:23 +0900 Subject: [PATCH 1/2] fix(avoidance): don't ignore objects on straight lane in intersection Signed-off-by: satoshi-ota --- .../src/utils.cpp | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index a4399c41bc0ef..e065606b719cb 100644 --- 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; } From f095899b98b7d58e65971e438546be36b0080199 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Sun, 24 Dec 2023 12:45:48 +0900 Subject: [PATCH 2/2] fix(avoidance): fix filtering flow Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/src/utils.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index e065606b719cb..ed7063c9fecba 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -635,12 +635,7 @@ bool isSatisfiedWithVehicleCondition( 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; - } - + // 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()); @@ -653,6 +648,12 @@ bool isSatisfiedWithVehicleCondition( } } + // 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; + } + if (object.is_within_intersection) { std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); if (turn_direction == "straight") {