mavkml.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 
3 '''
4 simple kml export for logfiles
5 Thomas Gubler <thomasgubler@gmail.com>
6 '''
7 from __future__ import print_function
8 
9 from builtins import range
10 
11 from argparse import ArgumentParser
12 import simplekml
13 from pymavlink.mavextra import *
14 from pymavlink import mavutil
15 import time
16 import re
17 import os
18 
19 mainstate_field = 'STAT.MainState'
20 position_field_types = [ # Order must be lon, lat, alt to match KML
21  ['Lon', 'Lat', 'Alt'], # PX4
22  ['Lng', 'Lat', 'Alt'] # APM > 3
23 ]
24 
25 colors = [simplekml.Color.red, simplekml.Color.green, simplekml.Color.blue,
26  simplekml.Color.violet, simplekml.Color.yellow, simplekml.Color.orange,
27  simplekml.Color.burlywood, simplekml.Color.azure, simplekml.Color.lightblue,
28  simplekml.Color.lawngreen, simplekml.Color.indianred, simplekml.Color.hotpink]
29 
30 kml = simplekml.Kml()
31 kml_linestrings = []
32 
33 
34 def add_to_linestring(position_data, kml_linestring):
35  '''add a point to the kml file'''
36  global kml
37 
38  # add altitude offset
39  position_data[2] += float(args.aoff)
40  kml_linestring.coords.addcoordinates([position_data])
41 
42 
43 def save_kml(filename):
44  '''saves the kml file'''
45  global kml
46  kml.save(filename)
47  print("KML written to %s" % filename)
48 
49 
50 def add_data(t, msg, msg_types, vars, fields, field_types, position_field_type):
51  '''add some data'''
52 
53  mtype = msg.get_type()
54  if mtype not in msg_types:
55  return
56 
57  for i in range(0, len(fields)):
58  if mtype not in field_types[i]:
59  continue
60  f = fields[i]
61  v = mavutil.evaluate_expression(f, vars)
62  if v is None:
63  continue
64 
65  # Check if we have position or state information
66  if f == mainstate_field:
67  # Handle main state information
68  # add_data.mainstate_current >= 0 : avoid starting a new linestring
69  # when mainstate comes after the first position data in the log
70  if (v != add_data.mainstate_current and
71  add_data.mainstate_current >= 0):
72  add_data.new_linestring = True
73  add_data.mainstate_current = v
74  else:
75  # Handle position information
76  # make sure lon, lat, alt is saved in the correct order in
77  # position_data (the same as in position_field_types)
78  add_data.position_data[i] = v
79 
80  # check if we have a full gps measurement
81  gps_measurement_ready = True
82  for v in add_data.position_data:
83  if v is None:
84  gps_measurement_ready = False
85  if not gps_measurement_ready:
86  return
87 
88  # if new line string is needed (because of state change): add previous
89  # linestring to kml_linestrings list, add a new linestring to the kml
90  # multigeometry and append to the new linestring
91  # else: append to current linestring
92  if add_data.new_linestring:
93  if add_data.current_kml_linestring is not None:
94  kml_linestrings.append(add_data.current_kml_linestring)
95 
96  name = "".join([args.source, ":", str(add_data.mainstate_current)])
97  add_data.current_kml_linestring = \
98  kml.newlinestring(name=name, altitudemode='absolute')
99 
100  # set rendering options
101  if args.extrude:
102  add_data.current_kml_linestring.extrude = 1
103  add_data.current_kml_linestring.style.linestyle.color = \
104  colors[max([add_data.mainstate_current, 0])]
105 
106  add_data.new_linestring = False
107 
108  add_to_linestring(add_data.position_data,
109  add_data.current_kml_linestring)
110  add_data.last_time = msg._timestamp
111 
112  else:
113  if (msg._timestamp - add_data.last_time) > 0.1:
114  add_to_linestring(add_data.position_data,
115  add_data.current_kml_linestring)
116  add_data.last_time = msg._timestamp
117 
118  # reset position_data
119  add_data.position_data = [None for n in position_field_type]
120 
121 
122 def process_file(filename, source):
123  '''process one file'''
124  print("Processing %s" % filename)
125  mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
126 
127  position_field_type = sniff_field_spelling(mlog, source)
128 
129  # init fields and field_types lists
130  fields = [args.source + "." + s for s in position_field_type]
131  fields.append(mainstate_field)
132  field_types = []
133 
134  msg_types = set()
135  re_caps = re.compile('[A-Z_][A-Z0-9_]+')
136 
137  for f in fields:
138  caps = set(re.findall(re_caps, f))
139  msg_types = msg_types.union(caps)
140  field_types.append(caps)
141 
142  add_data.new_linestring = True
143  add_data.mainstate_current = -1
144  add_data.current_kml_linestring = None
145  add_data.position_data = [None for n in position_field_type]
146  add_data.last_time = 0
147 
148  while True:
149  msg = mlog.recv_match(args.condition)
150  if msg is None:
151  break
152  tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60)
153  tdays += 719163 # pylab wants it since 0001-01-01
154  add_data(tdays, msg, msg_types, mlog.messages, fields, field_types, position_field_type)
155 
156 
157 def sniff_field_spelling(mlog, source):
158  '''attempt to detect whether APM or PX4 attributes names are in use'''
159  position_field_type_default = position_field_types[0] # Default to PX4 spelling
160 
161  msg = mlog.recv_match(source)
162  mlog._rewind() # Unfortunately it's either call this or return a mutated object
163 
164  position_field_selection = [spelling for spelling in position_field_types if hasattr(msg, spelling[0])]
165 
166  return position_field_selection[0] if position_field_selection else position_field_type_default
167 
168 
169 if __name__ == '__main__':
170  parser = ArgumentParser(description=__doc__)
171  parser.add_argument("--no-timestamps", dest="notimestamps",
172  action='store_true', help="Log doesn't have timestamps")
173  parser.add_argument("--condition", default=None,
174  help="select packets by a condition [default: %(default)s]")
175  parser.add_argument("--aoff", default=0.,
176  help="Altitude offset for paths that go through the"
177  "ground in google earth [default: %(default)s]")
178  parser.add_argument("-o", "--output", dest="filename_out", default="mav.kml",
179  help="Output filename [default: %(default)s] ")
180  parser.add_argument("-s", "--source", default="GPOS",
181  help="Select position data source"
182  "(GPOS or GPS) [default: %(default)s]")
183  parser.add_argument("-e", "--extrude", default=False,
184  action='store_true',
185  help="Extrude paths to ground [default: %(default)s]")
186  parser.add_argument("logs", metavar="LOG", nargs="+")
187 
188  args = parser.parse_args()
189 
190  filenames = []
191  for f in args.logs:
192  if os.path.exists(f):
193  filenames.append(f)
194 
195  if len(filenames) == 0:
196  print("No files to process")
197  sys.exit(1)
198 
199  for fi in range(0, len(filenames)):
200  f = filenames[fi]
201  process_file(f, args.source)
202 
203  save_kml(args.filename_out)
204 


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:07