From 80f4524defe81449587fe86af1a469551d9fc678 Mon Sep 17 00:00:00 2001
From: Mamoru Sobue <mamoru.sobue@tier4.jp>
Date: Mon, 9 Sep 2024 18:56:27 +0900
Subject: [PATCH] fix

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
---
 .../goal_searcher.hpp                         |  1 -
 .../util.hpp                                  | 10 ++++++++++
 .../src/goal_searcher.cpp                     | 20 ++++++-------------
 .../src/util.cpp                              | 20 +++++++++++++++++++
 4 files changed, 36 insertions(+), 15 deletions(-)

diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
index dd81650394740..aab922c873580 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
@@ -66,7 +66,6 @@ class GoalSearcher : public GoalSearcherBase
   BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   BasicPolygons2d getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes) const;
-  bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;
 
   LinearRing2d vehicle_footprint_{};
   bool left_side_parking_{true};
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
index 8fbe2facc87d1..b97cbfa7a9308 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
@@ -123,6 +123,16 @@ std::vector<Polygon2d> createPathFootPrints(
   const PathWithLaneId & path, const double base_to_front, const double base_to_rear,
   const double width);
 
+/**
+ * @brief check if footprint intersects with given areas
+ */
+bool isIntersectingAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas);
+
+/**
+ * @brief check if footprint is within one of the areas
+ */
+bool isWithinAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas);
+
 // debug
 MarkerArray createPullOverAreaMarkerArray(
   const autoware::universe_utils::MultiPolygon2d area_polygons,
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
index ca02808a1259e..085492f0fb52b 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
@@ -179,16 +179,18 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & p
 
       if (
         parameters_.bus_stop_area.use_bus_stop_area &&
-        !isInAreas(transformed_vehicle_footprint, bus_stop_area_polygons)) {
-        break;
+        !goal_planner_utils::isWithinAreas(transformed_vehicle_footprint, bus_stop_area_polygons)) {
+        continue;
       }
 
-      if (isInAreas(transformed_vehicle_footprint, no_parking_area_polygons)) {
+      if (goal_planner_utils::isIntersectingAreas(
+            transformed_vehicle_footprint, no_parking_area_polygons)) {
         // break here to exclude goals located laterally in no_parking_areas
         break;
       }
 
-      if (isInAreas(transformed_vehicle_footprint, no_stopping_area_polygons)) {
+      if (goal_planner_utils::isIntersectingAreas(
+            transformed_vehicle_footprint, no_stopping_area_polygons)) {
         // break here to exclude goals located laterally in no_stopping_areas
         break;
       }
@@ -520,16 +522,6 @@ BasicPolygons2d GoalSearcher::getBusStopAreaPolygons(const lanelet::ConstLanelet
   return area_polygons;
 }
 
-bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const
-{
-  for (const auto & area : areas) {
-    if (boost::geometry::intersects(area, footprint)) {
-      return true;
-    }
-  }
-  return false;
-}
-
 GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes(
   const GoalCandidates & goal_candidates,
   const std::shared_ptr<const PlannerData> & planner_data) const
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
index 656cdf91c0c54..7c362985ca4fd 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp
@@ -290,6 +290,26 @@ PredictedObjects filterObjectsByLateralDistance(
   return filtered_objects;
 }
 
+bool isIntersectingAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas)
+{
+  for (const auto & area : areas) {
+    if (boost::geometry::intersects(area, footprint)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+bool isWithinAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas)
+{
+  for (const auto & area : areas) {
+    if (boost::geometry::within(footprint, area)) {
+      return true;
+    }
+  }
+  return false;
+}
+
 MarkerArray createPullOverAreaMarkerArray(
   const autoware::universe_utils::MultiPolygon2d area_polygons,
   const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z)