From 3c43118a876d58bba4949d876302ff804436f37f Mon Sep 17 00:00:00 2001
From: beyza <bnk@leodrive.ai>
Date: Tue, 17 Jan 2023 14:15:32 +0300
Subject: [PATCH] feat(obstacle_avoidance_planner): add second check with
 boost::geometry::intersection

Signed-off-by: beyza <bnk@leodrive.ai>
---
 .../src/utils/utils.cpp                       | 32 +++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/planning/obstacle_avoidance_planner/src/utils/utils.cpp b/planning/obstacle_avoidance_planner/src/utils/utils.cpp
index bb74a3e57677c..cde0db97dcb3d 100644
--- a/planning/obstacle_avoidance_planner/src/utils/utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils/utils.cpp
@@ -38,6 +38,9 @@ typedef bg::model::polygon<bg_point> bg_polygon;
 
 namespace
 {
+namespace bg = boost::geometry;
+using tier4_autoware_utils::LinearRing2d;
+using tier4_autoware_utils::LineString2d;
 std::vector<double> convertEulerAngleToMonotonic(const std::vector<double> & angle)
 {
   if (angle.empty()) {
@@ -708,6 +711,35 @@ bool isOutsideDrivableAreaFromRectangleFootprint(
     tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_left, 0.0)
       .position;
 
+  LinearRing2d footprint_polygon;
+  LineString2d left_bound_line;
+  LineString2d right_bound_line;
+  LineString2d back_bound_line;
+
+  footprint_polygon.push_back({top_left_pos.x, top_left_pos.y});
+  footprint_polygon.push_back({top_right_pos.x, top_right_pos.y});
+  footprint_polygon.push_back({bottom_right_pos.x, bottom_right_pos.y});
+  footprint_polygon.push_back({bottom_left_pos.x, bottom_left_pos.y});
+
+  bg::correct(footprint_polygon);
+
+  for (const auto & p : left_bound) {
+    left_bound_line.push_back({p.x, p.y});
+  }
+
+  for (const auto & p : right_bound) {
+    right_bound_line.push_back({p.x, p.y});
+  }
+
+  back_bound_line = {left_bound_line.back(), right_bound_line.back()};
+
+  if (
+    bg::intersects(footprint_polygon, left_bound_line) ||
+    bg::intersects(footprint_polygon, right_bound_line) ||
+    bg::intersects(footprint_polygon, back_bound_line)) {
+    return true;
+  }
+
   const bool front_top_left = isFrontDrivableArea(top_left_pos, left_bound, right_bound);
   const bool front_top_right = isFrontDrivableArea(top_right_pos, left_bound, right_bound);
   const bool front_bottom_left = isFrontDrivableArea(bottom_left_pos, left_bound, right_bound);