From dfbb80e6be27299858af3a43dc3c5a90341e8e96 Mon Sep 17 00:00:00 2001
From: Takayuki Murooka <takayuki5168@gmail.com>
Date: Tue, 16 May 2023 02:56:55 +0900
Subject: [PATCH] fix(behavior_path_planner): extract backward obstacles
 correctly (#3719)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
---
 planning/behavior_path_planner/src/utils/utils.cpp | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
index 07658ad41e205..bbe43d4155700 100644
--- a/planning/behavior_path_planner/src/utils/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -234,6 +234,7 @@ std::vector<PolygonPoint> generatePolygonInsideBounds(
   }
 
   std::vector<PolygonPoint> inside_poly;
+  bool has_intersection = false;  // NOTE: between obstacle polygon and bound
   for (int i = 0; i < static_cast<int>(full_polygon.size()); ++i) {
     const auto & curr_poly = full_polygon.at(i);
     const auto & prev_poly = full_polygon.at(i == 0 ? full_polygon.size() - 1 : i - 1);
@@ -258,6 +259,7 @@ std::vector<PolygonPoint> generatePolygonInsideBounds(
       bound, intersection->first, intersection->second);
     const auto intersect_point =
       PolygonPoint{intersection->second, intersection->first, lon_dist, 0.0};
+    has_intersection = true;
 
     if (is_prev_outside && !is_curr_outside) {
       inside_poly.push_back(intersect_point);
@@ -270,7 +272,10 @@ std::vector<PolygonPoint> generatePolygonInsideBounds(
     continue;
   }
 
-  return inside_poly;
+  if (has_intersection) {
+    return inside_poly;
+  }
+  return std::vector<PolygonPoint>{};
 }
 
 std::vector<geometry_msgs::msg::Point> convertToGeometryPoints(