fix(autoware_map_based_prediction): adjust lateral duration when obje…

…ct is behind reference path (autowarefoundation#8973)

fix: adjust lateral duration when object is behind reference path

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

perception/autoware_map_based_prediction/src/path_generator.cpp

@@ -250,6 +250,15 @@ PredictedPath PathGenerator::generatePolynomialPath(
   terminal_point.d_vel = 0.0;
   terminal_point.d_acc = 0.0;
 
+  // if the object is behind of the reference path adjust the lateral_duration to reach the start of
+  // the reference path
+  double lateral_duration_adjusted = lateral_duration;
+  if (current_point.s < 0.0) {
+    const double distance_to_start = -current_point.s;
+    const double duration_to_reach = distance_to_start / terminal_point.s_vel;
+    lateral_duration_adjusted = std::max(lateral_duration, duration_to_reach);
+  }
+
   // calculate terminal d position, based on backlash width
   {
     if (backlash_width < 0.01 /*m*/) {
@@ -259,7 +268,7 @@ PredictedPath PathGenerator::generatePolynomialPath(
     } else {
       const double return_width = path_width / 2.0;  // [m]
       const double current_momentum_d =
-        current_point.d + 0.5 * current_point.d_vel * lateral_duration;
+        current_point.d + 0.5 * current_point.d_vel * lateral_duration_adjusted;
       const double momentum_d_abs = std::abs(current_momentum_d);
 
       if (momentum_d_abs < backlash_width) {
@@ -282,8 +291,8 @@ PredictedPath PathGenerator::generatePolynomialPath(
   }
 
   // Step 2. Generate Predicted Path on a Frenet coordinate
-  const auto frenet_predicted_path =
-    generateFrenetPath(current_point, terminal_point, ref_path_len, duration, lateral_duration);
+  const auto frenet_predicted_path = generateFrenetPath(
+    current_point, terminal_point, ref_path_len, duration, lateral_duration_adjusted);
 
   // Step 3. Interpolate Reference Path for converting predicted path coordinate
   const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path);