fix(avoidance): use find nearest index to calculate object's overhang…

… & longitudinal (autowarefoundation#4874) (autowarefoundation#906)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -396,7 +396,8 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
   double max_distance = std::numeric_limits<double>::lowest();
   for (const auto & p : obj.envelope_poly.outer()) {
     const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
-    const double arc_length = calcSignedArcLengthToFirstNearestPoint(path.points, ego_pos, point);
+    // TODO(someone): search around first position where the ego should avoid the object.
+    const double arc_length = calcSignedArcLength(path.points, ego_pos, point);
     min_distance = std::min(min_distance, arc_length);
     max_distance = std::max(max_distance, arc_length);
   }
@@ -412,7 +413,8 @@ double calcEnvelopeOverhangDistance(
 
   for (const auto & p : object_data.envelope_poly.outer()) {
     const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0);
-    const auto idx = findFirstNearestIndex(path.points, point);
+    // TODO(someone): search around first position where the ego should avoid the object.
+    const auto idx = findNearestIndex(path.points, point);
     const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point);
 
     const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() {