From 8cf68742578e584fd8853cf689d9454291c42fae Mon Sep 17 00:00:00 2001 From: Tao Peng Date: Mon, 23 Jun 2025 13:35:35 -0700 Subject: [PATCH 1/3] improve: handle NMEA GGA/GLL/RMC --- mapillary_tools/blackvue_parser.py | 63 +++++++++++++------ .../process_sequence_properties.py | 17 +++-- 2 files changed, 52 insertions(+), 28 deletions(-) diff --git a/mapillary_tools/blackvue_parser.py b/mapillary_tools/blackvue_parser.py index 07fc08e7d..29c6966ab 100644 --- a/mapillary_tools/blackvue_parser.py +++ b/mapillary_tools/blackvue_parser.py @@ -17,11 +17,11 @@ NMEA_LINE_REGEX = re.compile( rb""" ^\s* - \[(\d+)\] # timestamp + \[(\d+)\] # Timestamp \s* - (\$\w{5}.*) # nmea line + (\$\w{5}.*) # NMEA message \s* - (\[\d+\])? # strange timestamp + (\[\d+\])? # Strange timestamp \s*$ """, re.X, @@ -118,8 +118,16 @@ def _parse_gps_box(gps_data: bytes) -> T.Generator[geo.Point, None, None]: """ >>> list(_parse_gps_box(b"[1623057074211]$GPGGA,202530.00,5109.0262,N,11401.8407,W,5,40,0.5,1097.36,M,-17.00,M,18,TSTR*61")) [Point(time=1623057074211, lat=51.150436666666664, lon=-114.03067833333333, alt=1097.36, angle=None)] + >>> list(_parse_gps_box(b"[1629874404069]$GNGGA,175322.00,3244.53126,N,11710.97811,W,1,12,0.84,17.4,M,-34.0,M,,*45")) [Point(time=1629874404069, lat=32.742187666666666, lon=-117.1829685, alt=17.4, angle=None)] + + >>> list(_parse_gps_box(b"[1629874404069]$GNGLL,4404.14012,N,12118.85993,W,001037.00,A,A*67")) + [Point(time=1629874404069, lat=44.069002, lon=-121.31433216666667, alt=None, angle=None)] + + >>> list(_parse_gps_box(b"[1629874404069]$GNRMC,001031.00,A,4404.13993,N,12118.86023,W,0.146,,100117,,,A*7B")) + [Point(time=1629874404069, lat=44.06899883333333, lon=-121.31433716666666, alt=None, angle=None)] + >>> list(_parse_gps_box(b"[1623057074211]$GPVTG,,T,,M,0.078,N,0.144,K,D*28[1623057075215]")) [] """ @@ -128,24 +136,43 @@ def _parse_gps_box(gps_data: bytes) -> T.Generator[geo.Point, None, None]: if match is None: continue nmea_line_bytes = match.group(2) - if nmea_line_bytes.startswith(b"$GPGGA") or nmea_line_bytes.startswith( - b"$GNGGA" - ): - try: - nmea_line = nmea_line_bytes.decode("utf8") - except UnicodeDecodeError: - continue - try: - nmea = pynmea2.parse(nmea_line) - except pynmea2.nmea.ParseError: + + if not nmea_line_bytes: + continue + + try: + nmea_line = nmea_line_bytes.decode("utf8") + except UnicodeDecodeError: + continue + + if not nmea_line: + continue + + try: + message = pynmea2.parse(nmea_line) + except pynmea2.nmea.ParseError: + continue + + epoch_ms = int(match.group(1)) + + # https://tavotech.com/gps-nmea-sentence-structure/ + if message.sentence_type in ["GGA"]: + if not message.is_valid: continue - if not nmea.is_valid: + yield geo.Point( + time=epoch_ms, + lat=message.latitude, + lon=message.longitude, + alt=message.altitude, + angle=None, + ) + elif message.sentence_type in ["RMC", "GLL"]: + if not message.is_valid: continue - epoch_ms = int(match.group(1)) yield geo.Point( time=epoch_ms, - lat=nmea.latitude, - lon=nmea.longitude, - alt=nmea.altitude, + lat=message.latitude, + lon=message.longitude, + alt=None, angle=None, ) diff --git a/mapillary_tools/process_sequence_properties.py b/mapillary_tools/process_sequence_properties.py index e20513527..9c325614f 100644 --- a/mapillary_tools/process_sequence_properties.py +++ b/mapillary_tools/process_sequence_properties.py @@ -24,24 +24,21 @@ def split_sequence_by( """ output_sequences: list[list[SeqItem]] = [] - seq = iter(sequence) - - prev = next(seq, None) - if prev is None: - return output_sequences - - output_sequences.append([prev]) + if sequence: + output_sequences.append([sequence[0]]) - for cur in seq: + for prev, cur in geo.pairwise(sequence): # invariant: prev is processed if should_split(prev, cur): output_sequences.append([cur]) else: output_sequences[-1].append(cur) - prev = cur # invariant: cur is processed - assert sum(len(s) for s in output_sequences) == len(sequence) + assert sum(len(s) for s in output_sequences) == len(sequence), ( + output_sequences, + sequence, + ) return output_sequences From 421d1cd1fddd78ce06e75f0ce15f7b0f2f1aa119 Mon Sep 17 00:00:00 2001 From: Tao Peng Date: Mon, 23 Jun 2025 14:13:05 -0700 Subject: [PATCH 2/3] tests --- tests/unit/test_blackvue_parser.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/tests/unit/test_blackvue_parser.py b/tests/unit/test_blackvue_parser.py index 2a430ce9b..c4284679f 100644 --- a/tests/unit/test_blackvue_parser.py +++ b/tests/unit/test_blackvue_parser.py @@ -47,6 +47,20 @@ def test_parse_points(): geo.Point( time=0.0, lat=38.8861575, lon=-76.99239516666667, alt=10.2, angle=None ), + geo.Point( + time=0.0, + lat=38.8861575, + lon=-76.99239516666667, + alt=None, + angle=None, + ), + geo.Point( + time=0.0, + lat=38.88615816666667, + lon=-76.992434, + alt=None, + angle=None, + ), geo.Point( time=0.968, lat=38.88615816666667, lon=-76.992434, alt=7.7, angle=None ), From 88ff58b04190d6a00017966c0ddae76575963183 Mon Sep 17 00:00:00 2001 From: Tao Peng Date: Mon, 23 Jun 2025 14:17:02 -0700 Subject: [PATCH 3/3] tests --- tests/unit/test_blackvue_parser.py | 39 ++++++++++++++---------------- 1 file changed, 18 insertions(+), 21 deletions(-) diff --git a/tests/unit/test_blackvue_parser.py b/tests/unit/test_blackvue_parser.py index c4284679f..6cb1eb014 100644 --- a/tests/unit/test_blackvue_parser.py +++ b/tests/unit/test_blackvue_parser.py @@ -7,36 +7,33 @@ def test_parse_points(): gps_data = b""" - # GGA - [1623057130221]$GPGGA,201205.00,3853.16949,N,07659.54604,W,2,10,0.82,7.7,M,-34.7,M,,0000*6F +[1623057130221]$GPGGA,201205.00,3853.16949,N,07659.54604,W,2,10,0.82,7.7,M,-34.7,M,,0000*6F - # GGA - [1623057129253]$GPGGA,201204.00,3853.16945,N,07659.54371,W,2,10,0.99,10.2,M,-34.7,M,,0000*5C +[1623057129253]$GPGGA,201204.00,3853.16945,N,07659.54371,W,2,10,0.99,10.2,M,-34.7,M,,0000*5C - [1623057129253]$GPGSA,A,3,19,02,06,12,17,09,05,20,04,25,,,1.83,0.99,1.54*0C +[1623057129253]$GPGSA,A,3,19,02,06,12,17,09,05,20,04,25,,,1.83,0.99,1.54*0C - [1623057129253]$GPGSV,3,1,12,02,67,331,39,04,08,040,21,05,28,214,30,06,53,047,31*71 +[1623057129253]$GPGSV,3,1,12,02,67,331,39,04,08,040,21,05,28,214,30,06,53,047,31*71 - [1623057129253]$GPGSV,3,2,12,09,23,071,28,12,48,268,41,17,17,124,26,19,38,117,35*78 +[1623057129253]$GPGSV,3,2,12,09,23,071,28,12,48,268,41,17,17,124,26,19,38,117,35*78 - [1623057129253]$GPGSV,3,3,12,20,23,221,35,25,26,307,39,46,20,244,35,51,35,223,40*72 +[1623057129253]$GPGSV,3,3,12,20,23,221,35,25,26,307,39,46,20,244,35,51,35,223,40*72 - [1623057129253]$GPGLL,3853.16945,N,07659.54371,W,201204.00,A,D*70 +[1623057129255]$GPGLL,3853.16945,N,07659.54371,W,201204.00,A,D*70 - [1623057129253]$GPRMC,201205.00,A,3853.16949,N,07659.54604,W,5.849,284.43,070621,,,D*76 +[1623057129256]$GPRMC,201205.00,A,3853.16949,N,07659.54604,W,5.849,284.43,070621,,,D*76 - [1623057129253]$GPVTG,284.43,T,,M,5.849,N,10.833,K,D*08[1623057130221] +[1623057129257]$GPVTG,284.43,T,,M,5.849,N,10.833,K,D*08[1623057130221] - # GGA - [1623057130221]$GPGGA,201205.00,3853.16949,N,07659.54604,W,2,10,0.82,7.7,M,-34.7,M,,0000*6F +[1623057130258]$GPGGA,201205.00,3853.16949,N,07659.54604,W,2,10,0.82,7.7,M,-34.7,M,,0000*6F - # invalid line - [1623057130221]$GPGGA,**&^%$%$&(&(*(&&(^^*^*^^*&^&*)))) +# invalid line +[1623057130221]$GPGGA,**&^%$%$&(&(*(&&(^^*^*^^*&^&*)))) - # invalid line - [1623057130221]$GPGGA,\x00\x00\x1c\xff +# invalid line +[1623057130221]$GPGGA,\x00\x00\x1c\xff - [1623057130221]$GPGSA,A,3,19,02,06,12,17,09,05,20,04,25,,,1.65,0.82,1.43*08 +[1623057130221]$GPGSA,A,3,19,02,06,12,17,09,05,20,04,25,,,1.65,0.82,1.43*08 """ box = {"type": b"free", "data": [{"type": b"gps ", "data": gps_data}]} @@ -48,14 +45,14 @@ def test_parse_points(): time=0.0, lat=38.8861575, lon=-76.99239516666667, alt=10.2, angle=None ), geo.Point( - time=0.0, + time=0.002, lat=38.8861575, lon=-76.99239516666667, alt=None, angle=None, ), geo.Point( - time=0.0, + time=0.003, lat=38.88615816666667, lon=-76.992434, alt=None, @@ -65,6 +62,6 @@ def test_parse_points(): time=0.968, lat=38.88615816666667, lon=-76.992434, alt=7.7, angle=None ), geo.Point( - time=0.968, lat=38.88615816666667, lon=-76.992434, alt=7.7, angle=None + time=1.005, lat=38.88615816666667, lon=-76.992434, alt=7.7, angle=None ), ] == list(info.gps or [])