From be269fe9f2ec171e689e349aef32be26598a65d5 Mon Sep 17 00:00:00 2001
From: kosuke55 <kosuke.tnp@gmail.com>
Date: Mon, 12 Dec 2022 23:53:56 +0900
Subject: [PATCH 1/2] feat(behavior_path_planner): pull over no stopping area

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../scene_module/pull_over/goal_searcher.hpp  |  3 ++
 .../scene_module/pull_over/goal_searcher.cpp  | 32 +++++++++++++++++++
 2 files changed, 35 insertions(+)

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp
index f26d0e4cdebfe..de79f725e01c7 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp
@@ -27,6 +27,7 @@ namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using tier4_autoware_utils::LinearRing2d;
+using BasicPolygons2d = std::vector<lanelet::BasicPolygon2d>;
 
 class GoalSearcher : public GoalSearcherBase
 {
@@ -42,6 +43,8 @@ class GoalSearcher : public GoalSearcherBase
   bool checkCollision(const Pose & pose) const;
   bool checkCollisionWithLongitudinalDistance(
     const Pose & ego_pose, const PredictedObjects & dynamic_objects) const;
+  BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
+  bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;
 
   LinearRing2d vehicle_footprint_{};
   std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map_{};
diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp
index dea04a5ab85fe..29a0c2f680b35 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp
@@ -26,6 +26,7 @@
 namespace behavior_path_planner
 {
 using lane_departure_checker::LaneDepartureChecker;
+using lanelet::autoware::NoStoppingArea;
 using tier4_autoware_utils::calcOffsetPose;
 using tier4_autoware_utils::inverseTransformPose;
 
@@ -86,9 +87,15 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose)
 
       const auto transformed_vehicle_footprint =
         transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose));
+
+      if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) {
+        continue;
+      }
+
       if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) {
         continue;
       }
+
       if (checkCollision(search_pose)) {
         continue;
       }
@@ -241,4 +248,29 @@ void GoalSearcher::createAreaPolygons(std::vector<Pose> original_search_poses)
   }
 }
 
+BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const
+{
+  // std::vector<lanelet::BasicPolygon2d> polys{}
+  BasicPolygons2d area_polygons{};
+  for (const auto & ll : lanes) {
+    for (const auto & reg_elem : ll.regulatoryElementsAs<NoStoppingArea>()) {
+      for (const auto & area : reg_elem->noStoppingAreas()) {
+        const auto & area_poly = lanelet::utils::to2D(area).basicPolygon();
+        area_polygons.push_back(area_poly);
+      }
+    }
+  }
+  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;
+}
+
 }  // namespace behavior_path_planner

From 1c26078c95e8f4a2410ed8ea0c3c177e99e12fc5 Mon Sep 17 00:00:00 2001
From: Kosuke Takeuchi <kosuke.tnp@gmail.com>
Date: Thu, 22 Dec 2022 16:13:24 +0900
Subject: [PATCH 2/2] Update
 planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp

---
 .../src/scene_module/pull_over/goal_searcher.cpp                 | 1 -
 1 file changed, 1 deletion(-)

diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp
index 29a0c2f680b35..c75d216730e50 100644
--- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp
+++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp
@@ -250,7 +250,6 @@ void GoalSearcher::createAreaPolygons(std::vector<Pose> original_search_poses)
 
 BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const
 {
-  // std::vector<lanelet::BasicPolygon2d> polys{}
   BasicPolygons2d area_polygons{};
   for (const auto & ll : lanes) {
     for (const auto & reg_elem : ll.regulatoryElementsAs<NoStoppingArea>()) {