fix of PR9641

Signed-off-by: Mamoru Sobue <[email protected]>
autowarefoundation · Dec 13, 2024 · 96f632d · 96f632d
1 parent 1548a30
commit 96f632d
Showing 1 changed file with 19 additions and 21 deletions.
diff --git a/...ner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/...ner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp
@@ -119,7 +119,7 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
   const GoalCandidate & goal_candidate, const size_t id,
   const std::shared_ptr<const PlannerData> planner_data,
   const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const
+  const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const
 {
   const double pull_over_velocity = parameters_.pull_over_velocity;
 
@@ -244,14 +244,23 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
 
     autoware::motion_utils::insertOrientation(shifted_path.path.points, true);
 
+    // combine road lanes and shoulder lanes to find closest lanelet id
+    const auto lanes = std::invoke([&]() -> lanelet::ConstLanelets {
+      auto lanes = road_lanes;
+      lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
+      return lanes;  // not copy the value (Return Value Optimization)
+    });
+
     // set goal pose with velocity 0
     {
       PathPointWithLaneId p{};
       p.point.longitudinal_velocity_mps = 0.0;
       p.point.pose = goal_pose;
-      p.lane_ids = shifted_path.path.points.back().lane_ids;
-      for (const auto & lane : shoulder_lanes) {
-        p.lane_ids.push_back(lane.id());
+      lanelet::Lanelet goal_lanelet{};
+      if (lanelet::utils::query::getClosestLanelet(lanes, goal_pose, &goal_lanelet)) {
+        p.lane_ids = {goal_lanelet.id()};
+      } else {
+        p.lane_ids = shifted_path.path.points.back().lane_ids;
       }
       shifted_path.path.points.push_back(p);
     }
@@ -260,24 +269,13 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
     for (size_t i = path_shifter.getShiftLines().front().start_idx;
          i < shifted_path.path.points.size() - 1; ++i) {
       auto & point = shifted_path.path.points.at(i);
-      // set velocity
       point.point.longitudinal_velocity_mps =
         std::min(point.point.longitudinal_velocity_mps, static_cast<float>(pull_over_velocity));
-
-      // add target lanes to points after shift start
-      // add road lane_ids if not found
-      for (const auto id : shifted_path.path.points.back().lane_ids) {
-        if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) {
-          point.lane_ids.push_back(id);
-        }
-      }
-      // add shoulder lane_id if not found
-      for (const auto & lane : shoulder_lanes) {
-        if (
-          std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) ==
-          point.lane_ids.end()) {
-          point.lane_ids.push_back(lane.id());
-        }
+      lanelet::Lanelet lanelet{};
+      if (lanelet::utils::query::getClosestLanelet(lanes, point.point.pose, &lanelet)) {
+        point.lane_ids = {lanelet.id()};  // overwrite lane_ids
+      } else {
+        point.lane_ids = shifted_path.path.points.at(i - 1).lane_ids;
       }
     }
 
@@ -314,7 +312,7 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
     });
     const bool is_in_lanes = std::invoke([&]() -> bool {
       const auto drivable_lanes =
-        utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
+        utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes);
       const auto & dp = planner_data->drivable_area_expansion_parameters;
       const auto expanded_lanes = utils::expandLanelets(
         drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,