Commit d59fdeff authored by enfo's avatar enfo

Added pos2kml.py tool

parent e0d55efd
# -*- coding: utf-8 -*-
"""
Created on Fri May 4 21:08:58 2018
@author: Enfenion
"""
import argparse
import os
import rtk
import datetime
kml_start='''<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://www.opengis.net/kml/2.2">
<Document>
<name>Paths</name>
<description>Examples of paths. Note that the tessellate tag is by default
set to 0. If you want to create tessellated lines, they must be authored
(or edited) directly in KML.</description>
<Style id="yellowLineGreenPoly">
<LineStyle>
<color>7f00ffff</color>
<width>4</width>
</LineStyle>
<PolyStyle>
<color>7f00ff00</color>
</PolyStyle>
</Style>
<Placemark>
<name>Absolute Extruded</name>
<description>Transparent green wall with yellow outlines</description>
<styleUrl>#yellowLineGreenPoly</styleUrl>
<LineString>
<extrude>1</extrude>
<tessellate>1</tessellate>
<altitudeMode>absolute</altitudeMode>
<coordinates>
'''
kml_end=''' </coordinates>
</LineString>
</Placemark>
</Document>
</kml>
'''
kml2_start='''<?xml version="1.0" encoding="UTF-8"?>
<kml xmlns="http://www.opengis.net/kml/2.2" xmlns:gx="http://www.google.com/kml/ext/2.2" xmlns:kml="http://www.opengis.net/kml/2.2" xmlns:atom="http://www.w3.org/2005/Atom">
<Document>
<name>UAV-icons.kml</name>
<Style id="sn_airports">
<IconStyle>
<color>ffffaa00</color>
<scale>1.2</scale>
<Icon>
<href>http://maps.google.com/mapfiles/kml/shapes/airports.png</href>
</Icon>
<hotSpot x="0.5" y="0" xunits="fraction" yunits="fraction"/>
</IconStyle>
<ListStyle>
</ListStyle>
<LineStyle>
<color>99ffaa00</color>
<width>6</width>
</LineStyle>
</Style>
<StyleMap id="msn_airports">
<Pair>
<key>normal</key>
<styleUrl>#sn_airports</styleUrl>
</Pair>
<Pair>
<key>highlight</key>
<styleUrl>#sh_airports</styleUrl>
</Pair>
</StyleMap>
<Style id="sh_airports">
<IconStyle>
<color>ffffaa00</color>
<scale>1.4</scale>
<Icon>
<href>http://maps.google.com/mapfiles/kml/shapes/airports.png</href>
</Icon>
<hotSpot x="0.5" y="0" xunits="fraction" yunits="fraction"/>
</IconStyle>
<ListStyle>
</ListStyle>
<LineStyle>
<color>99ffaa00</color>
<width>8</width>
</LineStyle>
</Style>
<Placemark>
<name>UAV</name>
<styleUrl>#msn_airports</styleUrl>
<gx:Track>
<altitudeMode>relativeToGround</altitudeMode>
'''
kml2_end=''' </gx:Track>
</Placemark>
</Document>
</kml>
'''
def create_kml(data, destname, interval):
lats, lons, alts = data['latitude'], data['longitude'], data['height']
gpsweek, gpsseconds = data['week'], data['gpst']
mode=2
leapseconds = 0
last = datetime.datetime(1980, 1, 6, 0, 0, 0)
with open(destname, 'w') as f:
if mode == 1:
f.write(kml_start)
for ix in range(len(lats)):
f.write('%.9f,%.9f,%.2f\n' % (lons[ix], lats[ix], alts[ix]))
f.write(kml_end)
else:
f.write(kml2_start)
for ix in range(len(lats)):
d = rtk.weeksecondstoutc(gpsweek[ix],gpsseconds[ix],leapseconds)
if (d - last).total_seconds() < interval:
continue
last = d
f.write('<when>%s</when>\n' % d.strftime("%Y-%m-%dT%H:%M:%SZ"))
f.write('<gx:coord>')
f.write('%.9f,%.9f,%.2f' % (lons[ix], lats[ix], alts[ix]))
f.write('</gx:coord>\n')
f.write(kml2_end)
print('Stored .kml to %s' % destname)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Convert an RTK .pos-file to a .kml for Google Earth')
parser.add_argument('pos', help='RTK .pos-file')
parser.add_argument('--output', '-O', dest='dest', help='Optional destination file (default *.kml)')
parser.add_argument('--interval', '-d', dest='interval', type=float, help='Interval between each data point', default=1)
args = parser.parse_args()
filename = args.pos
if args.dest:
destname = args.dest
else:
destname = os.path.splitext(filename)[0] + '.kml'
data, names = rtk.parse_file(filename)
create_kml(data, destname, args.interval)
# -*- coding: utf-8 -*-
"""
Created on Fri May 4 21:44:51 2018
@author: Enfenion
"""
import re
import datetime
def weeksecondstoutc(gpsweek,gpsseconds,leapseconds):
epoch = datetime.datetime(1980, 1, 6, 0, 0, 0)
elapsed = datetime.timedelta(days=(gpsweek * 7), seconds=(gpsseconds + leapseconds))
return epoch + elapsed
def split_line(line):
if '|' in line:
return line.split('|')
if '\t' in line:
return line.split('\t')
return re.sub(' +', ' ', line).split(' ')
def parseline(line, names):
values = split_line(line)
converters = [int] + [float]*4 + [int]*2 + [float]*8
conv_vals = [c(v) for c, v in zip(converters, values) ]
return dict(zip(names, conv_vals))
def count_lines(f):
n_lines = 0
for line in f:
if line.startswith('%'):
continue
n_lines += 1
f.seek(0)
return n_lines
def handle_name(name):
name = name.strip().lower()
if '(' in name:
name = name[:name.index('(')]
name = name.replace('-', '_')
return name
def extract_data(f, n_lines):
names = ['week', 'tow', 'ecef_x', 'ecef_y', 'ecef_z', 'Q', 'ns',
'sdx', 'sdy', 'sdz', 'sdxy', 'sdyz', 'sdzx', 'age', 'ratio']
ref_pos = None
tmpbuf = [0] * n_lines
print('Found %d lines' % n_lines)
data = {}
c_line = 0
for line in f:
if line.startswith('%'):
if 'age' in line:
names = [handle_name(name) for name in split_line(line.strip('% '))]
if names[0] == 'gpst':
names.insert(0, 'week')
if 'ref pos' in line:
data_start = line.index(':', 1)
ref_pos = [float(p) for p in split_line(line[data_start+1:].strip())]
continue
data_line = parseline(line, names)
for key, val in data_line.items():
try:
data[key][c_line] = val
except KeyError:
data[key] = list(tmpbuf)
data[key][c_line] = val
c_line += 1
f.seek(0)
return data, names, ref_pos
def parse_file(infile):
ref_pos = None
with open(infile, 'rU') as f:
n_lines = count_lines(f)
data, names, ref_pos = extract_data(f, n_lines)
if 'ecef_x' in names:
try:
from pymap3d import ecef2geodetic
print('Adding latlon')
lat, lon, height = ecef2geodetic(data['ecef_x'], data['ecef_y'], data['ecef_z'])
data['latitude'] = lat
data['longitude'] = lon
data['height'] = height
names += ['latitude', 'longitude', 'height']
except ImportError:
print('Install pymap3d to automatically add enu solution')
if 'latitude' in names and ref_pos is not None:
try:
from pymap3d import geodetic2enu
print('Adding enu')
east, north, up = geodetic2enu(data['latitude'], data['longitude'], data['height'], *ref_pos)
data['e_baseline'] = east
data['n_baseline'] = north
data['u_baseline'] = up
names += ['e_baseline', 'n_baseline', 'u_baseline']
except ImportError:
print('Install pymap3d to automatically add enu solution')
elif 'e_baseline' in names:
try:
from pymap3d import enu2geodetic
print('Adding latlon')
lat, lon, height = enu2geodetic(data['e_baseline'], data['n_baseline'], data['u_baseline'], *ref_pos)
data['latitude'] = lat
data['longitude'] = lon
data['height'] = height
names += ['latitude', 'longitude', 'height']
except ImportError:
print('Install pymap3d to automatically add enu solution')
return data, names
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment