From ea52340fe23fa197fc0a23a1e5b98556ebe20b3f Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 9 Aug 2024 10:24:26 +0900 Subject: [PATCH] revert (map_based_prediction): use linear interpolation for path conversion (#8400)" (#8417) Revert "perf(map_based_prediction): use linear interpolation for path conversion (#8400)" This reverts commit 147403f1765346be9c5a3273552d86133298a899. Signed-off-by: Taekjin LEE --- .../src/map_based_prediction_node.cpp | 34 +++++-------------- 1 file changed, 8 insertions(+), 26 deletions(-) diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 6b7b3489bf051..d22c59082e5db 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -2402,15 +2402,9 @@ std::vector MapBasedPredictionNode::convertPathType( continue; } - const double dx = current_p.position.x - prev_p.position.x; - const double dy = current_p.position.y - prev_p.position.y; - const double sin_yaw_half = dy / (std::sqrt(dx * dx + dy * dy) + dx); - const double cos_yaw_half = std::sqrt(1 - sin_yaw_half * sin_yaw_half); - current_p.orientation.x = 0.0; - current_p.orientation.y = 0.0; - current_p.orientation.z = sin_yaw_half; - current_p.orientation.w = cos_yaw_half; - + const double lane_yaw = std::atan2( + current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); + current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); prev_p = current_p; } @@ -2439,29 +2433,17 @@ std::vector MapBasedPredictionNode::convertPathType( } } - const double dx = current_p.position.x - prev_p.position.x; - const double dy = current_p.position.y - prev_p.position.y; - const double sin_yaw_half = dy / (std::sqrt(dx * dx + dy * dy) + dx); - const double cos_yaw_half = std::sqrt(1 - sin_yaw_half * sin_yaw_half); - current_p.orientation.x = 0.0; - current_p.orientation.y = 0.0; - current_p.orientation.z = sin_yaw_half; - current_p.orientation.w = cos_yaw_half; - + const double lane_yaw = std::atan2( + current_p.position.y - prev_p.position.y, current_p.position.x - prev_p.position.x); + current_p.orientation = autoware::universe_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); prev_p = current_p; } } // Resample Path - const bool use_akima_spline_for_xy = true; - const bool use_lerp_for_z = true; - // the options use_akima_slpine_for_xy and use_lerp_for_z are set to true - // but the implementation of use_akima_slpine_for_xy in resamplePoseVector and - // resamplePointVector is opposite to the options so the options are set to true to use linear - // interpolation for xy - const auto resampled_converted_path = autoware::motion_utils::resamplePoseVector( - converted_path, reference_path_resolution_, use_akima_spline_for_xy, use_lerp_for_z); + const auto resampled_converted_path = + autoware::motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); converted_paths.push_back(resampled_converted_path); }