fix(bpp): update collided polygon pose only once (autowarefoundation#…

…9338) * fix(bpp): update collided polygon pose only once Signed-off-by: Zulfaqar Azmi <[email protected]> * add expected pose Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]>
a-maumau · Nov 15, 2024 · 8dfc04a · 8dfc04a
1 parent d819a66
commit 8dfc04a
Showing 1 changed file with 17 additions and 12 deletions.
diff --git a/...nner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/...nner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
@@ -622,13 +622,15 @@ std::vector<Polygon2d> 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<Polygon2d> 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;
     }
   }