From 329dd877f216a9ef001ffa0508c946e0918be79d Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Thu, 20 Jan 2022 09:48:36 +0900 Subject: [PATCH] fix(lanelet2_extension): fix getAngleDifference (#264) * chore(query): update and add gtest Signed-off-by: tanaka3 * ci(pre-commit): autofix * chore(query): add deprecated attribute to getAngleDifference Signed-off-by: tanaka3 * chore(query): deprecated as comment to avoid clang errror Signed-off-by: tanaka3 * Revert "chore(query): deprecated as comment to avoid clang errror" This reverts commit 28cc95d75ae55ddeef3813c91c18f0a02b95a41d. Signed-off-by: tanaka3 * Revert "chore(query): add deprecated attribute to getAngleDifference" This reverts commit 3b56ce7a91d76a029eadc190b224a295c01eb02f. Signed-off-by: tanaka3 * Revert "ci(pre-commit): autofix" This reverts commit 61e05c4718eef343f8ba198fe46e501048e0b784. Signed-off-by: tanaka3 * Revert "chore(query): update and add gtest" This reverts commit 027c4cf53883c233f28da75c633e53278ce2a1ca. Signed-off-by: tanaka3 * fix(lanelet2_extension): fix getAngleDifference Signed-off-by: tanaka3 * fix(lanelet2_extension): ref value Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: tanaka3 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- map/lanelet2_extension/lib/query.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/map/lanelet2_extension/lib/query.cpp b/map/lanelet2_extension/lib/query.cpp index 95ad8d5ba40d4..d24ebb3aa3703 100644 --- a/map/lanelet2_extension/lib/query.cpp +++ b/map/lanelet2_extension/lib/query.cpp @@ -34,17 +34,6 @@ #include using lanelet::utils::to2D; -namespace -{ -double getAngleDifference(const double angle1, const double angle2) -{ - const double normalized_angle1 = tier4_autoware_utils::normalizeRadian(angle1); - const double normalized_angle2 = tier4_autoware_utils::normalizeRadian(angle2); - const double diff_angle = std::fabs(normalized_angle1 - normalized_angle2); - return diff_angle; -} - -} // namespace namespace lanelet { @@ -727,7 +716,7 @@ bool query::getClosestLanelet( lanelet::ConstLineString3d segment = getClosestSegment(search_point, llt.centerline()); double segment_angle = std::atan2( segment.back().y() - segment.front().y(), segment.back().x() - segment.front().x()); - double angle_diff = getAngleDifference(segment_angle, pose_yaw); + double angle_diff = std::abs(tier4_autoware_utils::normalizeRadian(segment_angle - pose_yaw)); if (angle_diff < min_angle) { min_angle = angle_diff; *closest_lanelet_ptr = llt;