def write_to_kml(self,filename):
k = kml.KML()
ns = '{http://www.opengis.net/kml/2.2}'
d = kml.Document(ns=ns, name='SNOPT Stitched Trajectory')
#styles
s = styles.Style(id='yellowLineGreenPoly')
ls = styles.LineStyle(color='7f00ff00',width=4)
ps = styles.PolyStyle(color='7f00ff00')
s.append_style(ls)
s.append_style(ps)
d.append_style(s)
#placemark
pm = kml.Placemark(name='Stitched Trajectory',description='Trajectory for EADDDAS',styleUrl='#yellowLineGreenPoly')
geom = Geometry()
coords = []
for i,x in enumerate(self.east):
coord = [0]*3
coord[1] = self.ka.lat + self.north[i]/111111.0 #lat
coord[0] = self.ka.lon + self.east[i]/(111111.0*cos(coord[1]*pi/180)) #lon
coord[2] = self.ka.alt + self.up[i] #alt
coords.append(tuple(coord))
geom.geometry = LineString(coords)
geom.extrude = True
geom.tessellate = True
geom.altitude_mode = 'absolute'
pm.geometry = geom
d.append(pm)
k.append(d)
kmlfile = open(filename,"w")
kmlfile.write(k.to_string(prettyprint=True))
kmlfile.close()
def kml_write(x, y, c):
#Put coordinates into an xml file
ns = '{http://www.opengis.net/kml/2.2}'
d = kml.Document(ns=ns, name='MOBILE GCS')
k = kml.KML(ns=ns)
p= kml.Placemark(ns, name='MOBILE GCS', styleUrl='sn_van')
s = styles.Style(id='sn_van')
IS = styles.IconStyle(scale=1.2, icon_href='http://maps.google.com/mapfiles/kml/shapes/truck.png', heading=(c - 90))
s.append_style(IS)
#AC Geometry
geom = Geometry()
geom.geometry = Point(x, y, 0)
geom.altitude_mode = 'relativeToGround'
p.geometry = geom
d.append_style(s)
d.append(p)
#Write
k.append(d)
kmlfile = open('TOL_GCS.kml',"w")
kmlfile.write(k.to_string(prettyprint=True))
kmlfile.close()
def make_kml(x, y, alt, c, type):
ns = '{http://www.opengis.net/kml/2.2}'
if type == "wp":
p= kml.Placemark(ns, name='WP', styleUrl='wp')
s = styles.Style(id='wp')
IS = styles.IconStyle(scale=1.2, icon_href='https://maps.google.com/mapfiles/kml/shapes/capital_big_highlight.png', heading=(int(c) - 90))
s.append_style(IS)
d = kml.Document(ns=ns, name='WP')
elif type == "ac":
p= kml.Placemark(ns, name='AC', styleUrl='ac')
s = styles.Style(id='ac')
IS = styles.IconStyle(scale=1.2, icon_href='https://maps.google.com/mapfiles/kml/shapes/airports.png', heading=(int(c)))
s.append_style(IS)
d = kml.Document(ns=ns, name='AC')
elif type == "solo":
p= kml.Placemark(ns, name='SOLO', styleUrl='solo')
s = styles.Style(id='solo')
IS = styles.IconStyle(scale=1.2, icon_href='https://maps.google.com/mapfiles/kml/shapes/camera.png', heading=(int(c) - 90))
s.append_style(IS)
d = kml.Document(ns=ns, name='SOLO')
else:
return
geom = Geometry()
geom.geometry = Point(float(y), float(x), float(alt))
geom.altitude_mode = 'relativeToGround'
p.geometry = geom
d.append_style(s)
d.append(p)
return d
def run(self):
while True:
#poison pill check
if self.poison == True:
break
else:
#read gps data (don't block)
msg = self.master.recv_match(type='GLOBAL_POSITION_INT', blocking=False)
if msg:
#print msg
self.aclat = msg.lat/1e7
self.aclon = msg.lon/1e7
self.acalt = msg.alt/1e3
self.achdg = msg.hdg/1e2
#Find position relative to Ka-1 (ENU)
#Convert degrees to radians:
lat1 = self.kalat * pi/180
lon1 = self.kalon * pi/180
lat2 = self.aclat * pi/180
lon2 = self.aclon * pi/180
dlat = lat2-lat1
dlong = lon2-lon1
#Haversine formula
R = 6371000
a = sin(dlat/2)*sin(dlat/2) + cos(lat1)*cos(lat2)*sin(dlong/2.0)*sin(dlong/2.0)
c = 2.0 * atan2( sqrt(a), sqrt(1-a) )
d = R * c; #distance
b = atan2( sin(dlong)*cos(lat2), cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(dlong) )
self.east = d*cos(pi/2-b) #x meters offset from Datum (ENU)
self.north = d*sin(pi/2-b) #y meters offset from Datum (ENU)
self.up = self.acalt - self.kaalt #z meters offset from Datum (ENU), should never be negative lol
#write KML for plane
ns = '{http://www.opengis.net/kml/2.2}'
d = kml.Document(ns=ns, name='TOL GCS')
k = kml.KML(ns=ns)
#AIRCRAFT
p= kml.Placemark(ns, name='sUAS(' + '{0:.2f}'.format(self.east) +','+ '{0:.2f}'.format(self.north) +','+ '{0:.2f}'.format(self.up) + ')', styleUrl='sn_airports')
#AC Style
s = styles.Style(id='sn_airports')
IS = styles.IconStyle(scale=1.2, icon_href='http://maps.google.com/mapfiles/kml/shapes/airports.png', heading=self.achdg)
s.append_style(IS)
#AC Geometry
geom = Geometry()
geom.geometry = Point(self.aclon, self.aclat, self.acalt)
geom.altitude_mode = 'absolute'
p.geometry = geom
d.append_style(s)
d.append(p)
#MGS
p = kml.Placemark(ns, name='MGS (0,0,0)',styleUrl='sn_truck')
s = styles.Style(id='sn_truck')
IS = styles.IconStyle(scale=1.2, icon_href='http://maps.google.com/mapfiles/kml/shapes/truck.png')
s.append_style(IS)
#MGS Geometry
geom = Geometry()
geom.geometry = Point(self.kalon, self.kalat, self.kaalt)
geom.altitude_mode = 'absolute'
p.geometry = geom
d.append_style(s)
d.append(p)
#WRITE
k.append(d)
kmlfile = open('TOL_GCS.kml',"w")
kmlfile.write(k.to_string(prettyprint=True))
kmlfile.close()
def run(self):
while True:
#poison pill check
if self.poison == True:
break
else:
#read gps data (don't block)
msg = self.master.recv_match(type='GLOBAL_POSITION_INT',
blocking=False)
if msg:
#print msg
self.aclat = msg.lat / 1e7
self.aclon = msg.lon / 1e7
self.acalt = msg.alt / 1e3
self.achdg = msg.hdg / 1e2
#Find position relative to Ka-1 (ENU)
#Convert degrees to radians:
lat1 = self.kalat * pi / 180
lon1 = self.kalon * pi / 180
lat2 = self.aclat * pi / 180
lon2 = self.aclon * pi / 180
dlat = lat2 - lat1
dlong = lon2 - lon1
#Haversine formula
R = 6371000
a = sin(dlat / 2) * sin(dlat / 2) + cos(lat1) * cos(
lat2) * sin(dlong / 2.0) * sin(dlong / 2.0)
c = 2.0 * atan2(sqrt(a), sqrt(1 - a))
d = R * c
#distance
b = atan2(
sin(dlong) * cos(lat2),
cos(lat1) * sin(lat2) -
sin(lat1) * cos(lat2) * cos(dlong))
self.east = d * cos(
pi / 2 - b) #x meters offset from Datum (ENU)
self.north = d * sin(
pi / 2 - b) #y meters offset from Datum (ENU)
self.up = self.acalt - self.kaalt #z meters offset from Datum (ENU), should never be negative lol
#write KML for plane
ns = '{http://www.opengis.net/kml/2.2}'
d = kml.Document(ns=ns, name='TOL GCS')
k = kml.KML(ns=ns)
#AIRCRAFT
p = kml.Placemark(ns,
name='sUAS(' +
'{0:.2f}'.format(self.east) + ',' +
'{0:.2f}'.format(self.north) + ',' +
'{0:.2f}'.format(self.up) + ')',
styleUrl='sn_airports')
#AC Style
s = styles.Style(id='sn_airports')
IS = styles.IconStyle(
scale=1.2,
icon_href=
'http://maps.google.com/mapfiles/kml/shapes/airports.png',
heading=self.achdg)
s.append_style(IS)
#AC Geometry
geom = Geometry()
geom.geometry = Point(self.aclon, self.aclat, self.acalt)
geom.altitude_mode = 'absolute'
p.geometry = geom
d.append_style(s)
d.append(p)
#MGS
p = kml.Placemark(ns,
name='MGS (0,0,0)',
styleUrl='sn_truck')
s = styles.Style(id='sn_truck')
IS = styles.IconStyle(
scale=1.2,
icon_href=
'http://maps.google.com/mapfiles/kml/shapes/truck.png')
s.append_style(IS)
#MGS Geometry
geom = Geometry()
geom.geometry = Point(self.kalon, self.kalat, self.kaalt)
geom.altitude_mode = 'absolute'
p.geometry = geom
d.append_style(s)
d.append(p)
#WRITE
k.append(d)
kmlfile = open('TOL_GCS.kml', "w")
kmlfile.write(k.to_string(prettyprint=True))
kmlfile.close()
