From 8dfc04a3554155f52b39b4227c08ecb8800ecbf4 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 15 Nov 2024 18:29:12 +0900 Subject: [PATCH] fix(bpp): update collided polygon pose only once (#9338) * fix(bpp): update collided polygon pose only once Signed-off-by: Zulfaqar Azmi * add expected pose Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../path_safety_checker/safety_check.cpp | 29 +++++++++++-------- 1 file changed, 17 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..04b61b86dfa02 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,17 @@ 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.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + 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; } }