From 2814e8c672bdfb8d35f5f4ddd95154dc0d0c3577 Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Fri, 6 Dec 2024 06:25:15 +0900 Subject: [PATCH 1/2] test with different elevations Signed-off-by: Zhe Shen --- .../test/src/test_projector.cpp | 62 +++++++------------ 1 file changed, 24 insertions(+), 38 deletions(-) diff --git a/autoware_lanelet2_extension/test/src/test_projector.cpp b/autoware_lanelet2_extension/test/src/test_projector.cpp index acf08ac..49ca398 100644 --- a/autoware_lanelet2_extension/test/src/test_projector.cpp +++ b/autoware_lanelet2_extension/test/src/test_projector.cpp @@ -32,52 +32,38 @@ class TestSuite : public ::testing::Test // NOLINT for gtest TEST(TestSuite, ForwardMGRSProjection) // NOLINT for gtest { lanelet::projection::MGRSProjector projector; - // lat/lon in Tokyo - lanelet::GPSPoint gps_point; - gps_point.lat = 35.652832; - gps_point.lon = 139.839478; - gps_point.ele = 12.3456789; - lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point); - - // projected z value should not change - ASSERT_DOUBLE_EQ(mgrs_point.z(), gps_point.ele) - << "Forward projected z value should be " << gps_point.ele; - - // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html - // round the projected value to mm since the above reference only gives value - // in mm precision - ASSERT_EQ(projector.getProjectedMGRSGrid(), "54SUE") << "Projected grid should be " - << "54SUE"; - double rounded_x_mm = round(mgrs_point.x() * 1000) / 1000.0; - ASSERT_DOUBLE_EQ(rounded_x_mm, 94946.081) << "Forward projected x value should be " << 94946.081; - double rounded_y_mm = round(mgrs_point.y() * 1000) / 1000.0; - ASSERT_DOUBLE_EQ(rounded_y_mm, 46063.748) << "Forward projected y value should be " << 46063.748; + // different elevation values + std::vector elevation_values = {0.0, -50.0, 1000.0}; // sea level, below sea level, very high + for (const double ele : elevation_values) { + lanelet::GPSPoint gps_point; + gps_point.lat = 35.652832; + gps_point.lon = 139.839478; + gps_point.ele = ele; + + lanelet::BasicPoint3d mgrs_point = projector.forward(gps_point); + + ASSERT_DOUBLE_EQ(mgrs_point.z(), gps_point.ele) + << "Forward projected z value should be " << gps_point.ele << " for elevation " << ele; + } } TEST(TestSuite, ReverseMGRSProjection) // NOLINT for gtest { lanelet::projection::MGRSProjector projector; - lanelet::BasicPoint3d mgrs_point; - mgrs_point.x() = 94946.0; - mgrs_point.y() = 46063.0; - mgrs_point.z() = 12.3456789; + std::vector elevation_values = {0.0, -50.0, 1000.0}; // sea level, below sea level, very high - projector.setMGRSCode("54SUE"); - lanelet::GPSPoint gps_point = projector.reverse(mgrs_point); + for (const double ele : elevation_values) { + lanelet::BasicPoint3d mgrs_point; + mgrs_point.x() = 94946.0; + mgrs_point.y() = 46063.0; + mgrs_point.z() = ele; - // projected z value should not change - ASSERT_DOUBLE_EQ(gps_point.ele, mgrs_point.z()) - << "Reverse projected z value should be " << mgrs_point.z(); + projector.setMGRSCode("54SUE"); + lanelet::GPSPoint gps_point = projector.reverse(mgrs_point); - // https://www.movable-type.co.uk/scripts/latlong-utm-mgrs.html - // round the projected value since the above reference only gives value up to - // precision of 1e-8 - double rounded_lat = round(gps_point.lat * 1e8) / 1e8; - ASSERT_DOUBLE_EQ(rounded_lat, 35.65282525) - << "Reverse projected latitude value should be " << 35.65282525; - double rounded_lon = round(gps_point.lon * 1e8) / 1e8; - ASSERT_DOUBLE_EQ(rounded_lon, 139.83947721) - << "Reverse projected longitude value should be " << 139.83947721; + ASSERT_DOUBLE_EQ(gps_point.ele, mgrs_point.z()) + << "Reverse projected z value should be " << mgrs_point.z() << " for elevation " << ele; + } } TEST(TestSuite, ForwardTransverseMercatorProjection) // NOLINT for gtest From 2ad378fbed40462ab9759550b205deb00fc36729 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Dec 2024 21:28:39 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- autoware_lanelet2_extension/test/src/test_projector.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/autoware_lanelet2_extension/test/src/test_projector.cpp b/autoware_lanelet2_extension/test/src/test_projector.cpp index 49ca398..40fb214 100644 --- a/autoware_lanelet2_extension/test/src/test_projector.cpp +++ b/autoware_lanelet2_extension/test/src/test_projector.cpp @@ -33,7 +33,8 @@ TEST(TestSuite, ForwardMGRSProjection) // NOLINT for gtest { lanelet::projection::MGRSProjector projector; // different elevation values - std::vector elevation_values = {0.0, -50.0, 1000.0}; // sea level, below sea level, very high + std::vector elevation_values = { + 0.0, -50.0, 1000.0}; // sea level, below sea level, very high for (const double ele : elevation_values) { lanelet::GPSPoint gps_point; gps_point.lat = 35.652832; @@ -50,7 +51,8 @@ TEST(TestSuite, ForwardMGRSProjection) // NOLINT for gtest TEST(TestSuite, ReverseMGRSProjection) // NOLINT for gtest { lanelet::projection::MGRSProjector projector; - std::vector elevation_values = {0.0, -50.0, 1000.0}; // sea level, below sea level, very high + std::vector elevation_values = { + 0.0, -50.0, 1000.0}; // sea level, below sea level, very high for (const double ele : elevation_values) { lanelet::BasicPoint3d mgrs_point;