fix(behavior_path_planner): fix pull out length to lane end (autoware…

…foundation#2692) Signed-off-by: kosuke55 <[email protected]> Signed-off-by: kosuke55 <[email protected]> Signed-off-by: Alexey Panferov <[email protected]>
lexavtanke · Jan 31, 2023 · a4e4677 · a4e4677
1 parent 22ec3f9
commit a4e4677
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(