fix preceding lanes

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -175,11 +175,6 @@ class NormalLaneChange : public LaneChangeBase
     const utils::path_safety_checker::RSSparams & rss_params,
     CollisionCheckDebugMap & debug_data) const;
 
-  LaneChangeTargetObjectIndices filterObject(
-    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
-    const lanelet::ConstLanelets & target_lanes,
-    const lanelet::ConstLanelets & target_backward_lanes) const;
-
   //! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
   //! @param obstacle_check_distance Distance to check ahead for any objects that might be
   //! obstructing ego path. It makes sense to use values like the maximum lane change distance.

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -1005,42 +1005,22 @@ void NormalLaneChange::filterObjectsByLanelets(
 
   // get backward lanes
   const auto backward_length = lane_change_parameters_->backward_lane_length;
-  const auto target_backward_lanes = [&]() -> std::vector<lanelet::ConstLanelets> {
-    if (target_lanes.empty()) {
-      return {};
-    }
-
-    const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);
-
-    if (arc_length.length >= backward_length) {
-      return {};
-    }
-
-    const auto & front_lane = target_lanes.front();
-    return route_handler->getPrecedingLaneletSequence(
-      front_lane, std::abs(backward_length - arc_length.length), {front_lane});
-  }();
+  const auto target_backward_lanes =
+    utils::getPrecedingLanelets(*route_handler, target_lanes, current_pose, backward_length);
 
   {
     lane_change_debug_.current_lanes = current_lanes;
     lane_change_debug_.target_lanes = target_lanes;
 
-    lanelet::ConstLanelets backward_lanes{};
-    const auto num_of_lanes = std::invoke([&target_backward_lanes]() {
-      size_t sum{0};
-      for (const auto & lanes : target_backward_lanes) {
-        sum += lanes.size();
-      }
-      return sum;
-    });
-
-    backward_lanes.reserve(num_of_lanes);
-
-    for (const auto & lanes : target_backward_lanes) {
-      backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end());
-    }
-
-    lane_change_debug_.target_backward_lanes = backward_lanes;
+    // TODO(Azu) change the type to std::vector<lanelet::ConstLanelet>
+    lane_change_debug_.target_backward_lanes.clear();
+    std::for_each(
+      target_backward_lanes.begin(), target_backward_lanes.end(),
+      [&](const lanelet::ConstLanelets & target_backward_lane) {
+        lane_change_debug_.target_backward_lanes.insert(
+          lane_change_debug_.target_backward_lanes.end(), target_backward_lane.begin(),
+          target_backward_lane.end());
+      });
   }
 
   const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp

@@ -299,6 +299,10 @@ lanelet::ConstLanelets extendPrevLane(
 lanelet::ConstLanelets extendLanes(
   const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
 
+std::vector<lanelet::ConstLanelets> getPrecedingLanelets(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
+  const Pose & current_pose, const double backward_length);
+
 lanelet::ConstLanelets getBackwardLanelets(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & current_pose, const double backward_length);

planning/behavior_path_planner_common/src/utils/utils.cpp

@@ -1513,8 +1513,7 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath(
   return lanes;
 }
 
-// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane
-lanelet::ConstLanelets getBackwardLanelets(
+std::vector<lanelet::ConstLanelets> getPrecedingLanelets(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
   const Pose & current_pose, const double backward_length)
 {
@@ -1529,18 +1528,21 @@ lanelet::ConstLanelets getBackwardLanelets(
   }
 
   const auto & front_lane = target_lanes.front();
-  const auto preceding_lanes = route_handler.getPrecedingLaneletSequence(
+  return route_handler.getPrecedingLaneletSequence(
     front_lane, std::abs(backward_length - arc_length.length), {front_lane});
+}
 
-  lanelet::ConstLanelets backward_lanes{};
-  const auto num_of_lanes = std::invoke([&preceding_lanes]() {
-    size_t sum{0};
-    for (const auto & lanes : preceding_lanes) {
-      sum += lanes.size();
-    }
-    return sum;
-  });
+lanelet::ConstLanelets getBackwardLanelets(
+  const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
+  const Pose & current_pose, const double backward_length)
+{
+  const auto preceding_lanes =
+    getPrecedingLanelets(route_handler, target_lanes, current_pose, backward_length);
+  const auto calc_sum = [](size_t sum, const auto & lanes) { return sum + lanes.size(); };
+  const auto num_of_lanes =
+    std::accumulate(preceding_lanes.begin(), preceding_lanes.end(), 0u, calc_sum);
 
+  lanelet::ConstLanelets backward_lanes{};
   backward_lanes.reserve(num_of_lanes);
 
   for (const auto & lanes : preceding_lanes) {