From 91bb0e2d7ed5908795612ad75c01d6345c4ccbca Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 15 Nov 2024 17:20:11 +0900 Subject: [PATCH] fix(bpp): update collided polygon pose only once Signed-off-by: Zulfaqar Azmi --- .../path_safety_checker/safety_check.cpp | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 5b629cf0c9a20..ae42a3b442947 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -622,13 +622,15 @@ std::vector get_collided_polygons( // check intersects if (boost::geometry::intersects(ego_polygon, obj_polygon)) { - debug.unsafe_reason = "overlap_polygon"; + if (collided_polygons.empty()) { + debug.unsafe_reason = "overlap_polygon"; + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + debug.extended_ego_polygon = ego_polygon; + debug.extended_obj_polygon = obj_polygon; + } collided_polygons.push_back(obj_polygon); - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = obj_pose; - debug.extended_ego_polygon = ego_polygon; - debug.extended_obj_polygon = obj_polygon; continue; } @@ -676,14 +678,15 @@ std::vector get_collided_polygons( // check intersects with extended polygon if (boost::geometry::intersects(extended_ego_polygon, extended_obj_polygon)) { - debug.unsafe_reason = "overlap_extended_polygon"; + if (collided_polygons.empty()) { + debug.unsafe_reason = "overlap_extended_polygon"; + debug.rss_longitudinal = rss_dist; + debug.inter_vehicle_distance = min_lon_length; + debug.extended_ego_polygon = extended_ego_polygon; + debug.extended_obj_polygon = extended_obj_polygon; + debug.is_front = is_object_front; + } collided_polygons.push_back(obj_polygon); - - debug.rss_longitudinal = rss_dist; - debug.inter_vehicle_distance = min_lon_length; - debug.extended_ego_polygon = extended_ego_polygon; - debug.extended_obj_polygon = extended_obj_polygon; - debug.is_front = is_object_front; } }