feat(goal_planner): add getBusStopArea function

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Sep 9, 2024 · 5031f85 · 5031f85
1 parent 95ba51d
commit 5031f85
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 0 deletions.
diff --git a/..._goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/..._goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
@@ -65,6 +65,7 @@ class GoalSearcher : public GoalSearcherBase
     const std::shared_ptr<const PlannerData> & planner_data) const;
   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_{};

diff --git a/...ng/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/...ng/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
@@ -19,8 +19,10 @@
 #include "autoware/behavior_path_planner_common/utils/path_utils.hpp"
 #include "autoware/behavior_path_planner_common/utils/utils.hpp"
 #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
+#include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp"
 #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp"
 #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp"
+#include "autoware_lanelet2_extension/utility/query.hpp"
 #include "autoware_lanelet2_extension/utility/utilities.hpp"
 
 #include <boost/geometry/algorithms/union.hpp>
@@ -490,6 +492,18 @@ BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLane
   return area_polygons;
 }
 
+BasicPolygons2d GoalSearcher::getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes) const
+{
+  BasicPolygons2d area_polygons{};
+  for (const auto & bus_stop_area_reg_elem : lanelet::utils::query::busStopAreas(lanes)) {
+    for (const auto & area : bus_stop_area_reg_elem->busStopAreas()) {
+      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) {