refactor: Refactor localToGeographic functions to remove unnecessary …

…origin_lat and origin_lon parameters

Signed-off-by: KhalilSelyan <[email protected]>

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};
 }

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);
 };

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);
 };

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};
 }