parser.py
Go to the documentation of this file.
1 # Software License Agreement (BSD License)
2 #
3 # Copyright (c) 2013, Eric Perko
4 # All rights reserved.
5 #
6 # Redistribution and use in source and binary forms, with or without
7 # modification, are permitted provided that the following conditions
8 # are met:
9 #
10 # * Redistributions of source code must retain the above copyright
11 # notice, this list of conditions and the following disclaimer.
12 # * Redistributions in binary form must reproduce the above
13 # copyright notice, this list of conditions and the following
14 # disclaimer in the documentation and/or other materials provided
15 # with the distribution.
16 # * Neither the names of the authors nor the names of their
17 # affiliated organizations may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 import re
34 import time
35 import calendar
36 import math
37 import logging
38 logger = logging.getLogger('rosout')
39 
40 
41 def safe_float(field):
42  try:
43  return float(field)
44  except ValueError:
45  return float('NaN')
46 
47 
48 def safe_int(field):
49  try:
50  return int(field)
51  except ValueError:
52  return 0
53 
54 
55 def convert_latitude(field):
56  return safe_float(field[0:2]) + safe_float(field[2:]) / 60.0
57 
58 
59 def convert_longitude(field):
60  return safe_float(field[0:3]) + safe_float(field[3:]) / 60.0
61 
62 
63 def convert_time(nmea_utc):
64  # Get current time in UTC for date information
65  utc_struct = time.gmtime() # immutable, so cannot modify this one
66  utc_list = list(utc_struct)
67  # If one of the time fields is empty, return NaN seconds
68  if not nmea_utc[0:2] or not nmea_utc[2:4] or not nmea_utc[4:6]:
69  return float('NaN')
70  else:
71  hours = int(nmea_utc[0:2])
72  minutes = int(nmea_utc[2:4])
73  seconds = int(nmea_utc[4:6])
74  utc_list[3] = hours
75  utc_list[4] = minutes
76  utc_list[5] = seconds
77  unix_time = calendar.timegm(tuple(utc_list))
78  return unix_time
79 
80 
81 def convert_status_flag(status_flag):
82  if status_flag == "A":
83  return True
84  elif status_flag == "V":
85  return False
86  else:
87  return False
88 
89 
91  return safe_float(knots) * 0.514444444444
92 
93 
94 # Need this wrapper because math.radians doesn't auto convert inputs
96  return math.radians(safe_float(degs))
97 
98 """Format for this is a sentence identifier (e.g. "GGA") as the key, with a
99 tuple of tuples where each tuple is a field name, conversion function and index
100 into the split sentence"""
101 parse_maps = {
102  "GGA": [
103  ("fix_type", int, 6),
104  ("latitude", convert_latitude, 2),
105  ("latitude_direction", str, 3),
106  ("longitude", convert_longitude, 4),
107  ("longitude_direction", str, 5),
108  ("altitude", safe_float, 9),
109  ("mean_sea_level", safe_float, 11),
110  ("hdop", safe_float, 8),
111  ("num_satellites", safe_int, 7),
112  ("utc_time", convert_time, 1),
113  ],
114  "RMC": [
115  ("utc_time", convert_time, 1),
116  ("fix_valid", convert_status_flag, 2),
117  ("latitude", convert_latitude, 3),
118  ("latitude_direction", str, 4),
119  ("longitude", convert_longitude, 5),
120  ("longitude_direction", str, 6),
121  ("speed", convert_knots_to_mps, 7),
122  ("true_course", convert_deg_to_rads, 8),
123  ]
124  }
125 
126 
127 def parse_nmea_sentence(nmea_sentence):
128  # Check for a valid nmea sentence
129  if not re.match('(^\$GP|^\$GN|^\$GL).*\*[0-9A-Fa-f]{2}$', nmea_sentence):
130  logger.debug("Regex didn't match, sentence not valid NMEA? Sentence was: %s"
131  % repr(nmea_sentence))
132  return False
133  fields = [field.strip(',') for field in nmea_sentence.split(',')]
134 
135  # Ignore the $ and talker ID portions (e.g. GP)
136  sentence_type = fields[0][3:]
137 
138  if not sentence_type in parse_maps:
139  logger.debug("Sentence type %s not in parse map, ignoring."
140  % repr(sentence_type))
141  return False
142 
143  parse_map = parse_maps[sentence_type]
144 
145  parsed_sentence = {}
146  for entry in parse_map:
147  parsed_sentence[entry[0]] = entry[1](fields[entry[2]])
148 
149  return {sentence_type: parsed_sentence}
def convert_status_flag(status_flag)
Definition: parser.py:81
def convert_time(nmea_utc)
Definition: parser.py:63
def convert_deg_to_rads(degs)
Definition: parser.py:95
def parse_nmea_sentence(nmea_sentence)
Definition: parser.py:127
def convert_latitude(field)
Definition: parser.py:55
def convert_longitude(field)
Definition: parser.py:59
def convert_knots_to_mps(knots)
Definition: parser.py:90


rosflight_utils
Author(s):
autogenerated on Wed Jul 3 2019 20:00:31