diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index d64cd81cf7b2d..578ae66e40006 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,20 @@ struct DebugData std::vector> obj_polygons; }; +std::vector getCrosswalksOnPath( + const geometry_msgs::msg::Pose & current_pose, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs); + +std::set getCrosswalkIdSetOnPath( + const geometry_msgs::msg::Pose & current_pose, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs); + +bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr); + std::vector getPolygonIntersects( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, const geometry_msgs::msg::Point & ego_pos, const size_t max_num); diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index b63a5fc723887..a3f2568b82f90 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -22,66 +22,6 @@ #include #include -namespace -{ -std::vector getCrosswalksOnPath( - const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, - const std::shared_ptr & overall_graphs) -{ - std::vector crosswalks; - - // Add current lane id - const auto nearest_lane_id = - behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); - - std::vector unique_lane_ids; - if (nearest_lane_id) { - // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); - } else { - // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); - } - - for (const auto lane_id : unique_lane_ids) { - const auto ll = lanelet_map->laneletLayer.get(lane_id); - - constexpr int PEDESTRIAN_GRAPH_ID = 1; - const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); - for (const auto & crosswalk : conflicting_crosswalks) { - crosswalks.push_back(crosswalk); - } - } - - return crosswalks; -} - -std::set getCrosswalkIdSetOnPath( - const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, - const std::shared_ptr & overall_graphs) -{ - std::set crosswalk_id_set; - - for (const auto & crosswalk : - getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { - crosswalk_id_set.insert(crosswalk.id()); - } - - return crosswalk_id_set; -} - -bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); - return !lanelet::utils::query::crosswalks(all_lanelets).empty(); -} -} // namespace - namespace behavior_velocity_planner { diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 614884af9a90e..72c091cd8bc85 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -48,6 +48,63 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::Line2d; using tier4_autoware_utils::Point2d; +std::vector getCrosswalksOnPath( + const geometry_msgs::msg::Pose & current_pose, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs) +{ + std::vector crosswalks; + + // Add current lane id + const auto nearest_lane_id = + behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); + + std::vector unique_lane_ids; + if (nearest_lane_id) { + // Add subsequent lane_ids from nearest lane_id + unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); + } else { + // Add all lane_ids in path + unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + } + + for (const auto lane_id : unique_lane_ids) { + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); + for (const auto & crosswalk : conflicting_crosswalks) { + crosswalks.push_back(crosswalk); + } + } + + return crosswalks; +} + +std::set getCrosswalkIdSetOnPath( + const geometry_msgs::msg::Pose & current_pose, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs) +{ + std::set crosswalk_id_set; + + for (const auto & crosswalk : + getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { + crosswalk_id_set.insert(crosswalk.id()); + } + + return crosswalk_id_set; +} + +bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); + return !lanelet::utils::query::crosswalks(all_lanelets).empty(); +} + std::vector getPolygonIntersects( const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, const geometry_msgs::msg::Point & ego_pos, diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index 3a39b569c0ef8..f2e844063568a 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -22,66 +22,6 @@ #include #include -namespace -{ -std::vector getCrosswalksOnPath( - const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, - const std::shared_ptr & overall_graphs) -{ - std::vector crosswalks; - - // Add current lane id - const auto nearest_lane_id = - behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); - - std::vector unique_lane_ids; - if (nearest_lane_id) { - // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); - } else { - // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); - } - - for (const auto lane_id : unique_lane_ids) { - const auto ll = lanelet_map->laneletLayer.get(lane_id); - - constexpr int PEDESTRIAN_GRAPH_ID = 1; - const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); - for (const auto & crosswalk : conflicting_crosswalks) { - crosswalks.push_back(crosswalk); - } - } - - return crosswalks; -} - -std::set getCrosswalkIdSetOnPath( - const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, - const std::shared_ptr & overall_graphs) -{ - std::set crosswalk_id_set; - - for (const auto & crosswalk : - getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { - crosswalk_id_set.insert(crosswalk.id()); - } - - return crosswalk_id_set; -} - -bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr) -{ - const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr); - return !lanelet::utils::query::crosswalks(all_lanelets).empty(); -} -} // namespace - namespace behavior_velocity_planner {