From 02e5e34fff8589a75888509ce6edf2434fb01379 Mon Sep 17 00:00:00 2001
From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com>
Date: Mon, 25 Apr 2022 09:33:44 +0900
Subject: [PATCH] fix(map_launch): map_loader package not working in UTM
 coordinates (#293)

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
---
 map_launch/launch/map.launch.py  | 14 +++++++++++++-
 map_launch/launch/map.launch.xml |  4 ++++
 2 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/map_launch/launch/map.launch.py b/map_launch/launch/map.launch.py
index a4ae54185..c4e6fad4f 100644
--- a/map_launch/launch/map.launch.py
+++ b/map_launch/launch/map.launch.py
@@ -11,7 +11,9 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
+import os
 
+from ament_index_python.packages import get_package_share_directory
 import launch
 from launch.actions import DeclareLaunchArgument
 from launch.actions import GroupAction
@@ -23,9 +25,17 @@
 from launch_ros.actions import Node
 from launch_ros.actions import PushRosNamespace
 from launch_ros.descriptions import ComposableNode
+import yaml
 
 
 def generate_launch_description():
+
+    lanelet2_map_origin_path = os.path.join(
+        get_package_share_directory("map_loader"), "config/lanelet2_map_loader.param.yaml"
+    )
+
+    with open(lanelet2_map_origin_path, "r") as f:
+        lanelet2_map_origin_param = yaml.safe_load(f)["/**"]["ros__parameters"]
     map_hash_generator = Node(
         package="map_loader",
         executable="map_hash_generator",
@@ -47,7 +57,9 @@ def generate_launch_description():
             {
                 "center_line_resolution": 5.0,
                 "lanelet2_map_path": LaunchConfiguration("lanelet2_map_path"),
-            }
+                "lanelet2_map_projector_type": "MGRS",  # Options: MGRS, UTM
+            },
+            lanelet2_map_origin_param,
         ],
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
diff --git a/map_launch/launch/map.launch.xml b/map_launch/launch/map.launch.xml
index 5bedbc1d5..49746c39a 100644
--- a/map_launch/launch/map.launch.xml
+++ b/map_launch/launch/map.launch.xml
@@ -1,4 +1,5 @@
 <launch>
+  <arg name="param_file" default="$(find-pkg-share map_loader)/config/lanelet2_map_loader.param.yaml"/>
   <arg name="map_path" default="" />
   <arg name="lanelet2_map_path" default="$(var map_path)/lanelet2_map.osm" />
   <arg name="pointcloud_map_path" default="$(var map_path)/pointcloud_map.pcd" />
@@ -12,6 +13,9 @@
     <node pkg="map_loader" exec="pointcloud_map_loader" name="pointcloud_map_loader">
       <remap from="output/pointcloud_map" to="/map/pointcloud_map" />
       <param name="pcd_paths_or_directory" value="[$(var pointcloud_map_path)]" />
+      <param name="lanelet2_map_projector_type" value="MGRS"/>
+      <!-- UTM or MGRS -->
+      <param from="$(var param_file)"/>
     </node>
 
     <include file="$(find-pkg-share map_tf_generator)/launch/map_tf_generator.launch.xml">