use only route lanelets

Signed-off-by: mitukou1109 <[email protected]>
autowarefoundation · Nov 12, 2024 · e0f2caa · e0f2caa
1 parent 6b0f0f1
commit e0f2caa
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 13 deletions.
diff --git a/planning/autoware_path_generator/src/node.cpp b/planning/autoware_path_generator/src/node.cpp
@@ -227,10 +227,11 @@ std::optional<PathWithLaneId> PathGenerator::get_centerline_path(
       s_end = std::clamp(s_end, 0.0, lane_length);
     }
 
-    if (
-      std::find(
-        planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(),
-        lanelet_sequence.back()) != planner_data_.goal_lanelets.end()) {
+    if (std::any_of(
+          planner_data_.goal_lanelets.begin(), planner_data_.goal_lanelets.end(),
+          [&](const auto & goal_lanelet) {
+            return lanelet_sequence.back().id() == goal_lanelet.id();
+          })) {
       const auto goal_arc_coordinates =
         lanelet::utils::getArcCoordinates(lanelet_sequence, planner_data_.goal_pose);
       s_end = std::clamp(s_end, 0.0, goal_arc_coordinates.length);

diff --git a/planning/autoware_path_generator/src/utils.cpp b/planning/autoware_path_generator/src/utils.cpp
@@ -37,6 +37,10 @@ std::optional<lanelet::ConstLanelets> get_lanelet_sequence(
   const geometry_msgs::msg::Pose & current_pose, const double forward_distance,
   const double backward_distance)
 {
+  if (!exists(planner_data.route_lanelets, lanelet)) {
+    return std::nullopt;
+  }
+
   auto lanelet_sequence = [&]() -> std::optional<lanelet::ConstLanelets> {
     const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose);
     if (arc_coordinate.length < backward_distance) {
@@ -72,18 +76,18 @@ std::optional<lanelet::ConstLanelets> get_lanelet_sequence(
 std::optional<lanelet::ConstLanelets> get_lanelet_sequence_after(
   const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance)
 {
+  if (!exists(planner_data.route_lanelets, lanelet)) {
+    return std::nullopt;
+  }
+
   lanelet::ConstLanelets lanelet_sequence{};
   auto current_lanelet = lanelet;
   auto length = 0.;
 
   while (rclcpp::ok() && length < distance) {
     auto next_lanelet = get_next_lanelet_within_route(current_lanelet, planner_data);
     if (!next_lanelet) {
-      const auto next_lanelets = planner_data.routing_graph_ptr->following(current_lanelet);
-      if (next_lanelets.empty()) {
-        break;
-      }
-      next_lanelet = next_lanelets.front();
+      break;
     }
 
     // loop check
@@ -102,17 +106,18 @@ std::optional<lanelet::ConstLanelets> get_lanelet_sequence_after(
 std::optional<lanelet::ConstLanelets> get_lanelet_sequence_up_to(
   const lanelet::ConstLanelet & lanelet, const PlannerData & planner_data, const double distance)
 {
+  if (!exists(planner_data.route_lanelets, lanelet)) {
+    return std::nullopt;
+  }
+
   lanelet::ConstLanelets lanelet_sequence{};
   auto current_lanelet = lanelet;
   auto length = 0.;
 
   while (rclcpp::ok() && length < distance) {
     auto previous_lanelets = get_previous_lanelets_within_route(current_lanelet, planner_data);
     if (!previous_lanelets) {
-      previous_lanelets = planner_data.routing_graph_ptr->previous(current_lanelet);
-      if (previous_lanelets->empty()) {
-        break;
-      }
+      break;
     }
 
     if (