Initial commit

map_to_kml.py

+#!/usr/bin/env python3
+
+"""Convert a simulator JSON map file into a klm file."""
+
+import argparse
+import json
+import math
+import os
+import xml.etree.ElementTree as ET
+
+style_map_dict = {
+    'terrain': {
+        'line': 'ff000000',
+        'poly': '00ffff55',
+    },
+    'obstacle': {
+        'line': 'ff58eeff',
+        'poly': 'ff58eeff',
+    },
+    'flag': {
+        'line': 'ff7fff00',
+        'poly': 'ff00aa00',
+    },
+}
+
+def lat_lon_distance(lat1, lon1, lat2, lon2):
+  """Return the distance in  meters between 2 coordinates."""
+  q1 = lat1 * math.pi / 180
+  q2 = lat2 * math.pi / 180
+  dq = (lat2 - lat1) * math.pi / 180
+  dl = (lon2 -lon1) * math.pi / 180
+  a = math.sin(dq / 2) * math.sin(dq / 2) \
+      + math.cos(q1) * math.cos(q2) * math.sin(dl / 2) * math.sin(dl / 2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+  return 6371e3 * c
+
+def longitude_to_X(lon, map_size):
+  """Mercator projection of longitude."""
+  return (map_size / 360) * (180 + lon)
+
+def latitude_to_Y(lat, map_size):
+  """Mercator projection latitude."""
+  return (map_size / 180) * (90 - lat)
+
+def convert_to_local_coordinates(latitude, longitude, map_info, map_size):
+  """Convert coordinates into local values."""
+  x = longitude_to_X(longitude, map_size)
+  y = latitude_to_Y(latitude, map_size)
+  local_x = ((x - map_info['min_x']) / (map_info['max_x'] - map_info['min_x'])) \
+            * 1000 - map_size / 2
+  local_y = ((y - map_info['min_y']) / (map_info['max_y'] - map_info['min_y'])) \
+            * 1000 - map_size / 2
+  return local_x, local_y
+
+def convert_to_geo_coordinates(x, y, map_info, map_size):
+  """Convert local values to coordinates."""
+  lon = (x + map_size / 2) / 1000
+  lon = lon * (map_info['max_x'] - map_info['min_x']) + map_info['min_x']
+  lon = lon / (map_size / 360) - 180
+
+  lat = (y + map_size / 2) / 1000
+  lat = lat * (map_info['max_y'] - map_info['min_y']) + map_info['min_y']
+  lat = 90 - lat / (map_size / 180)
+
+  return lon, lat
+
+def add_style_map_pair(style_map, k, name):
+  """Declare a style key."""
+  pair = ET.SubElement(style_map, 'Pair')
+  key = ET.SubElement(pair, 'key')
+  key.text = k
+  style_url = ET.SubElement(pair, 'styleUrl')
+  style_url.text = '#' + name
+
+def add_style_map(root, name, style_dict):
+  """Define a style."""
+  style_map = ET.SubElement(root, 'StyleMap', attrib={'id': name})
+  for key in ('normal', 'highlight'):
+    style_id = '_'.join((name, key))
+    add_style_map_pair(style_map, key, style_id)
+    style = ET.SubElement(root, 'Style', attrib={'id': style_id})
+    line_style = ET.SubElement(style, 'LineStyle')
+    line_color = ET.SubElement(line_style, 'color')
+    line_color.text = style_dict['line']
+    poly_style = ET.SubElement(style, 'PolyStyle')
+    poly_color = ET.SubElement(poly_style, 'color')
+    poly_color.text = style_dict['poly']
+
+def add_polygone(root, coordinate_tuple_list, style_map_id):
+  """Add a polygone to the map."""
+  placemark = ET.SubElement(root, 'Placemark')
+  style_url = ET.SubElement(placemark, 'styleUrl')
+  style_url.text = '#' + style_map_id
+  polygon = ET.SubElement(placemark, 'Polygon')
+  if coordinate_tuple_list[0][-1] > 0:
+    extrude = ET.SubElement(polygon, 'extrude')
+    extrude.text = '1'
+  altitude_mode = ET.SubElement(polygon, 'altitudeMode')
+  altitude_mode.text = 'relativeToGround'
+  outer_boundary = ET.SubElement(polygon, 'outerBoundaryIs')
+  linear_ring = ET.SubElement(outer_boundary, 'LinearRing')
+  coordinates = ET.SubElement(linear_ring, 'coordinates')
+  coordinates.text = ' '.join(
+    (','.join(str(x) for x in coor_tuple) for coor_tuple in coordinate_tuple_list))
+
+def build_rectangle(root, lat, lon, length, width, height, style_map_id, map_info, map_size):
+  """Build a rectangle."""
+  local_x, local_y = convert_to_local_coordinates(lat, lon, map_info, map_size)
+  max_lon, _ = convert_to_geo_coordinates(local_x + length, local_y, map_info, map_size)
+  _, max_lat = convert_to_geo_coordinates(local_x, local_y + width, map_info, map_size)
+  add_polygone(root, (
+    (lat, lon, height),
+    (lat, max_lon, height),
+    (max_lat, max_lon, height),
+    (max_lat, lon, height),
+    (lat, lon, height),
+  ), style_map_id)
+
+
+def write_kml(filename, map_dict):
+  """Write kml file."""
+  root = ET.Element('kml', attrib={'xmlns': 'http://www.opengis.net/kml/2.2'})
+  document = ET.SubElement(root, 'Document')
+  name = ET.SubElement(document, 'name')
+  name.text = filename
+
+  for name, style_dict in style_map_dict.items():
+    add_style_map(document, name, style_dict)
+
+  add_polygone(document, (
+    (map_dict['min_lat'], map_dict['min_lon'], 0),
+    (map_dict['min_lat'], map_dict['max_lon'], 0),
+    (map_dict['max_lat'], map_dict['max_lon'], 0),
+    (map_dict['max_lat'], map_dict['min_lon'], 0),
+    (map_dict['min_lat'], map_dict['min_lon'], 0),
+  ), 'terrain')
+
+  map_size = math.ceil(max(
+    lat_lon_distance(
+      map_dict['min_lat'], map_dict['min_lon'], map_dict['min_lat'], map_dict['max_lon']),
+    lat_lon_distance(
+      map_dict['min_lat'], map_dict['min_lon'], map_dict['max_lat'], map_dict['min_lon']),
+  ))
+  map_info = {
+    'min_x': longitude_to_X(map_dict['min_lon'], map_size),
+    'min_y': latitude_to_Y(map_dict['min_lat'], map_size),
+    'max_x': longitude_to_X(map_dict['max_lon'], map_size),
+    'max_y': latitude_to_Y(map_dict['max_lat'], map_size),
+  }
+
+  for obstacle in map_dict['obstacle_list']:
+    position = obstacle['position']
+    scale = obstacle['scale']
+    rotation = obstacle['rotation']['z'] * math.pi / 180
+    build_rectangle(
+      document,
+      position['latitude'],
+      position['longitude'],
+      scale['x'] * math.cos(rotation) + scale['y'] * math.sin(rotation),
+      - scale['x'] * math.sin(rotation) + scale['y'] * math.cos(rotation),
+      position['altitude'] + scale['z'],
+      'obstacle',
+      map_info,
+      map_size,
+    )
+
+  for flag in map_dict['flag_list']:
+    position = flag['position']
+    build_rectangle(document, position['latitude'], position['longitude'],
+                    1, 1, position['altitude'], 'flag', map_info, map_size)
+
+  tree = ET.ElementTree(root)
+  ET.indent(tree, '   ')
+  tree.write('.'.join((filename, 'kml')), encoding='UTF-8', xml_declaration=True)
+
+
+parser = argparse.ArgumentParser(
+                    prog='map_to_kml',
+                    description='convert a simulator map into kml format')
+parser.add_argument('map_path')
+args = parser.parse_args()
+
+map_filename, _ = os.path.basename(args.map_path).split('.')
+with open(args.map_path) as map_json:
+  map_json_dict = json.load(map_json)
+write_kml(map_filename, map_json_dict)