Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(autoware_geography_utils): apply modern C++17 style #109

Merged
merged 6 commits into from
Dec 18, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -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);

youtalk marked this conversation as resolved.
Show resolved Hide resolved
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(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
25 changes: 12 additions & 13 deletions common/autoware_geography_utils/src/height.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,20 +44,19 @@ double convert_height(
if (source_vertical_datum == target_vertical_datum) {
return height;
}
std::map<std::pair<std::string, std::string>, 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<std::pair<std::string, std::string>, 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
24 changes: 13 additions & 11 deletions common/autoware_geography_utils/src/lanelet2_projector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,30 +28,32 @@ namespace autoware::geography_utils
std::unique_ptr<lanelet::Projector> 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<lanelet::projection::UtmProjector>(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<lanelet::projection::MGRSProjector>(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<lanelet::projection::TransverseMercatorProjector>(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
15 changes: 6 additions & 9 deletions common/autoware_geography_utils/src/projection.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<lanelet::Projector> 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<lanelet::projection::MGRSProjector *>(projector.get());

// project x and y using projector
Expand Down Expand Up @@ -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<lanelet::projection::MGRSProjector *>(projector.get());
const auto * mgrs_projector =
dynamic_cast<lanelet::projection::MGRSProjector *>(projector.get());
// project latitude and longitude using projector
// note that the z is ignored in MGRS projection conventionally
projected_gps_point =
Expand Down
Loading