limit the path size and record time to calculate lanelets

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

Author: maxime-clem

planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp

@@ -36,6 +36,7 @@
 
 #include <lanelet2_core/geometry/LaneletMap.h>
 
+#include <algorithm>
 #include <memory>
 #include <string>
 #include <utility>
@@ -66,7 +67,16 @@ 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);
+  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);
   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,12 +85,14 @@ 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
+  stopwatch.tic("calculate_lanelets");
   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(
     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;
@@ -182,13 +194,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;
 }