feat(behavior_path_planner): pull over no stopping area

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Dec 15, 2022 · 60ebc60 · 60ebc60
1 parent e66b251
commit 60ebc60
Show file tree

Hide file tree

Showing 2 changed files with 35 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
 {
@@ -43,6 +44,8 @@ class GoalSearcher : public GoalSearcherBase
   bool checkCollisionWithLongitudinalDistance(
     const Pose & ego_pose, const PredictedObjects & dynamic_objects) const;
   void insertOrientationToPrev(std::vector<PathPointWithLaneId> & points);
+  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;
 
@@ -100,9 +101,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;
       }
@@ -255,4 +262,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