From 92bec0dc0832fd66b77b5bffac19dc3b9685e3fb Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 30 Sep 2024 09:35:05 +0900 Subject: [PATCH] fix(autoware_map_based_prediction): adjust lateral duration when object is behind reference path (#8973) fix: adjust lateral duration when object is behind reference path Signed-off-by: Taekjin LEE --- .../src/path_generator.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 210795d7e6b25..a2a1b8b3d3fda 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/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);