Merge pull request #984 from tier4/fix/lc_stop_distance_0.11

fix(lane_change): fix terminal stop distance (autowarefoundation#5392)
tier4 · Oct 31, 2023 · b3c1397 · b3c1397
2 parents 54931e5 + 3f7ceaa
commit b3c1397
Show file tree

Hide file tree

Showing 2 changed files with 49 additions and 10 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp
@@ -372,10 +372,12 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data);
 
 lanelet::ConstLanelets extendNextLane(
-  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
+  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes,
+  const bool only_in_route = false);
 
 lanelet::ConstLanelets extendPrevLane(
-  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
+  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes,
+  const bool only_in_route = false);
 
 lanelet::ConstLanelets extendLanes(
   const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);

diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -2994,48 +2994,85 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
 
   lanelet::ConstLanelet current_lane;
   lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, &current_lane);
-
-  return route_handler->getLaneletSequence(
+  auto current_lanes = route_handler->getLaneletSequence(
     current_lane, current_pose, p.backward_path_length, p.forward_path_length);
+
+  // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids'
+  // if the extended prior lanes is in same lane sequence with current lanes
+  const auto front_lane_ids = path.points.front().lane_ids;
+  auto have_front_lanes = [front_lane_ids](const auto & lanes) {
+    return std::any_of(lanes.begin(), lanes.end(), [&](const auto & lane) {
+      return std::find(front_lane_ids.begin(), front_lane_ids.end(), lane.id()) !=
+             front_lane_ids.end();
+    });
+  };
+  auto extended_lanes = current_lanes;
+  while (rclcpp::ok()) {
+    const size_t pre_extension_size = extended_lanes.size();  // Get existing size before extension
+    extended_lanes = extendPrevLane(route_handler, extended_lanes, true);
+    if (extended_lanes.size() == pre_extension_size) break;
+    if (have_front_lanes(extended_lanes)) {
+      current_lanes = extended_lanes;
+      break;
+    }
+  }
+
+  return current_lanes;
 }
 
 lanelet::ConstLanelets extendNextLane(
-  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes)
+  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes,
+  const bool only_in_route)
 {
+  if (lanes.empty()) return lanes;
+
   auto extended_lanes = lanes;
 
   // Add next lane
   const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back());
   if (!next_lanes.empty()) {
+    boost::optional<lanelet::ConstLanelet> target_next_lane;
+    if (!only_in_route) {
+      target_next_lane = next_lanes.front();
+    }
     // use the next lane in route if it exists
-    auto target_next_lane = next_lanes.front();
     for (const auto & next_lane : next_lanes) {
       if (route_handler->isRouteLanelet(next_lane)) {
         target_next_lane = next_lane;
       }
     }
-    extended_lanes.push_back(target_next_lane);
+    if (target_next_lane) {
+      extended_lanes.push_back(*target_next_lane);
+    }
   }
 
   return extended_lanes;
 }
 
 lanelet::ConstLanelets extendPrevLane(
-  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes)
+  const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes,
+  const bool only_in_route)
 {
+  if (lanes.empty()) return lanes;
+
   auto extended_lanes = lanes;
 
   // Add previous lane
   const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front());
   if (!prev_lanes.empty()) {
+    boost::optional<lanelet::ConstLanelet> target_prev_lane;
+    if (!only_in_route) {
+      target_prev_lane = prev_lanes.front();
+    }
     // use the previous lane in route if it exists
-    auto target_prev_lane = prev_lanes.front();
     for (const auto & prev_lane : prev_lanes) {
       if (route_handler->isRouteLanelet(prev_lane)) {
         target_prev_lane = prev_lane;
       }
     }
-    extended_lanes.insert(extended_lanes.begin(), target_prev_lane);
+    if (target_prev_lane) {
+      extended_lanes.insert(extended_lanes.begin(), *target_prev_lane);
+    }
   }
   return extended_lanes;
 }