fix(out_of_lane): ignore overlaps over all lanelets crossed by the eg…

…o path (autowarefoundation#6334)

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_velocity_out_of_lane_module/src/footprint.cpp

@@ -21,6 +21,7 @@
 #include <lanelet2_core/geometry/Polygon.h>
 #include <tf2/utils.h>
 
+#include <algorithm>
 #include <vector>
 
 namespace behavior_velocity_planner::out_of_lane
@@ -58,14 +59,19 @@ std::vector<lanelet::BasicPolygon2d> calculate_path_footprints(
   const auto base_footprint = make_base_footprint(params);
   std::vector<lanelet::BasicPolygon2d> path_footprints;
   path_footprints.reserve(ego_data.path.points.size());
-  for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) {
+  double length = 0.0;
+  const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) +
+                     params.front_offset + params.extra_front_offset;
+  for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size() && length < range; ++i) {
     const auto & path_pose = ego_data.path.points[i].point.pose;
     const auto angle = tf2::getYaw(path_pose.orientation);
     const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle);
     lanelet::BasicPolygon2d footprint;
     for (const auto & p : rotated_footprint.outer())
       footprint.emplace_back(p.x() + path_pose.position.x, p.y() + path_pose.position.y);
     path_footprints.push_back(footprint);
+    if (i + 1 < ego_data.path.points.size())
+      length += tier4_autoware_utils::calcDistance2d(path_pose, ego_data.path.points[i + 1].point);
   }
   return path_footprints;
 }

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp

@@ -24,15 +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 auto lanelet_map_ptr = route_handler.getLaneletMapPtr();
+  lanelet::ConstLanelets path_lanelets =
+    planning_utils::getLaneletsOnPath(ego_data.path, lanelet_map_ptr, ego_data.pose);
+  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);
+  for (const auto & dist_lanelet :
+       lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, path_ls)) {
+    if (!contains_lanelet(path_lanelets, dist_lanelet.second.id()))
+      path_lanelets.push_back(dist_lanelet.second);
+  }
+  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)
 {
   lanelet::ConstLanelets ignored_lanelets;
-  // ignore lanelets that follows path lanelets
-  for (const auto & path_lanelet : path_lanelets)
-    for (const auto & following : route_handler.getRoutingGraphPtr()->following(path_lanelet))
-      if (!contains_lanelet(path_lanelets, following.id())) ignored_lanelets.push_back(following);
   // ignore lanelets directly behind ego
   const auto behind =
     planning_utils::calculateOffsetPoint2d(ego_data.pose, params.rear_offset, 0.0);
@@ -55,26 +68,14 @@ lanelet::ConstLanelets calculate_other_lanelets(
   const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y);
   const auto lanelets_within_range = lanelet::geometry::findWithin2d(
     route_handler.getLaneletMapPtr()->laneletLayer, ego_point,
-    std::max(params.slow_dist_threshold, params.stop_dist_threshold));
-  const auto is_overlapped_by_path_lanelets = [&](const auto & l) {
-    for (const auto & path_ll : path_lanelets) {
-      if (
-        boost::geometry::overlaps(
-          path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) ||
-        boost::geometry::within(path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) ||
-        boost::geometry::within(l.polygon2d().basicPolygon(), path_ll.polygon2d().basicPolygon())) {
-        return true;
-      }
-    }
-    return false;
-  };
+    std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset +
+      params.extra_front_offset);
   for (const auto & ll : lanelets_within_range) {
     if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road")
       continue;
     const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id());
     const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id());
-    if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second))
-      other_lanelets.push_back(ll.second);
+    if (!is_path_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second);
   }
   return other_lanelets;
 }

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp

@@ -33,6 +33,14 @@ 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
+/// @param [in] ego_data data about the ego vehicle
+/// @param [in] route_handler route handler
+/// @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 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/overlapping_range.cpp

@@ -34,6 +34,7 @@ Overlap calculate_overlap(
   Overlap overlap;
   const auto & left_bound = lanelet.leftBound2d().basicLineString();
   const auto & right_bound = lanelet.rightBound2d().basicLineString();
+  // TODO(Maxime): these intersects (for each path footprint, for each lanelet) are too expensive
   const auto overlap_left = boost::geometry::intersects(path_footprint, left_bound);
   const auto overlap_right = boost::geometry::intersects(path_footprint, right_bound);
 

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

@@ -66,7 +66,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   ego_data.pose = planner_data_->current_odometry->pose;
   ego_data.path.points = path->points;
   ego_data.first_path_idx =
-    motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position);
+    motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position);
   motion_utils::removeOverlapPoints(ego_data.path.points);
   ego_data.velocity = planner_data_->current_velocity->twist.linear.x;
   ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold;
@@ -75,13 +75,13 @@ 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);
+  stopwatch.tic("calculate_lanelets");
+  const auto path_lanelets = calculate_path_lanelets(ego_data, *planner_data_->route_handler_);
   const auto ignored_lanelets =
     calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_);
   const auto other_lanelets = calculate_other_lanelets(
     ego_data, path_lanelets, ignored_lanelets, *planner_data_->route_handler_, params_);
+  const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets");
 
   debug_data_.footprints = path_footprints;
   debug_data_.path_lanelets = path_lanelets;
@@ -98,6 +98,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
       });
     if (overlapped_lanelet_it != other_lanelets.end()) {
       debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it);
+      // TODO(Maxime): we may want to just add the overlapped lane to the ignored lanelets
       RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module ()\n");
       return true;
     }
@@ -183,13 +184,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   RCLCPP_DEBUG(
     logger_,
     "Total time = %2.2fus\n"
+    "\tcalculate_lanelets = %2.0fus\n"
     "\tcalculate_path_footprints = %2.0fus\n"
     "\tcalculate_overlapping_ranges = %2.0fus\n"
     "\tcalculate_decisions = %2.0fus\n"
     "\tcalc_slowdown_points = %2.0fus\n"
     "\tinsert_slowdown_points = %2.0fus\n",
-    total_time_us, calculate_path_footprints_us, calculate_overlapping_ranges_us,
-    calculate_decisions_us, calc_slowdown_points_us, insert_slowdown_points_us);
+    total_time_us, calculate_lanelets_us, calculate_path_footprints_us,
+    calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us,
+    insert_slowdown_points_us);
   return true;
 }