Calculate path lanelets faster and do not ignore following lanelets

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

Author: maxime-clem

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp

@@ -25,23 +25,18 @@
 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 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);
-  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);
+  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;
 }
@@ -51,10 +46,6 @@ lanelet::ConstLanelets calculate_ignored_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);

planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp

@@ -35,15 +35,12 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane
 };
 
 /// @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
+/// @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
-/// @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);
+  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/scene_out_of_lane.cpp

@@ -56,6 +56,19 @@ OutOfLaneModule::OutOfLaneModule(
   velocity_factor_.init(PlanningBehavior::UNKNOWN);
 }
 
+size_t calculate_index_ahead(
+  const std::vector<PathPointWithLaneId> & points, const size_t start, const double dist_ahead)
+{
+  auto last_idx = start;
+  auto length = 0.0;
+  while (length < dist_ahead && last_idx + 1 < points.size()) {
+    length +=
+      tier4_autoware_utils::calcDistance2d(points[last_idx].point, points[last_idx + 1].point);
+    ++last_idx;
+  }
+  return last_idx;
+}
+
 bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
 {
   debug_data_.reset_data();
@@ -68,15 +81,9 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   ego_data.path.points = path->points;
   ego_data.first_path_idx =
     motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position);
-  auto last_idx = ego_data.first_path_idx;
-  auto length = 0.0;
-  while (length < std::max(params_.slow_dist_threshold, params_.stop_dist_threshold) &&
-         last_idx + 1 < path->points.size()) {
-    length += tier4_autoware_utils::calcDistance2d(
-      path->points[last_idx].point, path->points[last_idx + 1].point);
-    ++last_idx;
-  }
-  ego_data.path.points.resize(last_idx + 1);
+  ego_data.path.points.resize(calculate_index_ahead(
+    path->points, ego_data.first_path_idx,
+    std::max(params_.slow_dist_threshold, params_.stop_dist_threshold)));
   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;
@@ -86,8 +93,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints");
   // Calculate lanelets to ignore and consider
   stopwatch.tic("calculate_lanelets");
-  const auto path_lanelets =
-    calculate_path_lanelets(ego_data, *planner_data_->route_handler_, current_ego_footprint);
+  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(
@@ -191,7 +197,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
   debug_data_.ranges = inputs.ranges;
 
   const auto total_time_us = stopwatch.toc();
-  RCLCPP_DEBUG(
+  RCLCPP_WARN(
     logger_,
     "Total time = %2.2fus\n"
     "\tcalculate_lanelets = %2.0fus\n"