use LaneletSequence instead of ConstLanelets

Signed-off-by: mitukou1109 <mitukou1109@gmail.com>

planning/autoware_path_generator/include/autoware/path_generator/utils.hpp

@@ -48,7 +48,7 @@ std::optional<lanelet::ConstLanelet> get_next_lanelet_within_route(
   const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data);
 
 std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> get_waypoint_groups(
-  const lanelet::ConstLanelets & lanelets, const lanelet::LaneletMap & lanelet_map,
+  const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map,
   const double group_separation_threshold, const double interval_margin_ratio);
 }  // namespace utils
 }  // namespace autoware::path_generator

planning/autoware_path_generator/src/node.cpp

@@ -216,13 +216,15 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
 }
 
 std::optional<PathWithLaneId> PathGenerator::generate_path(
-  const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & current_pose,
+  const lanelet::LaneletSequence & lanelet_sequence, const geometry_msgs::msg::Pose & current_pose,
   const Params & params) const
 {
-  if (lanelets.empty()) {
+  if (lanelet_sequence.empty()) {
     return std::nullopt;
   }
 
+  const auto & lanelets = lanelet_sequence.lanelets();
+
   const auto arc_coordinates = lanelet::utils::getArcCoordinates(lanelets, current_pose);
   const auto s = arc_coordinates.length;  // s denotes longitudinal position in Frenet coordinates
   const auto s_start = std::max(0., s - params.backward_path_length);
@@ -244,11 +246,11 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
     return s_end;
   }();
 
-  return generate_path(lanelets, s_start, s_end, params);
+  return generate_path(lanelet_sequence, s_start, s_end, params);
 }
 
 std::optional<PathWithLaneId> PathGenerator::generate_path(
-  const lanelet::ConstLanelets & lanelets, const double s_start, const double s_end,
+  const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end,
   const Params & params) const
 {
   std::vector<PathPointWithLaneId> path_points_with_lane_id{};
@@ -264,18 +266,18 @@ std::optional<PathWithLaneId> PathGenerator::generate_path(
   };
 
   const auto waypoint_groups = utils::get_waypoint_groups(
-    lanelets, *planner_data_.lanelet_map_ptr, params.waypoint_group_separation_threshold,
+    lanelet_sequence, *planner_data_.lanelet_map_ptr, params.waypoint_group_separation_threshold,
     params.waypoint_group_interval_margin_ratio);
 
-  auto extended_lanelets = lanelets;
+  auto extended_lanelets = lanelet_sequence.lanelets();
   auto s_offset = 0.;
 
   for (const auto & [waypoints, interval] : waypoint_groups) {
     if (interval.first > 0.) {
       continue;
     }
     const auto prev_lanelet =
-      utils::get_previous_lanelet_within_route(lanelets.front(), planner_data_);
+      utils::get_previous_lanelet_within_route(extended_lanelets.front(), planner_data_);
     if (!prev_lanelet) {
       break;
     }

planning/autoware_path_generator/src/node.hpp

@@ -27,6 +27,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 
 #include <memory>
+
 namespace autoware::path_generator
 {
 using autoware_internal_planning_msgs::msg::PathPointWithLaneId;
@@ -85,11 +86,11 @@ class PathGenerator : public rclcpp::Node
     const geometry_msgs::msg::Pose & current_pose, const Params & params) const;
 
   std::optional<PathWithLaneId> generate_path(
-    const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & current_pose,
-    const Params & params) const;
+    const lanelet::LaneletSequence & lanelet_sequence,
+    const geometry_msgs::msg::Pose & current_pose, const Params & params) const;
 
   std::optional<PathWithLaneId> generate_path(
-    const lanelet::ConstLanelets & lanelets, const double s_start, const double s_end,
+    const lanelet::LaneletSequence & lanelet_sequence, const double s_start, const double s_end,
     const Params & params) const;
 };
 }  // namespace autoware::path_generator

planning/autoware_path_generator/src/utils.cpp

@@ -169,11 +169,10 @@ std::optional<lanelet::ConstLanelet> get_next_lanelet_within_route(
 }
 
 std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> get_waypoint_groups(
-  const lanelet::ConstLanelets & lanelets, const lanelet::LaneletMap & lanelet_map,
+  const lanelet::LaneletSequence & lanelet_sequence, const lanelet::LaneletMap & lanelet_map,
   const double group_separation_threshold, const double interval_margin_ratio)
 {
   std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> waypoint_groups{};
-  const lanelet::LaneletSequence lanelet_sequence(lanelets);
 
   const auto get_interval_bound =
     [&](const lanelet::ConstPoint3d & point, const double lateral_distance_factor) {
@@ -182,7 +181,7 @@ std::vector<std::pair<lanelet::ConstPoints3d, std::pair<double, double>>> get_wa
       return arc_coordinates.length + lateral_distance_factor * std::abs(arc_coordinates.distance);
     };
 
-  for (const auto & lanelet : lanelets) {
+  for (const auto & lanelet : lanelet_sequence) {
     if (!lanelet.hasAttribute("waypoints")) {
       continue;
     }