Added pos2kml.py tool (d59fdeff) · Commits · senti / sentiboard-utils

converters/pos2kml.py

0 → 100755

+147 −0

Changes for converters/pos2kml.py: 147 added lines, 0 removed lines.
Original line number	Diff line number	Diff line
		# -- 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)

converters/rtk.py

0 → 100755

+117 −0

Changes for converters/rtk.py: 117 added lines, 0 removed lines.
Original line number	Diff line number	Diff line
		# -- 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