mavkml.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 '''
00004 simple kml export for logfiles
00005 Thomas Gubler <thomasgubler@gmail.com>
00006 '''
00007 
00008 from argparse import ArgumentParser
00009 import simplekml
00010 from pymavlink.mavextra import *
00011 from pymavlink import mavutil
00012 import time
00013 import re
00014 
00015 mainstate_field = 'STAT.MainState'
00016 position_field_types = [  # Order must be lon, lat, alt to match KML
00017   ['Lon', 'Lat', 'Alt'],  # PX4
00018   ['Lng', 'Lat', 'Alt']   # APM > 3
00019 ]
00020 
00021 colors = [simplekml.Color.red, simplekml.Color.green, simplekml.Color.blue,
00022           simplekml.Color.violet, simplekml.Color.yellow, simplekml.Color.orange,
00023           simplekml.Color.burlywood, simplekml.Color.azure, simplekml.Color.lightblue,
00024           simplekml.Color.lawngreen, simplekml.Color.indianred, simplekml.Color.hotpink]
00025 
00026 kml = simplekml.Kml()
00027 kml_linestrings = []
00028 
00029 
00030 def add_to_linestring(position_data, kml_linestring):
00031     '''add a point to the kml file'''
00032     global kml
00033 
00034     # add altitude offset
00035     position_data[2] += float(args.aoff)
00036     kml_linestring.coords.addcoordinates([position_data])
00037 
00038 
00039 def save_kml(filename):
00040     '''saves the kml file'''
00041     global kml
00042     kml.save(filename)
00043     print("KML written to %s" % filename)
00044 
00045 
00046 def add_data(t, msg, msg_types, vars, fields, field_types, position_field_type):
00047     '''add some data'''
00048 
00049     mtype = msg.get_type()
00050     if mtype not in msg_types:
00051         return
00052 
00053     for i in range(0, len(fields)):
00054         if mtype not in field_types[i]:
00055             continue
00056         f = fields[i]
00057         v = mavutil.evaluate_expression(f, vars)
00058         if v is None:
00059             continue
00060 
00061         # Check if we have position or state information
00062         if f == mainstate_field:
00063             # Handle main state information
00064             # add_data.mainstate_current >= 0 : avoid starting a new linestring
00065             # when mainstate comes after the first position data in the log
00066             if (v != add_data.mainstate_current and
00067                     add_data.mainstate_current >= 0):
00068                 add_data.new_linestring = True
00069             add_data.mainstate_current = v
00070         else:
00071             # Handle position information
00072             # make sure lon, lat, alt is saved in the correct order in
00073             # position_data (the same as in position_field_types)
00074             add_data.position_data[i] = v
00075 
00076     # check if we have a full gps measurement
00077     gps_measurement_ready = True
00078     for v in add_data.position_data:
00079         if v is None:
00080             gps_measurement_ready = False
00081     if not gps_measurement_ready:
00082         return
00083 
00084     # if new line string is needed (because of state change): add previous
00085     # linestring to kml_linestrings list, add a new linestring to the kml
00086     # multigeometry and append to the new linestring
00087     # else: append to current linestring
00088     if add_data.new_linestring:
00089         if add_data.current_kml_linestring is not None:
00090             kml_linestrings.append(add_data.current_kml_linestring)
00091 
00092         name = "".join([args.source, ":", str(add_data.mainstate_current)])
00093         add_data.current_kml_linestring = \
00094             kml.newlinestring(name=name, altitudemode='absolute')
00095 
00096         # set rendering options
00097         if args.extrude:
00098             add_data.current_kml_linestring.extrude = 1
00099         add_data.current_kml_linestring.style.linestyle.color = \
00100             colors[max([add_data.mainstate_current, 0])]
00101 
00102         add_data.new_linestring = False
00103 
00104         add_to_linestring(add_data.position_data,
00105                       add_data.current_kml_linestring)
00106         add_data.last_time = msg._timestamp
00107 
00108     else:
00109         if (msg._timestamp - add_data.last_time) > 0.1:
00110             add_to_linestring(add_data.position_data,
00111                       add_data.current_kml_linestring)
00112             add_data.last_time = msg._timestamp
00113 
00114     # reset position_data
00115     add_data.position_data = [None for n in position_field_type]
00116 
00117 
00118 def process_file(filename, source):
00119     '''process one file'''
00120     print("Processing %s" % filename)
00121     mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
00122     
00123     position_field_type = sniff_field_spelling(mlog, source)
00124     
00125     # init fields and field_types lists
00126     fields = [args.source + "." + s for s in position_field_type]
00127     fields.append(mainstate_field)
00128     field_types = []
00129 
00130     msg_types = set()
00131     re_caps = re.compile('[A-Z_][A-Z0-9_]+')
00132 
00133     for f in fields:
00134         caps = set(re.findall(re_caps, f))
00135         msg_types = msg_types.union(caps)
00136         field_types.append(caps)
00137     
00138     add_data.new_linestring = True
00139     add_data.mainstate_current = -1
00140     add_data.current_kml_linestring = None
00141     add_data.position_data = [None for n in position_field_type]
00142     add_data.last_time = 0
00143 
00144     while True:
00145         msg = mlog.recv_match(args.condition)
00146         if msg is None:
00147             break
00148         tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60)
00149         tdays += 719163  # pylab wants it since 0001-01-01
00150         add_data(tdays, msg, msg_types, mlog.messages, fields, field_types, position_field_type)
00151         
00152 
00153 def sniff_field_spelling(mlog, source):
00154     '''attempt to detect whether APM or PX4 attributes names are in use'''
00155     position_field_type_default = position_field_types[0] # Default to PX4 spelling
00156     
00157     msg = mlog.recv_match(source)
00158     mlog._rewind() # Unfortunately it's either call this or return a mutated object
00159     
00160     position_field_selection = [spelling for spelling in position_field_types if hasattr(msg, spelling[0])]
00161 
00162     return position_field_selection[0] if position_field_selection else position_field_type_default
00163 
00164 
00165 if __name__ == '__main__':
00166     parser = ArgumentParser(description=__doc__)
00167     parser.add_argument("--no-timestamps", dest="notimestamps",
00168                       action='store_true', help="Log doesn't have timestamps")
00169     parser.add_argument("--condition", default=None,
00170                       help="select packets by a condition [default: %(default)s]")
00171     parser.add_argument("--aoff", default=0.,
00172                       help="Altitude offset for paths that go through the"
00173                       "ground in google earth [default: %(default)s]")
00174     parser.add_argument("-o", "--output", dest="filename_out", default="mav.kml",
00175                       help="Output filename [default: %(default)s] ")
00176     parser.add_argument("-s", "--source", default="GPOS",
00177                       help="Select position data source"
00178                       "(GPOS or GPS) [default: %(default)s]")
00179     parser.add_argument("-e", "--extrude", default=False,
00180                       action='store_true',
00181                       help="Extrude paths to ground [default: %(default)s]")
00182     parser.add_argument("logs", metavar="LOG", nargs="+")
00183 
00184     args = parser.parse_args()
00185 
00186     filenames = []
00187     for f in args.logs:
00188         if os.path.exists(f):
00189             filenames.append(f)
00190 
00191     if len(filenames) == 0:
00192         print("No files to process")
00193         sys.exit(1)
00194 
00195     for fi in range(0, len(filenames)):
00196         f = filenames[fi]
00197         process_file(f, args.source)
00198 
00199     save_kml(args.filename_out)
00200 


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57