From aa8d4b03ad7dfb6172b45ef7527ebdadf12f7bbd Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome Date: Tue, 3 Dec 2024 23:13:07 +0900 Subject: [PATCH] feat(autoware_geography_utils)!: move autoware_geography_utils to autoware.core Signed-off-by: Ryohsuke Mitsudome --- common/autoware_geography_utils/CHANGELOG.rst | 15 -- .../autoware_geography_utils/CMakeLists.txt | 37 ---- common/autoware_geography_utils/README.md | 5 - .../autoware/geography_utils/height.hpp | 33 ---- .../geography_utils/lanelet2_projector.hpp | 32 ---- .../autoware/geography_utils/projection.hpp | 33 ---- common/autoware_geography_utils/package.xml | 33 ---- .../autoware_geography_utils/src/height.cpp | 63 ------- .../src/lanelet2_projector.cpp | 54 ------ .../src/projection.cpp | 95 ----------- .../test/test_geography_utils.cpp | 26 --- .../test/test_height.cpp | 86 ---------- .../test/test_projection.cpp | 161 ------------------ 13 files changed, 673 deletions(-) delete mode 100644 common/autoware_geography_utils/CHANGELOG.rst delete mode 100644 common/autoware_geography_utils/CMakeLists.txt delete mode 100644 common/autoware_geography_utils/README.md delete mode 100644 common/autoware_geography_utils/include/autoware/geography_utils/height.hpp delete mode 100644 common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp delete mode 100644 common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp delete mode 100644 common/autoware_geography_utils/package.xml delete mode 100644 common/autoware_geography_utils/src/height.cpp delete mode 100644 common/autoware_geography_utils/src/lanelet2_projector.cpp delete mode 100644 common/autoware_geography_utils/src/projection.cpp delete mode 100644 common/autoware_geography_utils/test/test_geography_utils.cpp delete mode 100644 common/autoware_geography_utils/test/test_height.cpp delete mode 100644 common/autoware_geography_utils/test/test_projection.cpp diff --git a/common/autoware_geography_utils/CHANGELOG.rst b/common/autoware_geography_utils/CHANGELOG.rst deleted file mode 100644 index 623861bbd01c3..0000000000000 --- a/common/autoware_geography_utils/CHANGELOG.rst +++ /dev/null @@ -1,15 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_geography_utils -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(geography_utils): prefix package and namespace with autoware (`#7790 `_) - * refactor(geography_utils): prefix package and namespace with autoware - * move headers to include/autoware/ - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_geography_utils/CMakeLists.txt b/common/autoware_geography_utils/CMakeLists.txt deleted file mode 100644 index b4ab5c2f74494..0000000000000 --- a/common/autoware_geography_utils/CMakeLists.txt +++ /dev/null @@ -1,37 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_geography_utils) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -# GeographicLib -find_package(PkgConfig) -find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h - PATH_SUFFIXES GeographicLib -) -set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) -find_library(GeographicLib_LIBRARIES NAMES Geographic) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/height.cpp - src/projection.cpp - src/lanelet2_projector.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${GeographicLib_LIBRARIES} -) - -if(BUILD_TESTING) - find_package(ament_cmake_ros REQUIRED) - - file(GLOB_RECURSE test_files test/*.cpp) - - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME} - ) -endif() - -ament_auto_package() diff --git a/common/autoware_geography_utils/README.md b/common/autoware_geography_utils/README.md deleted file mode 100644 index fb4c2dc3a8312..0000000000000 --- a/common/autoware_geography_utils/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# geography_utils - -## Purpose - -This package contains geography-related functions used by other packages, so please refer to them as needed. diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp deleted file mode 100644 index 1f205eb8f8b18..0000000000000 --- a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ -#define AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ - -#include - -namespace autoware::geography_utils -{ - -typedef double (*HeightConversionFunction)( - const double height, const double latitude, const double longitude); -double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude); -double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude); -double convert_height( - const double height, const double latitude, const double longitude, - const std::string & source_vertical_datum, const std::string & target_vertical_datum); - -} // namespace autoware::geography_utils - -#endif // AUTOWARE__GEOGRAPHY_UTILS__HEIGHT_HPP_ diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp deleted file mode 100644 index 0eea2a9ff7fbb..0000000000000 --- a/common/autoware_geography_utils/include/autoware/geography_utils/lanelet2_projector.hpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ -#define AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ - -#include - -#include - -#include - -namespace autoware::geography_utils -{ -using MapProjectorInfo = autoware_map_msgs::msg::MapProjectorInfo; - -std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info); - -} // namespace autoware::geography_utils - -#endif // AUTOWARE__GEOGRAPHY_UTILS__LANELET2_PROJECTOR_HPP_ diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp deleted file mode 100644 index 5c4a69b15e192..0000000000000 --- a/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ -#define AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ - -#include -#include -#include - -namespace autoware::geography_utils -{ -using MapProjectorInfo = autoware_map_msgs::msg::MapProjectorInfo; -using GeoPoint = geographic_msgs::msg::GeoPoint; -using LocalPoint = geometry_msgs::msg::Point; - -LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info); -GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info); - -} // namespace autoware::geography_utils - -#endif // AUTOWARE__GEOGRAPHY_UTILS__PROJECTION_HPP_ diff --git a/common/autoware_geography_utils/package.xml b/common/autoware_geography_utils/package.xml deleted file mode 100644 index 8777bf4c13d45..0000000000000 --- a/common/autoware_geography_utils/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - - - autoware_geography_utils - 0.38.0 - The autoware_geography_utils package - Yamato Ando - Masahiro Sakamoto - NGUYEN Viet Anh - Taiki Yamada - Shintaro Sakoda - Ryu Yamamoto - Apache License 2.0 - Koji Minoda - - ament_cmake_auto - autoware_cmake - - autoware_lanelet2_extension - autoware_map_msgs - geographic_msgs - geographiclib - geometry_msgs - lanelet2_io - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_geography_utils/src/height.cpp b/common/autoware_geography_utils/src/height.cpp deleted file mode 100644 index 745dbf5b22cfc..0000000000000 --- a/common/autoware_geography_utils/src/height.cpp +++ /dev/null @@ -1,63 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include -#include -#include - -namespace autoware::geography_utils -{ - -double convert_wgs84_to_egm2008(const double height, const double latitude, const double longitude) -{ - GeographicLib::Geoid egm2008("egm2008-1"); - // cSpell: ignore ELLIPSOIDTOGEOID - return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::ELLIPSOIDTOGEOID); -} - -double convert_egm2008_to_wgs84(const double height, const double latitude, const double longitude) -{ - GeographicLib::Geoid egm2008("egm2008-1"); - // cSpell: ignore GEOIDTOELLIPSOID - return egm2008.ConvertHeight(latitude, longitude, height, GeographicLib::Geoid::GEOIDTOELLIPSOID); -} - -double convert_height( - const double height, const double latitude, const double longitude, - const std::string & source_vertical_datum, const std::string & target_vertical_datum) -{ - if (source_vertical_datum == target_vertical_datum) { - return height; - } - std::map, HeightConversionFunction> conversion_map; - conversion_map[{"WGS84", "EGM2008"}] = convert_wgs84_to_egm2008; - conversion_map[{"EGM2008", "WGS84"}] = convert_egm2008_to_wgs84; - - auto key = std::make_pair(source_vertical_datum, target_vertical_datum); - if (conversion_map.find(key) != conversion_map.end()) { - return conversion_map[key](height, latitude, longitude); - } else { - std::string error_message = - "Invalid conversion types: " + std::string(source_vertical_datum.c_str()) + " to " + - std::string(target_vertical_datum.c_str()); - - throw std::invalid_argument(error_message); - } -} - -} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/src/lanelet2_projector.cpp b/common/autoware_geography_utils/src/lanelet2_projector.cpp deleted file mode 100644 index 7de0935a3aa4e..0000000000000 --- a/common/autoware_geography_utils/src/lanelet2_projector.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include - -namespace autoware::geography_utils -{ - -std::unique_ptr get_lanelet2_projector(const MapProjectorInfo & projector_info) -{ - if (projector_info.projector_type == MapProjectorInfo::LOCAL_CARTESIAN_UTM) { - lanelet::GPSPoint position{ - projector_info.map_origin.latitude, projector_info.map_origin.longitude, - projector_info.map_origin.altitude}; - lanelet::Origin origin{position}; - lanelet::projection::UtmProjector projector{origin}; - return std::make_unique(projector); - - } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { - lanelet::projection::MGRSProjector projector{}; - projector.setMGRSCode(projector_info.mgrs_grid); - return std::make_unique(projector); - - } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { - lanelet::GPSPoint position{ - projector_info.map_origin.latitude, projector_info.map_origin.longitude, - projector_info.map_origin.altitude}; - lanelet::Origin origin{position}; - lanelet::projection::TransverseMercatorProjector projector{origin}; - return std::make_unique(projector); - } - const std::string error_msg = - "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"; - throw std::invalid_argument(error_msg); -} - -} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/src/projection.cpp b/common/autoware_geography_utils/src/projection.cpp deleted file mode 100644 index 3ab18b1d31698..0000000000000 --- a/common/autoware_geography_utils/src/projection.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -namespace autoware::geography_utils -{ - -Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) -{ - Eigen::Vector3d dst; - dst.x() = src.x; - dst.y() = src.y; - dst.z() = src.z; - return dst; -} - -LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) -{ - std::unique_ptr projector = get_lanelet2_projector(projector_info); - lanelet::GPSPoint position{geo_point.latitude, geo_point.longitude, geo_point.altitude}; - - lanelet::BasicPoint3d projected_local_point; - if (projector_info.projector_type == MapProjectorInfo::MGRS) { - const int mgrs_precision = 9; // set precision as 100 micro meter - const auto mgrs_projector = dynamic_cast(projector.get()); - - // project x and y using projector - // note that the altitude is ignored in MGRS projection conventionally - projected_local_point = mgrs_projector->forward(position, mgrs_precision); - } else { - // project x and y using projector - // note that the original projector such as UTM projector does not compensate for the altitude - // offset - projected_local_point = projector->forward(position); - - // correct z based on the map origin - // note that the converted altitude in local point is in the same vertical datum as the geo - // point - projected_local_point.z() = geo_point.altitude - projector_info.map_origin.altitude; - } - - LocalPoint local_point; - local_point.x = projected_local_point.x(); - local_point.y = projected_local_point.y(); - local_point.z = projected_local_point.z(); - - return local_point; -} - -GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo & projector_info) -{ - std::unique_ptr projector = get_lanelet2_projector(projector_info); - - lanelet::GPSPoint projected_gps_point; - if (projector_info.projector_type == MapProjectorInfo::MGRS) { - const auto mgrs_projector = dynamic_cast(projector.get()); - // project latitude and longitude using projector - // note that the z is ignored in MGRS projection conventionally - projected_gps_point = - mgrs_projector->reverse(to_basic_point_3d_pt(local_point), projector_info.mgrs_grid); - } else { - // project latitude and longitude using projector - // note that the original projector such as UTM projector does not compensate for the altitude - // offset - projected_gps_point = projector->reverse(to_basic_point_3d_pt(local_point)); - - // correct altitude based on the map origin - // note that the converted altitude in local point is in the same vertical datum as the geo - // point - projected_gps_point.ele = local_point.z + projector_info.map_origin.altitude; - } - - GeoPoint geo_point; - geo_point.latitude = projected_gps_point.lat; - geo_point.longitude = projected_gps_point.lon; - geo_point.altitude = projected_gps_point.ele; - return geo_point; -} - -} // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/test/test_geography_utils.cpp b/common/autoware_geography_utils/test/test_geography_utils.cpp deleted file mode 100644 index ee0e7428db2f2..0000000000000 --- a/common/autoware_geography_utils/test/test_geography_utils.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/geography_utils/height.hpp" -#include "autoware/geography_utils/lanelet2_projector.hpp" -#include "autoware/geography_utils/projection.hpp" - -#include - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - bool result = RUN_ALL_TESTS(); - return result; -} diff --git a/common/autoware_geography_utils/test/test_height.cpp b/common/autoware_geography_utils/test/test_height.cpp deleted file mode 100644 index f624f6c3ff9e3..0000000000000 --- a/common/autoware_geography_utils/test/test_height.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include -#include - -// Test case to verify if same source and target datums return original height -TEST(GeographyUtils, SameSourceTargetDatum) -{ - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - const std::string datum = "WGS84"; - - double converted_height = - autoware::geography_utils::convert_height(height, latitude, longitude, datum, datum); - - EXPECT_DOUBLE_EQ(height, converted_height); -} - -// Test case to verify valid source and target datums -TEST(GeographyUtils, ValidSourceTargetDatum) -{ - // Calculated with - // https://www.unavco.org/software/geodetic-utilities/geoid-height-calculator/geoid-height-calculator.html - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - const double target_height = -30.18; - - double converted_height = - autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "EGM2008"); - - EXPECT_NEAR(target_height, converted_height, 0.1); -} - -// Test case to verify invalid source and target datums -TEST(GeographyUtils, InvalidSourceTargetDatum) -{ - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - - EXPECT_THROW( - autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "INVALID2"), - std::invalid_argument); -} - -// Test case to verify invalid source datums -TEST(GeographyUtils, InvalidSourceDatum) -{ - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - - EXPECT_THROW( - autoware::geography_utils::convert_height(height, latitude, longitude, "INVALID1", "WGS84"), - std::invalid_argument); -} - -// Test case to verify invalid target datums -TEST(GeographyUtils, InvalidTargetDatum) -{ - const double height = 10.0; - const double latitude = 35.0; - const double longitude = 139.0; - - EXPECT_THROW( - autoware::geography_utils::convert_height(height, latitude, longitude, "WGS84", "INVALID2"), - std::invalid_argument); -} diff --git a/common/autoware_geography_utils/test/test_projection.cpp b/common/autoware_geography_utils/test/test_projection.cpp deleted file mode 100644 index b8d036c136eeb..0000000000000 --- a/common/autoware_geography_utils/test/test_projection.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include - -#include -#include - -TEST(GeographyUtilsProjection, ProjectForwardToMGRS) -{ - // source point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62426; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // target point - geometry_msgs::msg::Point local_point; - local_point.x = 86128.0; - local_point.y = 43002.0; - local_point.z = 10.0; - - // projector info - autoware_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS; - projector_info.mgrs_grid = "54SUE"; - projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; - - // conversion - const geometry_msgs::msg::Point converted_point = - autoware::geography_utils::project_forward(geo_point, projector_info); - - EXPECT_NEAR(converted_point.x, local_point.x, 1.0); - EXPECT_NEAR(converted_point.y, local_point.y, 1.0); - EXPECT_NEAR(converted_point.z, local_point.z, 1.0); -} - -TEST(GeographyUtilsProjection, ProjectReverseFromMGRS) -{ - // source point - geometry_msgs::msg::Point local_point; - local_point.x = 86128.0; - local_point.y = 43002.0; - local_point.z = 10.0; - - // target point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62426; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // projector info - autoware_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS; - projector_info.mgrs_grid = "54SUE"; - projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; - - // conversion - const geographic_msgs::msg::GeoPoint converted_point = - autoware::geography_utils::project_reverse(local_point, projector_info); - - EXPECT_NEAR(converted_point.latitude, geo_point.latitude, 0.0001); - EXPECT_NEAR(converted_point.longitude, geo_point.longitude, 0.0001); - EXPECT_NEAR(converted_point.altitude, geo_point.altitude, 0.0001); -} - -TEST(GeographyUtilsProjection, ProjectForwardAndReverseMGRS) -{ - // source point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62426; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // projector info - autoware_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::MGRS; - projector_info.mgrs_grid = "54SUE"; - projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; - - // conversion - const geometry_msgs::msg::Point converted_local_point = - autoware::geography_utils::project_forward(geo_point, projector_info); - const geographic_msgs::msg::GeoPoint converted_geo_point = - autoware::geography_utils::project_reverse(converted_local_point, projector_info); - - EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); - EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); - EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); -} - -TEST(GeographyUtilsProjection, ProjectForwardToLocalCartesianUTMOrigin) -{ - // source point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62406; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // target point - geometry_msgs::msg::Point local_point; - local_point.x = 0.0; - local_point.y = -22.18; - local_point.z = 20.0; - - // projector info - autoware_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; - projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; - projector_info.map_origin.latitude = 35.62426; - projector_info.map_origin.longitude = 139.74252; - projector_info.map_origin.altitude = -10.0; - - // conversion - const geometry_msgs::msg::Point converted_point = - autoware::geography_utils::project_forward(geo_point, projector_info); - - EXPECT_NEAR(converted_point.x, local_point.x, 1.0); - EXPECT_NEAR(converted_point.y, local_point.y, 1.0); - EXPECT_NEAR(converted_point.z, local_point.z, 1.0); -} - -TEST(GeographyUtilsProjection, ProjectForwardAndReverseLocalCartesianUTMOrigin) -{ - // source point - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.62426; - geo_point.longitude = 139.74252; - geo_point.altitude = 10.0; - - // projector info - autoware_map_msgs::msg::MapProjectorInfo projector_info; - projector_info.projector_type = autoware_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM; - projector_info.vertical_datum = autoware_map_msgs::msg::MapProjectorInfo::WGS84; - projector_info.map_origin.latitude = 35.0; - projector_info.map_origin.longitude = 139.0; - projector_info.map_origin.altitude = 0.0; - - // conversion - const geometry_msgs::msg::Point converted_local_point = - autoware::geography_utils::project_forward(geo_point, projector_info); - const geographic_msgs::msg::GeoPoint converted_geo_point = - autoware::geography_utils::project_reverse(converted_local_point, projector_info); - - EXPECT_NEAR(converted_geo_point.latitude, geo_point.latitude, 0.0001); - EXPECT_NEAR(converted_geo_point.longitude, geo_point.longitude, 0.0001); - EXPECT_NEAR(converted_geo_point.altitude, geo_point.altitude, 0.0001); -}