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..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,8 +20,9 @@ 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( 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/height.cpp b/common/autoware_geography_utils/src/height.cpp index 745dbf5b22..3c8b8d62e6 100644 --- a/common/autoware_geography_utils/src/height.cpp +++ b/common/autoware_geography_utils/src/height.cpp @@ -44,20 +44,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 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 diff --git a/common/autoware_geography_utils/src/projection.cpp b/common/autoware_geography_utils/src/projection.cpp index e113c8da7b..bf3e50eacf 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 Eigen::Vector3d{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 =