Add a function to calculate the lanelets crossed by the ego path points

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp

@@ -24,6 +24,28 @@
 
 namespace behavior_velocity_planner::out_of_lane
 {
+lanelet::ConstLanelets calculate_path_lanelets(
+  const EgoData & ego_data, const route_handler::RouteHandler & route_handler,
+  const lanelet::BasicPolygon2d & ego_footprint)
+{
+  const auto lanelet_map_ptr = route_handler.getLaneletMapPtr();
+  lanelet::ConstLanelets path_lanelets =
+    planning_utils::getLaneletsOnPath(ego_data.path, lanelet_map_ptr, ego_data.pose);
+  const auto add_unique_within = [&](const auto & geometry) {
+    const auto dist_lanelet_pairs =
+      lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, geometry);
+    for (const auto & dist_lanelet_pair : dist_lanelet_pairs)
+      if (!contains_lanelet(path_lanelets, dist_lanelet_pair.second.id()))
+        path_lanelets.push_back(dist_lanelet_pair.second);
+  };
+  add_unique_within(ego_footprint);
+  for (const auto & p : ego_data.path.points) {
+    const auto pt = lanelet::BasicPoint2d(p.point.pose.position.x, p.point.pose.position.y);
+    add_unique_within(pt);
+  }
+  return path_lanelets;
+}
+
 lanelet::ConstLanelets calculate_ignored_lanelets(
   const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets,
   const route_handler::RouteHandler & route_handler, const PlannerParam & params)

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp

@@ -33,6 +33,17 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane
            return l.id() == id;
          }) != lanelets.end();
 };
+
+/// @brief calculate lanelets crossed by the ego path
+/// @details calculated from the ids of the path msg, the lanelets containing path points, and the
+/// lanelets currently overlapped by the ego footprint
+/// @param [in] ego_data data about the ego vehicle
+/// @param [in] route_handler route handler
+/// @param [in] ego_footprint footprint of ego at its current pose
+/// @return lanelets crossed by the ego vehicle
+lanelet::ConstLanelets calculate_path_lanelets(
+  const EgoData & ego_data, const route_handler::RouteHandler & route_handler,
+  const lanelet::BasicPolygon2d & ego_footprint);
 /// @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

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

@@ -75,9 +75,8 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   const auto path_footprints = calculate_path_footprints(ego_data, params_);
   const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints");
   // Calculate lanelets to ignore and consider
-  const auto path_lanelets = planning_utils::getLaneletsOnPath(
-    ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(),
-    planner_data_->current_odometry->pose);
+  const auto path_lanelets =
+    calculate_path_lanelets(ego_data, *planner_data_->route_handler_, current_ego_footprint);
   const auto ignored_lanelets =
     calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_);
   const auto other_lanelets = calculate_other_lanelets(