From 451417ac8cb67c27a11f5aeb124069f2e0ebfe20 Mon Sep 17 00:00:00 2001
From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
Date: Tue, 26 Mar 2024 17:01:00 +0900
Subject: [PATCH] fix(out_of_lane): calculate path lanelets that we can miss
 during a lane change (#6600)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
---
 .../src/lanelets_selection.cpp                | 41 ++++++++++++++++++-
 .../src/lanelets_selection.hpp                |  7 ++++
 2 files changed, 46 insertions(+), 2 deletions(-)

diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp
index 3c90a8cbf0dce..738ea22106b29 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp
@@ -24,12 +24,47 @@
 
 namespace behavior_velocity_planner::out_of_lane
 {
+
+lanelet::ConstLanelets consecutive_lanelets(
+  const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet)
+{
+  lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet);
+  const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet);
+  consecutives.insert(consecutives.end(), previous.begin(), previous.end());
+  return consecutives;
+}
+
+lanelet::ConstLanelets get_missing_lane_change_lanelets(
+  lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler)
+{
+  lanelet::ConstLanelets missing_lane_change_lanelets;
+  const auto & routing_graph = *route_handler.getRoutingGraphPtr();
+  lanelet::ConstLanelets adjacents;
+  lanelet::ConstLanelets consecutives;
+  for (const auto & ll : path_lanelets) {
+    const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll);
+    std::copy_if(
+      consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives),
+      [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); });
+    const auto adjacents_of_ll = routing_graph.besides(ll);
+    std::copy_if(
+      adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents),
+      [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); });
+  }
+  std::copy_if(
+    adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets),
+    [&](const auto & l) {
+      return !contains_lanelet(missing_lane_change_lanelets, l.id()) &&
+             !contains_lanelet(path_lanelets, l.id()) && contains_lanelet(consecutives, l.id());
+    });
+  return missing_lane_change_lanelets;
+}
+
 lanelet::ConstLanelets calculate_path_lanelets(
   const EgoData & ego_data, const route_handler::RouteHandler & route_handler)
 {
   const auto lanelet_map_ptr = route_handler.getLaneletMapPtr();
-  lanelet::ConstLanelets path_lanelets =
-    planning_utils::getLaneletsOnPath(ego_data.path, lanelet_map_ptr, ego_data.pose);
+  lanelet::ConstLanelets path_lanelets;
   lanelet::BasicLineString2d path_ls;
   for (const auto & p : ego_data.path.points)
     path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y);
@@ -38,6 +73,8 @@ lanelet::ConstLanelets calculate_path_lanelets(
     if (!contains_lanelet(path_lanelets, dist_lanelet.second.id()))
       path_lanelets.push_back(dist_lanelet.second);
   }
+  const auto missing_lanelets = get_missing_lane_change_lanelets(path_lanelets, route_handler);
+  path_lanelets.insert(path_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end());
   return path_lanelets;
 }
 
diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp
index 9c3aee46c8be5..87757a0cb2230 100644
--- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp
+++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp
@@ -41,6 +41,13 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane
 /// @return lanelets crossed by the ego vehicle
 lanelet::ConstLanelets calculate_path_lanelets(
   const EgoData & ego_data, const route_handler::RouteHandler & route_handler);
+/// @brief calculate lanelets that may not be crossed by the path but may be overlapped during a
+/// lane change
+/// @param [in] path_lanelets lanelets driven by the ego vehicle
+/// @param [in] route_handler route handler
+/// @return lanelets that may be overlapped by a lane change (and are not already in path_lanelets)
+lanelet::ConstLanelets get_missing_lane_change_lanelets(
+  lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler);
 /// @brief calculate lanelets that should be ignored
 /// @param [in] ego_data data about the ego vehicle
 /// @param [in] path_lanelets lanelets driven by the ego vehicle