update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Jul 18, 2023 · 963a479 · 963a479
1 parent 2da1814
commit 963a479
Show file tree

Hide file tree

Showing 4 changed files with 72 additions and 120 deletions.
diff --git a/...ng/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/...ng/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp
@@ -24,6 +24,7 @@
 #include <limits>
 #include <memory>
 #include <optional>
+#include <set>
 #include <string>
 #include <utility>
 #include <vector>
@@ -82,6 +83,20 @@ struct DebugData
   std::vector<std::vector<geometry_msgs::msg::Point>> obj_polygons;
 };
 
+std::vector<lanelet::ConstLanelet> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
+
+std::set<int64_t> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
+
+bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr);
+
 std::vector<geometry_msgs::msg::Point> 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
@@ -22,66 +22,6 @@
 #include <string>
 #include <vector>
 
-namespace
-{
-std::vector<lanelet::ConstLanelet> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
-{
-  std::vector<lanelet::ConstLanelet> crosswalks;
-
-  // Add current lane id
-  const auto nearest_lane_id =
-    behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose);
-
-  std::vector<int64_t> 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<int64_t> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
-{
-  std::set<int64_t> 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
@@ -48,6 +48,63 @@ using tier4_autoware_utils::createPoint;
 using tier4_autoware_utils::Line2d;
 using tier4_autoware_utils::Point2d;
 
+std::vector<lanelet::ConstLanelet> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
+{
+  std::vector<lanelet::ConstLanelet> crosswalks;
+
+  // Add current lane id
+  const auto nearest_lane_id =
+    behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose);
+
+  std::vector<int64_t> 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<int64_t> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
+{
+  std::set<int64_t> 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<geometry_msgs::msg::Point> 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
@@ -22,66 +22,6 @@
 #include <string>
 #include <vector>
 
-namespace
-{
-std::vector<lanelet::ConstLanelet> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
-{
-  std::vector<lanelet::ConstLanelet> crosswalks;
-
-  // Add current lane id
-  const auto nearest_lane_id =
-    behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose);
-
-  std::vector<int64_t> 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<int64_t> 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<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
-{
-  std::set<int64_t> 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
 {