realize dynamic lat/lon to utm based on launch args in rviz launch file

etsi_its_rviz_plugins/launch/demo.launch.py

@@ -1,74 +1,90 @@
+import math
 import os
-from pathlib import Path
+
+import pyproj
 from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription, LaunchContext
+from launch import LaunchDescription
+from launch_ros.actions import Node
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
 
-latitude_description_path = Path('latitude_destination.urdf')
-longitude_description_path = Path('longitude_destination.urdf')
 
-def render_latitude(context: LaunchContext, latitude):
-    latitude_str = context.perform_substitution(latitude)
-    latitude_description_config = latitude_str
-    latitude_description_path.write_text(latitude_description_config)
+def utmFromLatLon(lat, lon):
+    zone, is_northern = utmZoneFromLatLon(lat, lon)
+    transformer = pyproj.Transformer.from_crs("EPSG:4326", "EPSG:" + epsgCodeFromUtmZone(zone, is_northern)) # EPSG:4326 is WGS84
+    return transformer.transform(lat, lon)
 
-def render_longitude(context: LaunchContext, longitude):
-    longitude_str = context.perform_substitution(longitude)
-    longitude_description_config = longitude_str
-    longitude_description_path.write_text(longitude_description_config)
+def utmZoneFromLatLon(lat, lon):
+    zone_number = int((lon + 180) // 6) + 1
+    is_northern = (lat >= 0)
+    return zone_number, is_northern
 
-def generate_launch_description():
-    latitude = LaunchConfiguration('latitude', default='50.785407')
-    longitude = LaunchConfiguration('longitude', default='6.043521')
-    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
+def utmFrameNameFromUtmZone(utm_zone, is_northern=True):
+    if is_northern:
+        return f"utm_{utm_zone}N"
+    else:
+        return f"utm_{utm_zone}S"
 
-    rviz_config = 'config/demo.rviz'
-    rviz_config_path = os.path.join(
-        get_package_share_directory('etsi_its_rviz_plugins'),
-        rviz_config)
+def epsgCodeFromUtmZone(utm_zone, is_northern=True):
+    if is_northern:
+        epsg_code = 32600 + utm_zone
+    else:
+        epsg_code = 32700 + utm_zone
+    return str(epsg_code)
 
-    return LaunchDescription([
-        DeclareLaunchArgument(
-            'use_sim_time',
-            default_value='true',
-            description='Use simulation clock if true'),
+def gridConvergenceAngleFromLatLon(lat, lon):
+    l = lon * math.pi / 180
+    zone, is_northern = utmZoneFromLatLon(lat,lon)
+    l0 = (zone * 6 - 183) * math.pi / 180
+    phi = lat * math.pi / 180
+    return math.atan(math.tan(l - l0) * math.sin(phi))
+
+
+# https://robotics.stackexchange.com/a/104402
+def generate_launch_description_with_resolved_launch_args(launch_context):
 
-        DeclareLaunchArgument(
-            'latitude',
-            default_value='50.785407',
-            description='set value for latitude'),
+    # get reference position lat/lon launch argument values
+    lat = float(LaunchConfiguration("lat").perform(launch_context))
+    lon = float(LaunchConfiguration("lon").perform(launch_context))
 
-        DeclareLaunchArgument(
-            'longitude',
-            default_value='6.043521',
-            description='set value for longitude'),
+    # convert lat/lon to utm
+    utm_x, utm_y = utmFromLatLon(lat, lon)
+    utm_conv_angle = gridConvergenceAngleFromLatLon(lat, lon)
+    utm_zone, is_northern = utmZoneFromLatLon(lat, lon)
+    utm_frame_name = utmFrameNameFromUtmZone(utm_zone, is_northern)
 
-        OpaqueFunction(function=render_latitude, args=[LaunchConfiguration('latitude')]),
-        OpaqueFunction(function=render_longitude, args=[LaunchConfiguration('longitude')]),
+    return [
 
+        # rviz2
         Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz2',
-            parameters=[{'use_sim_time': use_sim_time}],
-            arguments=['-d'+str(rviz_config_path)]),
+            package="rviz2",
+            executable="rviz2",
+            name="rviz2",
+            parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}],
+            arguments=["-d", os.path.join(get_package_share_directory("etsi_its_rviz_plugins"), "config/demo.rviz")],
+            output="screen",
+        ),
 
+        # static_transform_publisher for utm -> map transformation
         Node(
-            package='tf2_ros',
-            executable='static_transform_publisher',
-            arguments=["291608.065", "5630129.104", "0.0", "0.0", "0.0", "-0.0399933", "utm_32N", "map"],
+            package="tf2_ros",
+            executable="static_transform_publisher",
+            arguments=["--frame-id", utm_frame_name, "--child-frame-id", "map", "--x", str(utm_x), "--y", str(utm_y), "--z", "0.0", "--roll", "0.0", "--pitch", "0.0", "--yaw", str(utm_conv_angle)],
         ),
 
+        # NavSatFix publisher for AerialMapDisplay
         ExecuteProcess(
-            cmd=['ros2', 'topic', 'pub', '-r 0.1', '/map_link/navsatfix', 'sensor_msgs/msg/NavSatFix', 
-                '{header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, ' +
-                'latitude: ' + latitude_description_path.read_text() +
-                ', ' +
-                'longitude: ' + longitude_description_path.read_text() +
-                ', ' +
-                'altitude: 0.0}'],
-            output='screen',
+            cmd=["ros2", "topic", "pub", "-r 0.1", "/map_link/navsatfix", "sensor_msgs/msg/NavSatFix", f"{{header: {{stamp: {{sec: 0, nanosec: 0}}, frame_id: 'map'}}, latitude: {lat}, longitude: {lon}, altitude: 0.0}}"],
+            output="screen",
         ),
-    ])
+    ]
+
+
+def generate_launch_description():
+
+    return LaunchDescription([
+        DeclareLaunchArgument("use_sim_time", default_value="false", description="use simulation time"),
+        DeclareLaunchArgument("lat", default_value="50.786750", description="reference position latitude (map frame)"),
+        DeclareLaunchArgument("lon", default_value="6.046295", description="reference position longitude (map frame)"),
+        OpaqueFunction(function=generate_launch_description_with_resolved_launch_args)
+    ])

etsi_its_rviz_plugins/package.xml

@@ -25,6 +25,7 @@
   <depend condition="$ROS_VERSION == 2">etsi_its_msgs</depend>
   <depend condition="$ROS_VERSION == 2">etsi_its_msgs_utils</depend>
   <depend condition="$ROS_VERSION == 2">pluginlib</depend>
+  <depend condition="$ROS_VERSION == 2">python3-pyproj</depend>
   <depend condition="$ROS_VERSION == 2">rclcpp</depend>
   <depend condition="$ROS_VERSION == 2">rviz_common</depend>
   <depend condition="$ROS_VERSION == 2">rviz_default_plugins</depend>