feat(behavior_path_planner): pull over no stopping area (autowarefoun…

…dation#2492) * feat(behavior_path_planner): pull over no stopping area Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Update planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
tier4 · Dec 28, 2022 · 71eaca1 · 71eaca1
1 parent bfc55e0
commit 71eaca1
Show file tree

Hide file tree

Showing 2 changed files with 34 additions and 0 deletions.
diff --git a/...avior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp b/...avior_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
@@ -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,28 @@ void GoalSearcher::createAreaPolygons(std::vector<Pose> original_search_poses)
   }
 }
 
+BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const
+{
+  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