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 index 409fab42cd5d2..59b248ce9b11e 100644 --- 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 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(