From 4f5af69503e7f8da7300727a053eee97e98cec59 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 17 Dec 2024 12:54:24 +0900 Subject: [PATCH 1/6] use using Signed-off-by: Yutaka Kondo --- .../include/autoware/geography_utils/height.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp index 1f205eb8f8..81a9b44d8c 100644 --- a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp @@ -20,8 +20,8 @@ namespace autoware::geography_utils { -typedef double (*HeightConversionFunction)( - const double height, const double latitude, const double longitude); +using HeightConversionFunction = double (*)(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( From cb94c7784ccf35ef7e81c7fe34b8132c2695c033 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 17 Dec 2024 13:24:20 +0900 Subject: [PATCH 2/6] refactor height Signed-off-by: Yutaka Kondo --- .../autoware/geography_utils/height.hpp | 3 ++- .../autoware_geography_utils/src/height.cpp | 26 +++++++++---------- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp index 81a9b44d8c..0b0dbec0ba 100644 --- a/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/height.hpp @@ -20,7 +20,8 @@ namespace autoware::geography_utils { -using HeightConversionFunction = double (*)(const double height, const double latitude, const double longitude); +using HeightConversionFunction = + double (*)(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); diff --git a/common/autoware_geography_utils/src/height.cpp b/common/autoware_geography_utils/src/height.cpp index 745dbf5b22..b0cd615c0d 100644 --- a/common/autoware_geography_utils/src/height.cpp +++ b/common/autoware_geography_utils/src/height.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include namespace autoware::geography_utils @@ -44,20 +45,19 @@ double convert_height( 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); + static const std::map, HeightConversionFunction> + conversion_map{ + {{"WGS84", "EGM2008"}, convert_wgs84_to_egm2008}, + {{"EGM2008", "WGS84"}, convert_egm2008_to_wgs84}, + }; + + const auto key = std::make_pair(source_vertical_datum, target_vertical_datum); + if (const auto it = conversion_map.find(key); it != conversion_map.end()) { + return it->second(height, latitude, longitude); } + + throw std::invalid_argument( + "Invalid conversion types: " + source_vertical_datum + " to " + target_vertical_datum); } } // namespace autoware::geography_utils From f96b8764fbee51282724e430ac3ce9cdcf2618fc Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 17 Dec 2024 16:59:18 +0900 Subject: [PATCH 3/6] refactor projection Signed-off-by: Yutaka Kondo --- .../autoware/geography_utils/projection.hpp | 6 ++++-- .../autoware_geography_utils/src/projection.cpp | 15 ++++++--------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp index 5c4a69b15e..86869dde49 100644 --- a/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp +++ b/common/autoware_geography_utils/include/autoware/geography_utils/projection.hpp @@ -25,8 +25,10 @@ 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); +[[nodiscard]] LocalPoint project_forward( + const GeoPoint & geo_point, const MapProjectorInfo & projector_info); +[[nodiscard]] GeoPoint project_reverse( + const LocalPoint & local_point, const MapProjectorInfo & projector_info); } // namespace autoware::geography_utils diff --git a/common/autoware_geography_utils/src/projection.cpp b/common/autoware_geography_utils/src/projection.cpp index e113c8da7b..872de8a08b 100644 --- a/common/autoware_geography_utils/src/projection.cpp +++ b/common/autoware_geography_utils/src/projection.cpp @@ -22,23 +22,19 @@ namespace autoware::geography_utils { -Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) +[[nodiscard]] 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; + return {src.x, src.y, src.z}; } 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}; + const 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 + constexpr int mgrs_precision = 9; // set precision as 100 micro meter const auto mgrs_projector = dynamic_cast(projector.get()); // project x and y using projector @@ -70,7 +66,8 @@ GeoPoint project_reverse(const LocalPoint & local_point, const MapProjectorInfo lanelet::GPSPoint projected_gps_point; if (projector_info.projector_type == MapProjectorInfo::MGRS) { - const auto mgrs_projector = dynamic_cast(projector.get()); + 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 = From 70a4b62c03b70e749b1d8a60433955eac814d45e Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 17 Dec 2024 17:09:44 +0900 Subject: [PATCH 4/6] refactor lanelet2_projector Signed-off-by: Yutaka Kondo --- .../src/lanelet2_projector.cpp | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/common/autoware_geography_utils/src/lanelet2_projector.cpp b/common/autoware_geography_utils/src/lanelet2_projector.cpp index 76c69a85ae..9eee54b532 100644 --- a/common/autoware_geography_utils/src/lanelet2_projector.cpp +++ b/common/autoware_geography_utils/src/lanelet2_projector.cpp @@ -28,30 +28,32 @@ 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{ + const 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}; + const lanelet::Origin origin{position}; + const lanelet::projection::UtmProjector projector{origin}; return std::make_unique(projector); + } - } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { + 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{ + if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { + const 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}; + const lanelet::Origin origin{position}; + const lanelet::projection::TransverseMercatorProjector projector{origin}; return std::make_unique(projector); } - const std::string error_msg = + + throw std::invalid_argument( "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"; - throw std::invalid_argument(error_msg); + ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"); } } // namespace autoware::geography_utils From 70ed42cac7ac46867d0ccbe9ee66e554ed2298cb Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 18 Dec 2024 11:11:54 +0900 Subject: [PATCH 5/6] set class name Signed-off-by: Yutaka Kondo --- common/autoware_geography_utils/src/projection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_geography_utils/src/projection.cpp b/common/autoware_geography_utils/src/projection.cpp index 872de8a08b..bf3e50eacf 100644 --- a/common/autoware_geography_utils/src/projection.cpp +++ b/common/autoware_geography_utils/src/projection.cpp @@ -24,7 +24,7 @@ namespace autoware::geography_utils [[nodiscard]] Eigen::Vector3d to_basic_point_3d_pt(const LocalPoint src) { - return {src.x, src.y, src.z}; + return Eigen::Vector3d{src.x, src.y, src.z}; } LocalPoint project_forward(const GeoPoint & geo_point, const MapProjectorInfo & projector_info) From 4079cedc24d64aa4b77fdc15a8d22908a504e2ce Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Wed, 18 Dec 2024 11:18:47 +0900 Subject: [PATCH 6/6] revert string Signed-off-by: Yutaka Kondo --- common/autoware_geography_utils/src/height.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/common/autoware_geography_utils/src/height.cpp b/common/autoware_geography_utils/src/height.cpp index b0cd615c0d..3c8b8d62e6 100644 --- a/common/autoware_geography_utils/src/height.cpp +++ b/common/autoware_geography_utils/src/height.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include namespace autoware::geography_utils @@ -45,7 +44,7 @@ double convert_height( if (source_vertical_datum == target_vertical_datum) { return height; } - static const std::map, HeightConversionFunction> + static const std::map, HeightConversionFunction> conversion_map{ {{"WGS84", "EGM2008"}, convert_wgs84_to_egm2008}, {{"EGM2008", "WGS84"}, convert_egm2008_to_wgs84},