Processing/SyntheticDataset: sbet_handler.py

File sbet_handler.py, 21.6 KB (added by benj, 2 years ago)

Library for handling Applanix-format SBET files and AISA format .nav files

Line 
1#! /usr/bin/env python
2
3# Library to read and write Applanix's SBET (Smoothed Best Estimate of Trajectory) format
4# Also writes AISA's nav file format
5#
6# Author: Ben Taylor
7# Last edited: 26th Oct 2009
8#
9# Change history:
10# 7th May 2009: Created
11# 10th Jun 2009: Added writeNav function
12# 11th Aug 2009: Added getPosition and checkApproxEquality functions
13# 12th Aug 2009: Rewrote getPosition to be more efficient, removed checkApproxEquality (no longer used, not appropriate
14#                to this library on its own), added readNav function
15# 13th Aug 2009: Updated readNav to allow returning partial output
16# 8th Oct 2009: Minor fix to readNav to make it return partial output in more circumstances and to be able to scan corrupt files
17# 26th Oct 2009: Added index_only option to getPosition
18#
19# Available functions:
20# readSbet: Reads an SBET file into a list of records
21# writeSbet: Writes an SBET file from a list of (appropriately-ordered) records
22# readNav: Reads a nav file into a list of records
23# writeNav: Writes an AISA nav file from a list of (appropriately-ordered) records
24# getPosition: Gets the position record closest to a specified GPS time
25#
26# You may use or alter this script as you wish, but no warranty of any kind is offered, nor is it guaranteed
27# not to cause security holes in an unsafe environment.
28
29# SBET format contains the following fields (all 8-byte double-precision, in order).
30# All must be set in the input data array to be able to write an SBET, and they will
31# be returned in this order when reading one.
32#
33# 1: GPS time (s)
34# 2: Latitude (rad)
35# 3: Longitude (rad)
36# 4: Altitude (m)
37# 5: E-W velocity (m/s)
38# 6: N-S velocity (m/s)
39# 7: Vertical velocity (m/s)
40# 8: Attitude roll component (rad)
41# 9: Attitude pitch component (rad)
42# 10: Attitude heading component (rad)
43# 11: Wander angle (rad)
44# 12: E-W acceleration (m/s^2)
45# 13: N-S acceleration (m/s^2)
46# 14: Vertical acceleration (m/s^2)
47# 15: X-axis angular rate (rad/s)
48# 16: Y-axis angular rate (rad/s)
49# 17: Z-axis angular rate (rad/s)
50
51import re
52import sys
53import os
54import os.path
55import struct
56import math
57import warnings
58
59record_length = 136 # Record length in bytes of an SBET record
60nav_record_length = 28 # Record length in bytes of a nav record
61sync_record_length = 14 # Record length in bytes of a sync record
62sbet_rec_format = "=ddddddddddddddddd" # Record format for SBET record - 17 double-precision fields
63nav_rec_format = "=hdhhHiihh" # Record format for nav record - short-double-short-short-Ushort-int-int-short-short (standard size, leaving it as native causes pain on 64-bit machines, believe me)
64sync_rec_format = "=hhhhhhh" # Record format for specim sync record - 7 signed shorts
65
66# Function readSbet
67# Reads an SBET file into a list of records
68#
69# Arguments:
70# filename: Name of SBET file to read
71#
72# Returns: List of records read from the SBET file
73def readSbet(filename):
74   
75    record = ""
76    output = []
77   
78    # Check given file exists
79    if (not os.path.isfile(filename)):
80        raise IOError, "File " + filename + " does not exist or is not a file"
81    # end if
82   
83    # Open the SBET file
84    try:
85        sbet = open(filename, "rb")
86    except:
87        raise IOError, "Could not open SBET file " + filename, sys.exc_info()[2]
88    # end try
89   
90    # Read the first record
91    try:
92        record = sbet.read(record_length)
93    except:
94        tb = sys.exc_info()[2] # Get traceback (causes circular reference to clean up later)
95        try:
96            sbet.close()
97        except:
98            pass
99        # end try
100        raise IOError, "Reading failed on input SBET file " + filename, tb
101    finally:
102        tb = None # Break circular reference
103    # end try
104   
105    # For each record read from the file
106    while (record != ""):
107   
108        # If we ran out of file before we got a whole record, this probably isn't a valid SBET
109        if (len(record) < record_length):
110            raise IOError, "SBET record length was shorter than expected, file may be corrupt or not an SBET file"
111        # end if
112       
113        # Unpack data from binary to list and append to output
114        recdata = struct.unpack(sbet_rec_format, record)
115        output.append(recdata)
116       
117        # Read the next record
118        try:
119            record = sbet.read(record_length)
120        except:
121            tb = sys.exc_info()[2] # Get traceback (causes circular reference to clean up later)
122            try:
123                sbet.close()
124            except:
125                pass
126            # end try
127            raise IOError, "Reading failed on input SBET file " + filename, tb
128        finally:
129            tb = None # Break circular reference
130        # end try
131    # end while
132   
133    # Close the file
134    try:
135        sbet.close() # Don't really care if this fails
136    except:
137        pass
138    # end try
139   
140    return output
141# end function
142
143# Function writeSbet
144# Writes an SBET file from a list of (appropriately-ordered) records
145#
146# Arguments:
147# filename: Name of SBET file to write to
148# data: List containing records to be written
149def writeSbet(filename, data):
150
151    # Open the output file for writing
152    try:
153        sbet = open(filename, "wb")
154    except:
155        raise IOError, "Unable to open output SBET file " + filename + " for writing.", sys.exc_info()[2]
156    # end try
157   
158    # For each given record
159    for record in data:
160   
161        # If the record has other than 17 fields, it can't be put into an SBET file - remove the file created
162        if (len(record) != 17):
163            sbet.close()
164            try:
165                os.remove(filename)
166            except:
167                pass
168            # end try
169            raise ValueError, "Given data contains records with other than 17 fields, SBET files require exactly 17 fields"
170        # end if
171       
172        # Pack the record into a binary string
173        datastr = struct.pack(sbet_rec_format, record[0], record[1], record[2], record[3], record[4], record[5], record[6], record[7], record[8], record[9], record[10], record[11], record[12], record[13], record[14], record[15], record[16])
174       
175        # Write the packed string to the output file
176        try:
177            sbet.write(datastr)
178        except:
179            tb = sys.exc_info()[2] # Get traceback (causes circular reference to clean up later)
180            # If writing fails, remove the partially-written file
181            sbet.close()
182            try:
183                os.remove(filename)
184            except:
185                pass
186            # end try
187            raise IOError, "Failed writing data to output SBET file " + filename, tb
188        finally:
189            tb = None # Break circular reference
190        # end try
191    # end for
192   
193    # Close the file
194    try:
195        sbet.close()
196    except:
197        pass
198    # end try
199# end function
200
201##################################
202# Function readNav
203# Reads a nav file into a list of records
204#
205# Arguments:
206# filename: Name of nav file to read
207# partial: If True then if readNav encounters an invalid nav record it will return the valid
208#       records up to that point rather than raising an error (it will produce a warning instead,
209#       and will still raise an error if it fails at the start of the file).
210# thorough: If True then if it finds an invalid record it will scan the file byte by byte until it finds
211#       another valid record (potentially slow). If False it will just error on the first invalid record.
212#
213# Returns: List of records read from the nav file
214##################################
215def readNav(filename, partial=False, thorough=False):
216   
217    record = False
218    output = []
219    readall = False
220    byte1 = None
221    byte2 = None
222   
223    # Check given file exists
224    if (not os.path.isfile(filename)):
225        raise IOError, "File " + filename + " does not exist or is not a file"
226    # end if
227   
228    # Open the nav file
229    try:
230        nav = open(filename, "rb")
231    except:
232        raise IOError, "Could not open nav file " + filename, sys.exc_info()[2]
233    # end try
234   
235    # For each record read from the file
236    while (record != ""):
237       
238        # If we're in readall mode (ie found corrupt records and reading everything) then read two bytes and see if we've got a record
239        if (readall):
240            while (readall):
241           
242                # Read one byte
243                try:
244                    byte2 = nav.read(1)
245                except:
246                    try:
247                        sbet.close()
248                    except:
249                        pass
250                    # end try
251                    raise IOError, "Reading failed on input nav file " + filename
252                # end try
253               
254                # If we've read two bytes then check to see if it looks like the start of a record
255                if (byte1 != None):
256                    # If we read zero bytes we're at EOF
257                    if (len(byte2) == 0):
258                        if (not partial):
259                            raise IOError, "EOF reached without being in a valid record"
260                        else:
261                            if (len(output) > 0):
262                                warnings.warn("EOF reached without being in a valid record, " + filename + " may be corrupt or not a nav file. Outputting " + str(len(output)) + " valid results so far because partial output specified", UserWarning, 3)
263                                try:
264                                    nav.close()
265                                except:
266                                    pass
267                                # end try
268                                return output
269                            else:
270                                raise IOError, "EOF reached without being in a valid record"
271                            # end if
272                        # end if
273                    else:
274                        # Concatenate the last two bytes we read and unpack them
275                        dataitem = struct.unpack("h", byte1 + byte2)[0]
276                       
277                        # If the number is a start of record flag then we're probably on a new valid record,
278                        # read the rest of the record and then start reading normally
279                        if ((dataitem == -28160) or (dataitem == -32257)):
280                            try:
281                                extra = nav.read(sync_record_length - 2)
282                            except:
283                                tb = sys.exc_info()[2] # Get traceback (causes circular reference to clean up later)
284                                try:
285                                    sbet.close()
286                                except:
287                                    pass
288                                # end try
289                                raise IOError, "Reading failed on input nav file " + filename, tb
290                            finally:
291                                tb = None # Break circular reference
292                            # end try
293                           
294                            # Add the two bytes we've got to the ones we just read to get the whole record
295                            record = byte1 + byte2 + extra
296                            readall = False
297                        # end if
298                    # end if
299                # end if
300               
301                # If we're here then we didn't start a valid record, discard byte1, copy byte2 to byte1 and then scan the next byte
302                byte1 = byte2           
303            # end while
304        else:
305            # Read enough for a sync record
306            try:
307                record = nav.read(sync_record_length)
308            except:
309                tb = sys.exc_info()[2] # Get traceback (causes circular reference to clean up later)
310                try:
311                    sbet.close()
312                except:
313                    pass
314                # end try
315                raise IOError, "Reading failed on input nav file " + filename, tb
316            finally:
317                tb = None # Break circular reference
318            # end try
319        # end if
320       
321        # If we've got a zero-length record we're at EOF
322        if (len(record) == 0):
323            break
324        # end if
325   
326        # If we ran out of file before we got a whole record, this probably isn't a valid SBET
327        if (len(record) < sync_record_length):
328            if (not partial):
329                raise IOError, "Nav record length was shorter than expected, file may be corrupt or not a nav file"
330            else:
331                if (len(output) > 0):
332                    warnings.warn("Nav record length was shorter than expected, " + filename + " may be corrupt or not a nav file. Outputting " + str(len(output)) + " valid results so far because partial output specified", UserWarning, 3)
333                    break
334                else:
335                    raise IOError, "Nav record length was shorter than expected, file may be corrupt or not a nav file"
336                # end if
337            # end if
338        # end if
339       
340        # Unpack data from binary to list
341        recdata = struct.unpack(sync_rec_format, record)
342       
343        # If the record starts with -28160, it's a nav record and we need to read more from the file for this record
344        if (recdata[0] == -28160):
345           
346            # Read enough extra bytes to make up a nav record and add them to the end of the previous data
347            try:
348                record = record + nav.read(nav_record_length - sync_record_length)
349            except:
350                tb = sys.exc_info()[2] # Get traceback (causes circular reference to clean up later)
351                try:
352                    sbet.close()
353                except:
354                    pass
355                # end try
356                raise IOError, "Reading failed on input nav file " + filename, tb
357            finally:
358                tb = None # Break circular reference
359            # end try
360           
361            # If we ran out of file before we got a whole record, this probably isn't a valid nav file
362            if (len(record) < nav_record_length):
363                if (not partial):
364                    raise IOError, "Nav record length was shorter than expected, file may be corrupt or not a nav file"
365                else:
366                    if (len(output) > 0):
367                        warnings.warn("Nav record length was shorter than expected, " + filename + " may be corrupt or not a nav file. Outputting " + str(len(output)) + " valid results so far because partial output specified", UserWarning, 3)
368                        break
369                    else:
370                        raise IOError, "Nav record length was shorter than expected, file may be corrupt or not a nav file"
371                    # end if
372                # end if
373            # end if
374           
375            # Unpack data from binary to list (again, format is different)
376            recdata = struct.unpack(nav_rec_format, record)
377        elif (recdata[0] != -32257):
378            # If we get in here then the record is neither a nav record nor a sync record, in which case we don't know
379            # what it is and this isn't a valid nav file.
380            if (not thorough):
381                if (not partial):
382                    raise IOError, "Unknown record type encountered (neither nav nor sync), " + filename + " is not a valid nav file."
383                else:
384                    if (len(output) > 0):
385                        warnings.warn("Unknown record type encountered (neither nav nor sync), " + filename + " is not a valid nav file. Outputting " + str(len(output)) + " valid results so far because partial output specified", UserWarning, 3)
386                        break
387                    else:
388                        raise IOError, "Unknown record type encountered at start of file (neither nav nor sync), " + filename + " is not a valid nav file."
389                    # end if
390                # end if
391            else:
392                # If we're in here then we need to read byte-by-byte until we find something that looks like a valid record
393                readall = True
394            # end if
395        # end if
396       
397        # Copy record to output
398        if (not readall):       
399            output.append(recdata)
400        # end if
401    # end while
402   
403    # Close the file
404    try:
405        nav.close() # Don't really care if this fails
406    except:
407        pass
408    # end try
409   
410    return output
411# end function
412
413# Function writeNav
414# Writes an AISA nav file from a list of (appropriately-ordered) records
415#
416# Arguments:
417# filename: Name of nav file to write to
418# data: List containing records to be written
419def writeNav(filename, data):
420
421    # Open the output file for writing
422    try:
423        nav = open(filename, "wb")
424    except:
425        raise IOError, "Unable to open output nav file " + filename + " for writing.", sys.exc_info()[2]
426    # end try
427   
428    # For each given record
429    for record in data:
430        issync = False
431   
432        # Check number of records to see if it's a valid nav (9 fields) or sync (7 fields) record
433        if (len(record) == 9):
434            issync = False
435        elif (len(record) == 7):
436            issync = True
437        else:
438            nav.close()
439            try:
440                os.remove(filename)
441            except:
442                pass
443            # end try
444            raise ValueError, "Given data contains records with other than 7 or 9 fields, nav files require exactly 7 or 9 fields"
445        # end if
446       
447        # Pack the record into a binary string
448        if (not issync):
449            datastr = struct.pack(nav_rec_format, int(record[0]), float(record[1]), int(record[2]), int(record[3]), int(record[4]), int(record[5]), int(record[6]), int(record[7]), int(record[8]))
450        else:
451            datastr = struct.pack(sync_rec_format, int(record[0]), int(record[1]), int(record[2]), int(record[3]), int(record[4]), int(record[5]), int(record[6]))
452        # end if
453       
454        # Write the packed string to the output file
455        try:
456            nav.write(datastr)
457        except:
458            tb = sys.exc_info()[2] # Get traceback (causes circular reference to clean up later)
459            # If writing fails, remove the partially-written file
460            nav.close()
461            try:
462                os.remove(filename)
463            except:
464                pass
465            # end try
466            raise IOError, "Failed writing data to output nav file " + filename, tb
467        finally:
468            tb = None # Break circular reference
469        # end try
470    # end for
471   
472    # Close the file
473    try:
474        nav.close()
475    except:
476        pass
477    # end try
478# end function
479
480################################
481# Function getPosition
482# Gets the GPS position closest to the given GPS time from the given SBET file
483# Note: This is confusing because it turns out the SBET file actually uses UTC
484# (but times it as a time-of-week like GPS). I've elected to leave the variable
485# names and comments as GPS time because it's then clear it's a time-of-week.
486# Behaviour is unaffected because it just does a straight lookup on the time in
487# the file anyway.
488#
489# Arguments
490# gpstime: GPS time of week to match
491# sbet_data: Data list read from an SBET file using readSbet()
492# index_only: If True returns the index position of the matching record rather than the time. Default False
493#
494# Returns: A list containing the details for the SBET record found (format as for the SBET file format), or the index of the matching SBET record
495# Raises KeyError if the given GPS time was not found in the SBET file
496################################
497def getPosition(gpstime, sbet_data, index_only=False):
498   
499    output = []
500    mintimeoffset = 1.0
501    start_time = sbet_data[0][0]
502    second_time = sbet_data[1][0]
503   
504    # Work out which index the record before the given time will be from the time difference between the records
505    # Then start from one earlier than that as insurance against rounding errors
506    time_inc = round(float(second_time - start_time), 5)
507    time_diff = float(gpstime - start_time)
508    search_index = int(math.floor(time_diff / time_inc)) - 1
509   
510    # If we shifted earlier from the first record, shift back again
511    if (search_index == -1):
512        search_index = 0
513    # end if
514   
515    # Check the calculated index isn't out of range (if it is then the given time is outside the SBET range)
516    if ((search_index < 0) or (search_index >= len(sbet_data))):
517        raise KeyError, "No SBET records found matching GPS time " + str(gpstime)
518    # end if
519   
520    # Loop through the next few records, stop when we've gone past the given time
521    while ((search_index < len(sbet_data) - 1) and (sbet_data[search_index][0] < gpstime)):
522   
523        # If the time is exactly equal we can just return it (unlikely though)
524        if (sbet_data[search_index][0] == gpstime):
525            output = sbet_data[search_index]
526            break
527        # end if
528       
529        # If the next record is past the given time then the time we want is between this record and the next
530        # - return whichever is closer
531        if (sbet_data[search_index+1][0] > gpstime):
532            if (gpstime - sbet_data[search_index][0] < sbet_data[search_index+1][0] - gpstime):
533                output = sbet_data[search_index]
534            else:
535                output = sbet_data[search_index+1]
536                search_index = search_index + 1
537            # end if
538            break
539        else:
540            # If the next record is still less than the given time, increment the index
541            search_index = search_index + 1
542        # end if
543    # end while
544   
545    # If we don't have anything in output then we didn't find a match
546    if (output == []):
547        raise KeyError, "No SBET records found matching GPS time " + str(gpstime)
548    # end if
549   
550    if (not index_only):
551        return output
552    else:
553        return search_index
554    # end if
555# end function