From 06682c23f9079820df756ead5b42d7b04bd8a2ff Mon Sep 17 00:00:00 2001
From: KhalilSelyan <khalil@leodrive.ai>
Date: Wed, 2 Oct 2024 11:38:42 +0300
Subject: [PATCH] refactor: Refactor localToGeographic functions to remove
unnecessary origin_lat and origin_lon parameters
Signed-off-by: KhalilSelyan <khalil@leodrive.ai>
---
.../src/goal_pose.cpp | 22 +++++++------------
.../src/include/goal_pose.hpp | 3 +--
.../src/include/path_overlay.hpp | 3 +--
.../src/path_overlay.cpp | 22 +++++++------------
4 files changed, 18 insertions(+), 32 deletions(-)
diff --git a/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/goal_pose.cpp b/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/goal_pose.cpp
index 4b3b52ac60aa2..6dd874c1ae228 100644
--- a/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/goal_pose.cpp
+++ b/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/goal_pose.cpp
@@ -41,7 +41,7 @@ std::pair<double, double> GoalPose::localToGeographic(
double local_x, double local_y, double origin_lat, double origin_lon)
{
if (projector_type_ == tier4_map_msgs::msg::MapProjectorInfo::MGRS) {
- return localToGeographicMGRS(local_x, local_y, origin_lat, origin_lon, mgrs_grid_);
+ return localToGeographicMGRS(local_x, local_y, mgrs_grid_);
} else if (projector_type_ == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) {
return localToGeographicUTM(local_x, local_y, origin_lat, origin_lon);
}
@@ -72,29 +72,23 @@ std::pair<double, double> GoalPose::localToGeographicUTM(
}
std::pair<double, double> GoalPose::localToGeographicMGRS(
- double local_x, double local_y, double origin_lat, double origin_lon,
- const std::string & mgrs_grid)
+ double local_x, double local_y, const std::string & mgrs_grid)
{
int zone;
bool north_p;
- double origin_x, origin_y, gamma, k, mgrs_x, mgrs_y;
-
- // Convert origin coordinates to UTM
- GeographicLib::UTMUPS::Forward(
- origin_lat, origin_lon, zone, north_p, origin_x, origin_y, gamma, k);
+ double mgrs_origin_x, mgrs_origin_y;
+ int prec;
// Convert MGRS grid string to UTM coordinates
- std::string mgrs_string = mgrs_grid + "0000000000000000";
- int prec;
- GeographicLib::MGRS::Reverse(mgrs_string, zone, north_p, mgrs_x, mgrs_y, prec);
+ GeographicLib::MGRS::Reverse(mgrs_grid, zone, north_p, mgrs_origin_x, mgrs_origin_y, prec, false);
// Calculate the global UTM coordinates by adding local offsets
- double global_x = mgrs_x + local_x;
- double global_y = mgrs_y + local_y;
+ double utm_x = mgrs_origin_x + local_x;
+ double utm_y = mgrs_origin_y + local_y;
// Convert global UTM coordinates to geographic coordinates
double goal_lat, goal_lon;
- GeographicLib::UTMUPS::Reverse(zone, north_p, global_x, global_y, goal_lat, goal_lon);
+ GeographicLib::UTMUPS::Reverse(zone, north_p, utm_x, utm_y, goal_lat, goal_lon);
return {goal_lat, goal_lon};
}
diff --git a/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/goal_pose.hpp b/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/goal_pose.hpp
index 62c8e19e8c209..56457d5a7ed9c 100644
--- a/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/goal_pose.hpp
+++ b/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/goal_pose.hpp
@@ -54,8 +54,7 @@ class GoalPose
std::pair<double, double> localToGeographicUTM(
double local_x, double local_y, double origin_lat, double origin_lon);
std::pair<double, double> localToGeographicMGRS(
- double local_x, double local_y, double origin_lat, double origin_lon,
- const std::string & mgrs_grid);
+ double local_x, double local_y, const std::string & mgrs_grid);
std::pair<int, int> getTileOffsets(
double lat, double lon, int zoom, const QRectF & backgroundRect);
};
diff --git a/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/path_overlay.hpp b/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/path_overlay.hpp
index 6f54206781d3c..a69f8d69efe72 100644
--- a/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/path_overlay.hpp
+++ b/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/include/path_overlay.hpp
@@ -56,8 +56,7 @@ class PathOverlay
std::pair<double, double> localToGeographicUTM(
double local_x, double local_y, double origin_lat, double origin_lon);
std::pair<double, double> localToGeographicMGRS(
- double local_x, double local_y, double origin_lat, double origin_lon,
- const std::string & mgrs_grid);
+ double local_x, double local_y, const std::string & mgrs_grid);
std::pair<int, int> getTileOffsets(
double lat, double lon, int zoom, const QRectF & backgroundRect);
};
diff --git a/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/path_overlay.cpp b/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/path_overlay.cpp
index 9a9ae968bf694..8baa1a75cda40 100644
--- a/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/path_overlay.cpp
+++ b/common/autoware_overlay_rviz_plugin/autoware_minimap_overlay_rviz_plugin/src/path_overlay.cpp
@@ -49,7 +49,7 @@ std::pair<double, double> PathOverlay::localToGeographic(
double local_x, double local_y, double origin_lat, double origin_lon)
{
if (projector_type_ == tier4_map_msgs::msg::MapProjectorInfo::MGRS) {
- return localToGeographicMGRS(local_x, local_y, origin_lat, origin_lon, mgrs_grid_);
+ return localToGeographicMGRS(local_x, local_y, mgrs_grid_);
} else if (projector_type_ == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) {
return localToGeographicUTM(local_x, local_y, origin_lat, origin_lon);
}
@@ -80,29 +80,23 @@ std::pair<double, double> PathOverlay::localToGeographicUTM(
}
std::pair<double, double> PathOverlay::localToGeographicMGRS(
- double local_x, double local_y, double origin_lat, double origin_lon,
- const std::string & mgrs_grid)
+ double local_x, double local_y, const std::string & mgrs_grid)
{
int zone;
bool north_p;
- double origin_x, origin_y, gamma, k, mgrs_x, mgrs_y;
-
- // Convert origin coordinates to UTM
- GeographicLib::UTMUPS::Forward(
- origin_lat, origin_lon, zone, north_p, origin_x, origin_y, gamma, k);
+ double mgrs_origin_x, mgrs_origin_y;
+ int prec;
// Convert MGRS grid string to UTM coordinates
- std::string mgrs_string = mgrs_grid + "0000000000000000";
- int prec;
- GeographicLib::MGRS::Reverse(mgrs_string, zone, north_p, mgrs_x, mgrs_y, prec);
+ GeographicLib::MGRS::Reverse(mgrs_grid, zone, north_p, mgrs_origin_x, mgrs_origin_y, prec, false);
// Calculate the global UTM coordinates by adding local offsets
- double global_x = mgrs_x + local_x;
- double global_y = mgrs_y + local_y;
+ double utm_x = mgrs_origin_x + local_x;
+ double utm_y = mgrs_origin_y + local_y;
// Convert global UTM coordinates to geographic coordinates
double goal_lat, goal_lon;
- GeographicLib::UTMUPS::Reverse(zone, north_p, global_x, global_y, goal_lat, goal_lon);
+ GeographicLib::UTMUPS::Reverse(zone, north_p, utm_x, utm_y, goal_lat, goal_lon);
return {goal_lat, goal_lon};
}