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 6bdad3a3063ef..dd81650394740 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 @@ -65,6 +65,7 @@ class GoalSearcher : public GoalSearcherBase const std::shared_ptr & 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/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 2d49f930c2692..54f0b6bfabf87 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 @@ -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 @@ -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) {