fix(behavior_path_planner): fix pull out length to lane end (#2692)

Signed-off-by: kosuke55 <[email protected]> Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Jan 19, 2023 · fcacc80 · fcacc80
1 parent 9e3e3a0
commit fcacc80
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp
@@ -557,8 +557,10 @@ std::vector<Pose> PullOutModule::searchBackedPoses()
     // check the back pose is near the lane end
     const double length_to_backed_pose =
       lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length;
-    const double length_to_lane_end =
-      lanelet::utils::getLaneletLength2d(status_.pull_out_lanes.back());
+    double length_to_lane_end = 0.0;
+    for (const auto & lane : status_.pull_out_lanes) {
+      length_to_lane_end += lanelet::utils::getLaneletLength2d(lane);
+    }
     const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose;
     if (distance_from_lane_end < parameters_.ignore_distance_from_lane_end) {
       RCLCPP_WARN_THROTTLE(