-
Notifications
You must be signed in to change notification settings - Fork 13
Expand file tree
/
Copy pathanalyzer.py
More file actions
executable file
·3468 lines (2950 loc) · 161 KB
/
analyzer.py
File metadata and controls
executable file
·3468 lines (2950 loc) · 161 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#!/usr/bin/env python3
from typing import Tuple, Union, List, Any
from collections import namedtuple, defaultdict
import copy
import inspect
import os
import sys
import webbrowser
from gpstime import gpstime, gps2unix
from palettable.tableau import Tableau_20
import plotly
import plotly.graph_objs as go
from plotly.subplots import make_subplots
from pymap3d import geodetic2ecef
# If running as a script, add fusion-engine-client/ to the Python import path and correct __package__ to enable relative
# imports.
if __name__ == "__main__" and (__package__ is None or __package__ == ''):
root_dir = os.path.normpath(os.path.join(os.path.dirname(__file__), '../..'))
sys.path.append(root_dir)
__package__ = "fusion_engine_client.analysis"
from ..messages import *
from .attitude import get_enu_rotation_matrix
from .data_loader import DataLoader, TimeRange
from ..parsers.file_index import HostTimeIndexMap
from ..utils import trace as logging
from ..utils.argument_parser import ArgumentParser, ExtendedBooleanAction, TriStateBooleanAction, CSVAction
from ..utils.log import define_cli_arguments as define_log_search_arguments, locate_log
from ..utils.numpy_utils import find_first
from ..utils.trace import HighlightFormatter
_logger = logging.getLogger('point_one.fusion_engine.analysis.analyzer')
SolutionTypeInfo = namedtuple('SolutionTypeInfo', ['name', 'style'])
_SOLUTION_TYPE_MAP = {
SolutionType.Invalid: SolutionTypeInfo(name='Invalid', style={'color': 'black'}),
SolutionType.Integrate: SolutionTypeInfo(name='Integrated', style={'color': 'cyan'}),
SolutionType.AutonomousGPS: SolutionTypeInfo(name='Standalone', style={'color': 'red'}),
SolutionType.DGPS: SolutionTypeInfo(name='DGPS', style={'color': 'blue'}),
SolutionType.RTKFloat: SolutionTypeInfo(name='RTK Float', style={'color': 'green'}),
SolutionType.RTKFixed: SolutionTypeInfo(name='RTK Fixed', style={'color': 'orange'}),
SolutionType.PPP: SolutionTypeInfo(name='PPP', style={'color': 'pink'}),
SolutionType.Visual: SolutionTypeInfo(name='Vision', style={'color': 'purple'}),
}
def _data_to_table(col_titles: List[str], values: List[List[Any]], row_major: bool = False, id='table'):
if row_major:
# If values is row major (outer index is the table rows), transpose it.
col_values = list(map(list, zip(*values)))
else:
col_values = values
table_html = f'''\
<table id={id}>
<tbody style="vertical-align: top">
<tr style="background-color: #a2c4fa">
'''
for title in col_titles:
table_html += f'<th>{title}</th>'
table_html += '</tr>'
num_rows = min([len(l) for l in col_values])
for row_idx in range(num_rows):
table_html += '<tr>'
separator_row = col_values[0][row_idx] is None
for col_data in col_values:
if separator_row:
table_html += '<td><hr></td>'
else:
table_html += f'<td>{col_data[row_idx]}</td>'
table_html += '</tr>'
table_html += '''\
</tbody>
</table>
'''
return table_html.replace('\n', '')
_page_template = '''\
<!DOCTYPE html PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html>
<head>
<meta content="text/html; charset=ISO-8859-1" http-equiv="content-type">
<title>%(title)s</title>
</head>
<body>
%(body)s
</body>
</html>
'''
class Analyzer(object):
logger = _logger
LONG_LOG_DURATION_SEC = 2 * 3600.0
HIGH_MEASUREMENT_RATE_HZ = 40.0
def __init__(self,
file: Union[DataLoader, str], ignore_index: bool = False,
output_dir: str = None, prefix: str = '',
time_range: TimeRange = None, max_messages: int = None,
time_axis: str = 'relative',
truncate_long_logs: bool = True, source_id: Optional[List[int]] = None):
"""!
@brief Create an analyzer for the specified log.
@param file A @ref DataLoader instance, or the path to a file to be loaded.
@param ignore_index If `True`, do not use the `.p1i` index file if present, and instead regenerate it from the
`.p1log` data file.
@param output_dir The directory where output will be stored.
@param prefix An optional prefix to be appended to the generated filenames.
@param time_range An optional @ref TimeRange object specifying desired start and end time bounds of the data to
be read. See @ref TimeRange for more details.
@param max_messages If set, read up to the specified maximum number of messages. Applies across all message
types.
@param time_axis Specify the way in which time will be plotted:
- `absolute`, `abs` - Absolute P1 or system timestamps
- `relative`, `rel` - Elapsed time since the start of the log
@param truncate_long_logs If `True`, reduce or skip certain plots if the log extremely long (as defined by
@ref LONG_LOG_DURATION_SEC).
"""
if isinstance(file, str):
self.reader = DataLoader(file, ignore_index=ignore_index)
self.host_time_mapper = HostTimeIndexMap.from_data_path(self.reader.get_index(), file)
if self.host_time_mapper is not None:
_logger.info('Loaded host time map.')
else:
self.reader = file
self.host_time_mapper = None
self.output_dir = output_dir
self.prefix = prefix
self.params = {
'time_range': time_range,
'max_messages': max_messages,
'show_progress': True,
'return_numpy': True
}
# If source ID was unspecified, use _all_ source IDs found in the log. If source ID _was_ specified, use the
# intersection of the requested source ID(s) and the available source IDs.
if source_id is None:
self.source_ids = self.reader.get_available_source_ids()
else:
source_ids = set(source_id)
unavailable_source_ids = source_ids.difference(self.reader.get_available_source_ids())
if len(unavailable_source_ids) > 0:
self.logger.warning('Not all source IDs requested are available. Cannot extract the following '
'source IDs: {}'.format(unavailable_source_ids))
self.source_ids = source_ids.intersection(self.reader.get_available_source_ids())
# If the requested pose source IDs are unavailable, warn.
if len(self.source_ids) == 0:
self.logger.warning('Requested source IDs unavailable. Cannot extract pose data.')
if len(self.source_ids) > 0:
self.default_source_id = min(self.source_ids)
else:
self.default_source_id = 0
if time_axis in ('relative', 'rel'):
self.time_axis = 'relative'
self.t0 = self.reader.t0
if self.t0 is None:
self.t0 = Timestamp()
self.system_t0 = self.reader.get_system_t0()
if self.system_t0 is None:
self.system_t0 = np.nan
self.p1_time_label = 'Relative Time (sec)'
self.system_time_label = 'Relative Time (sec)'
elif time_axis in ('absolute', 'abs'):
self.time_axis = 'absolute'
self.t0 = Timestamp(0.0)
self.system_t0 = 0.0
self.p1_time_label = 'P1 Time (sec)'
self.system_time_label = 'System Time (sec)'
else:
raise ValueError(f"Unsupported time axis specifier '{time_axis}'.")
self.plots = {}
self.summary = ''
self._mapbox_token_missing = False
self._gnss_signals_data = None
if self.output_dir is not None:
if not os.path.exists(self.output_dir):
os.makedirs(self.output_dir)
# Determine if this is a long log. In practice, some plots can be extremely slow to generate for long logs
# because of plotly limitations when handling a lot of traces (signal status, sky plot), or some may generate
# HTML files, but may fail to load in the browser because of plotly call stack errors:
# Uncaught RangeError: Maximum call stack size exceeded
#
# To get around this, those plots may be reduced in scope, downsampled, or disabled entirely unless the user
# says not to.
_, processing_duration_sec = self._calculate_duration()
self.long_log_detected = processing_duration_sec > self.LONG_LOG_DURATION_SEC
self.truncate_data = False
if self.long_log_detected:
if truncate_long_logs:
_logger.warning('Log duration very long (%.1f hours > %.1f hours). Some plots may be reduced or '
'disabled.' %
(processing_duration_sec / 3600.0, self.LONG_LOG_DURATION_SEC / 3600.0))
self.truncate_data = True
else:
_logger.warning('Log duration very long (%.1f hours > %.1f hours). Some plots may be very slow to '
'generate or load.' %
(processing_duration_sec / 3600.0, self.LONG_LOG_DURATION_SEC / 3600.0))
def plot_time_scale(self):
if self.output_dir is None:
return
# Setup the figure.
time_axis_str = 'Relative Time' if self.time_axis == 'relative' else 'P1/System Time'
p1_time_axis_str = 'Relative Time' if self.time_axis == 'relative' else 'P1 Time'
figure = make_subplots(rows=2, cols=1, print_grid=False, shared_xaxes=True,
subplot_titles=[f'Device Time vs. {time_axis_str}',
f'Pose Message Interval vs. {p1_time_axis_str}'])
figure['layout'].update(showlegend=True, modebar_add=['v1hovermode'])
figure['layout']['xaxis1'].update(title=f"{time_axis_str} (sec)", showticklabels=True)
figure['layout']['xaxis2'].update(title=f"{p1_time_axis_str} (sec)", showticklabels=True)
figure['layout']['yaxis1'].update(title="Absolute Time",
ticktext=['P1/GPS Time', 'System Time'],
tickvals=[1, 2])
figure['layout']['yaxis2'].update(title="Interval (sec)", rangemode="tozero")
# Read the pose data to get P1 and GPS timestamps.
result = self.reader.read(message_types=[PoseMessage], source_ids=self.default_source_id, **self.params)
pose_data = result[PoseMessage.MESSAGE_TYPE]
if len(pose_data.p1_time) > 0:
time = pose_data.p1_time - float(self.t0)
# Calculate time intervals, rounded to the nearest 0.1 ms.
dp1_time = np.diff(time, prepend=np.nan)
dp1_time = np.round(dp1_time * 1e4) * 1e-4
dgps_time = np.diff(pose_data.gps_time, prepend=np.nan)
dgps_time = np.round(dgps_time * 1e4) * 1e-4
# plotly starts to struggle with > 3 hours of data and won't display mouseover text, so decimate if
# necessary.
decimation_limit_sec = 3 * 3600.0
dt_sec = time[-1] - time[0]
dp1_stats = None
dgps_stats = None
if dt_sec >= decimation_limit_sec:
step = math.ceil(dt_sec / decimation_limit_sec)
idx = np.full_like(time, False, dtype=bool)
idx[0::step] = True
time = time[idx]
p1_time = pose_data.p1_time[idx]
gps_time = pose_data.gps_time[idx]
# Since we are going to decimate the data, we first calculate min/max values for all epochs in each step
# size. That way we can plot min/max, in addition to the value that does not get dropped, to avoid
# hiding outliers that do get dropped (e.g., missing a gap of 0.2 seconds (1x 10 Hz output dropped) when
# decimating by 3).
def _calc_stats(input):
num_remaining = len(idx) % step
if num_remaining == 0:
subset = input
else:
subset = input[:-num_remaining]
grouped = subset.reshape((-1, step))
stats = {
'max': np.nanmax(grouped, axis=1),
'min': np.nanmin(grouped, axis=1)
}
if num_remaining != 0:
stats['max'] = np.append(stats['max'], np.nanmax(input[-num_remaining:]))
stats['min'] = np.append(stats['min'], np.nanmin(input[-num_remaining:]))
return stats
dp1_stats = _calc_stats(dp1_time)
dgps_stats = _calc_stats(dgps_time)
dp1_time = dp1_time[idx]
dgps_time = dgps_time[idx]
figure.layout.annotations[0].text += "<br>Decimated %dx" % step
figure.layout.annotations[1].text += "<br>Decimated %dx" % step
else:
p1_time = pose_data.p1_time
gps_time = pose_data.gps_time
text = ['P1: %.3f sec<br>%s' % (p, self._gps_sec_to_string(g)) for p, g in zip(p1_time, gps_time)]
figure.add_trace(go.Scattergl(x=time, y=np.full_like(time, 1), name='P1/GPS Time', text=text,
hoverlabel={'namelength': -1},
mode='markers', marker={'color': 'blue'}),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=dp1_time, name='P1 Time Interval', text=text,
hoverlabel={'namelength': -1},
mode='markers', marker={'color': 'red'}),
2, 1)
if dp1_stats is not None:
figure.add_trace(go.Scattergl(x=time, y=dp1_stats['max'], name='P1 Time Interval (Max)',
hoverlabel={'namelength': -1},
mode='markers', marker={'symbol': 'triangle-up-open'}),
2, 1)
figure.add_trace(go.Scattergl(x=time, y=dp1_stats['min'], name='P1 Time Interval (Min)',
hoverlabel={'namelength': -1},
mode='markers', marker={'symbol': 'triangle-down-open'}),
2, 1)
figure.add_trace(go.Scattergl(x=time, y=dgps_time, name='GPS Time Interval', text=text,
hoverlabel={'namelength': -1},
mode='markers', marker={'color': 'green'}),
2, 1)
if dgps_stats is not None:
figure.add_trace(go.Scattergl(x=time, y=dgps_stats['max'], name='GPS Time Interval (Max)',
hoverlabel={'namelength': -1},
mode='markers', marker={'symbol': 'triangle-up-open'}),
2, 1)
figure.add_trace(go.Scattergl(x=time, y=dgps_stats['min'], name='GPS Time Interval (Min)',
hoverlabel={'namelength': -1},
mode='markers', marker={'symbol': 'triangle-down-open'}),
2, 1)
# Read system timestamps from event notifications, if present.
result = self.reader.read(message_types=[EventNotificationMessage], **self.params)
event_data = result[EventNotificationMessage.MESSAGE_TYPE]
system_time_sec = None
if len(event_data.messages) > 0:
system_time_sec = np.array([(m.system_time_ns * 1e-9) for m in event_data.messages])
if system_time_sec is not None:
time = system_time_sec - self.system_t0
# plotly starts to struggle with > 2 hours of data and won't display mouseover text, so decimate if
# necessary.
dt_sec = time[-1] - time[0]
if dt_sec > 7200.0:
step = math.ceil(dt_sec / 7200.0)
idx = np.full_like(time, False, dtype=bool)
idx[0::step] = True
time = time[idx]
system_time_sec = system_time_sec[idx]
text = ['System: %.3f sec' % t for t in system_time_sec]
figure.add_trace(go.Scattergl(x=time, y=np.full_like(time, 2), name='System Time', text=text,
hoverlabel={'namelength': -1},
mode='markers', marker={'color': 'purple'}),
1, 1)
self._add_figure(name="time_scale", figure=figure, title="Time Scale")
def plot_latency(self):
if self.output_dir is None:
return
if self.host_time_mapper is None:
return
# Read the pose data to get P1 and GPS timestamps.
# The message_index will be used to map back to the host times the message data was received.
# This will then be compared to the GPS time, assuming the host time was synchronized to GPS time.
result = self.reader.read(message_types=[PoseMessage], return_message_index=True,
source_ids=self.default_source_id, **self.params)
pose_data = result[PoseMessage.MESSAGE_TYPE]
valid_idx = ~np.isnan(pose_data.gps_time)
if np.sum(valid_idx) == 0:
return
p1_time = pose_data.p1_time[valid_idx]
gps_time = pose_data.gps_time[valid_idx]
message_index = pose_data.message_index[valid_idx]
last_gps_time = gps_time[-1]
# Setup the figure.
figure = make_subplots(rows=1, cols=1, print_grid=False, shared_xaxes=True,
subplot_titles=[f'Pose Message Latency'])
figure['layout'].update(showlegend=True, modebar_add=['v1hovermode'])
figure['layout']['xaxis1'].update(title=self.p1_time_label, showticklabels=True)
figure['layout']['yaxis1'].update(title="Latency (sec)")
time = p1_time - float(self.t0)
# Use the last GPS Time to get the offset between UNIX and GPS time. This includes the epoch difference and the
# current leap second.
gps_posix_offset = gps2unix(last_gps_time) - last_gps_time
gps_posix_times = ((gps_time + gps_posix_offset) * 1e9).astype('datetime64[ns]')
host_posix_times = self.host_time_mapper.get_host_timestamps(message_index)
# NOTE: The difference between host_posix_times and gps_posix_times is a combination of:
# 1. The time the positioning engine took to generate the message
# 2. The time the message took to be sent over the transport (e.x. TCP)
# 3. The time the delay before the host was able to generate the timestamp (may be large/noisy if the data was
# timestamped in user space instead of in hardware or the kernel)
# 4. The accuracy of the host's timestamping clock.
#
# This absolute latency value is only reliable if the host clock was synced to GPS time.
latency_sec = (host_posix_times - gps_posix_times).astype(float) / 1e9
text = ['P1: %.3f sec<br>%s' % (p, g) for p, g in zip(p1_time, latency_sec)]
figure.add_trace(go.Scattergl(x=time, y=latency_sec, name='Pose Message Latency', text=text,
hoverlabel={'namelength': -1},
mode='markers', marker={'color': 'blue'}),
1, 1)
figure.update_layout(title_text='NOTE: Latency assumes the host system clock is synced to GPS time. '
'Any error will impact the latency computation.')
self._add_figure(name="host_latency", figure=figure, title="Host Recieved Latency")
def plot_reset_timing(self):
if self.output_dir is None:
return
# Find reset events.
result = self.reader.read(message_types=[EventNotificationMessage], return_message_index=True, **self.params)
event_data = result[EventNotificationMessage.MESSAGE_TYPE]
reset_idx = event_data.event_type == EventType.RESET
if not np.any(reset_idx):
self.logger.info('No reset events detected. Skipping reset timing type plot.')
return
self.logger.info('Calculating reset recovery times...')
# Note that events contain system time, not P1 time. We'll assume system time is close enough to P1 time for
# purposes of calculating elapsed reset time below. In the future, we'll have a mechanism for accurately
# converting between system and P1 time.
reset_system_time_sec = event_data.system_time[reset_idx]
reset_idx = np.where(reset_idx)[0]
reset_message_indices = [event_data.message_index[i] for i in reset_idx]
# For each reset in the log, try to find the pose messages immediately following the reset where the solution
# type first goes invalid, and then where it goes valid again.
dt_reset_to_valid = np.full(reset_idx.shape, np.nan)
dt_reset_to_invalid = np.full(reset_idx.shape, np.nan)
dt_invalid_to_valid = np.full(reset_idx.shape, np.nan)
unstarted_resets = []
log_reader = self.reader.get_log_reader()
for i, reset_index in enumerate(reset_message_indices):
next_reset_index = reset_message_indices[i + 1] if i < len(reset_message_indices) - 1 else None
# Filter to all pose messages _after_ the reset event.
log_reader.clear_filters()
log_reader.rewind()
log_reader.filter_in_place(slice(reset_index + 1, next_reset_index, 1))
log_reader.filter_in_place(self.params['time_range'])
log_reader.filter_in_place(PoseMessage)
log_reader.set_show_progress(False)
# Find the pose where the solution went invalid after the reset, then where it went valid after that.
invalid_p1_time = None
valid_p1_time = None
while True:
try:
_, message, pose_index = self.reader.read_next(return_message_index=True)
except StopIteration:
break
if invalid_p1_time is None:
if message.solution_type == SolutionType.Invalid:
invalid_p1_time = message.get_p1_time()
invalid_p1_time_sec = float(invalid_p1_time)
if invalid_p1_time_sec >= reset_system_time_sec[i]:
dt_reset_to_invalid[i] = invalid_p1_time_sec - reset_system_time_sec[i]
else:
dt_reset_to_invalid[i] = 0.0
else:
if message.solution_type != SolutionType.Invalid:
valid_p1_time = message.get_p1_time()
valid_p1_time_sec = float(valid_p1_time)
invalid_p1_time_sec = float(invalid_p1_time)
if valid_p1_time_sec >= reset_system_time_sec[i]:
dt_reset_to_valid[i] = valid_p1_time_sec - reset_system_time_sec[i]
else:
dt_reset_to_valid[i] = 0.0
dt_invalid_to_valid[i] = valid_p1_time_sec - invalid_p1_time_sec
self.logger.info(' Processed %d/%d resets.' % (i + 1, len(reset_message_indices)))
break
if invalid_p1_time is None:
self.logger.warning('Unable to determine start time for reset %d at system time %.3f sec.' %
(i, reset_system_time_sec[i]))
unstarted_resets.append(i)
elif valid_p1_time is None:
self.logger.warning('Unable to calculate recovery time for reset %d at system time %.3f sec.' %
(i, reset_system_time_sec[i]))
# Setup the figure.
figure = make_subplots(rows=1, cols=1, print_grid=False, shared_xaxes=True,
subplot_titles=['Reset Recovery Time'])
figure['layout'].update(showlegend=True, modebar_add=['v1hovermode'])
figure['layout']['xaxis1'].update(title=self.system_time_label, showticklabels=True)
figure['layout']['yaxis1'].update(title="Elapsed Time (sec)", rangemode="tozero")
time = reset_system_time_sec - self.system_t0
text = ["System Time: %.3f sec" % (t + self.system_t0) for t in time]
figure.add_trace(go.Scattergl(x=time, y=dt_reset_to_valid, text=text,
name='Command -> Valid', hoverlabel={'namelength': -1},
mode='markers'),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=dt_reset_to_invalid, text=text,
name='Command -> Invalid', hoverlabel={'namelength': -1},
mode='markers'),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=dt_invalid_to_valid, text=text,
name='Invalid -> Valid', hoverlabel={'namelength': -1},
mode='markers'),
1, 1)
if len(unstarted_resets) > 0:
idx = np.array(unstarted_resets)
time = time[idx]
text = ["System Time: %.3f sec" % (t + self.system_t0) for t in time]
figure.add_trace(go.Scattergl(x=time, y=np.zeros_like(time), text=text,
name='Unstarted Resets', hoverlabel={'namelength': -1},
mode='markers'),
1, 1)
self._add_figure(name="reset_timing", figure=figure, title="Reset Recovery Timing")
def plot_pose(self):
"""!
@brief Plot position/attitude solution data.
"""
if self.output_dir is None:
return
# Read the pose data.
result = self.reader.read(message_types=[PoseMessage], source_ids=self.default_source_id, **self.params)
pose_data = result[PoseMessage.MESSAGE_TYPE]
if len(pose_data.p1_time) == 0:
self.logger.info('No pose data available. Skipping pose vs. time plot.')
return
time = pose_data.p1_time - float(self.t0)
valid_idx = np.logical_and(~np.isnan(pose_data.p1_time), pose_data.solution_type != SolutionType.Invalid)
if not np.any(valid_idx):
self.logger.info('No valid position solutions detected.')
return
first_idx = find_first(valid_idx)
# If there are no valid indices, use the last index.
if first_idx < 0:
first_idx = len(valid_idx) - 1
c_enu_ecef = get_enu_rotation_matrix(*pose_data.lla_deg[0:2, first_idx], deg=True)
# Setup the figure.
figure = make_subplots(rows=2, cols=3, print_grid=False, shared_xaxes=True,
subplot_titles=['Attitude (YPR)', 'ENU Displacement', 'Body Velocity',
'Attitude Std', 'ENU Position Std', 'Velocity Std'])
figure['layout'].update(showlegend=True, modebar_add=['v1hovermode'])
for i in range(6):
figure['layout']['xaxis%d' % (i + 1)].update(title=self.p1_time_label, showticklabels=True, matches='x')
figure['layout']['yaxis1'].update(title="Degrees")
figure['layout']['yaxis2'].update(title="Meters")
figure['layout']['yaxis3'].update(title="Meters/Second")
figure['layout']['yaxis4'].update(title="Degrees")
figure['layout']['yaxis5'].update(title="Meters")
figure['layout']['yaxis6'].update(title="Meters/Second")
# Plot YPR.
figure.add_trace(go.Scattergl(x=time, y=pose_data.ypr_deg[0, :], name='Yaw', legendgroup='yaw',
mode='lines', line={'color': 'red'}),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=pose_data.ypr_deg[1, :], name='Pitch', legendgroup='pitch',
mode='lines', line={'color': 'green'}),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=pose_data.ypr_deg[2, :], name='Roll', legendgroup='roll',
mode='lines', line={'color': 'blue'}),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=pose_data.ypr_std_deg[0, :], name='Yaw', legendgroup='yaw',
showlegend=False, mode='lines', line={'color': 'red'}),
2, 1)
figure.add_trace(go.Scattergl(x=time, y=pose_data.ypr_std_deg[1, :], name='Pitch', legendgroup='pitch',
showlegend=False, mode='lines', line={'color': 'green'}),
2, 1)
figure.add_trace(go.Scattergl(x=time, y=pose_data.ypr_std_deg[2, :], name='Roll', legendgroup='roll',
showlegend=False, mode='lines', line={'color': 'blue'}),
2, 1)
# Plot position/displacement.
position_ecef_m = np.array(geodetic2ecef(lat=pose_data.lla_deg[0, :], lon=pose_data.lla_deg[1, :],
alt=pose_data.lla_deg[0, :], deg=True))
displacement_ecef_m = position_ecef_m - position_ecef_m[:, first_idx].reshape(3, 1)
displacement_enu_m = c_enu_ecef.dot(displacement_ecef_m)
figure.add_trace(go.Scattergl(x=time, y=displacement_enu_m[0, :], name='East', legendgroup='e',
mode='lines', line={'color': 'red'}),
1, 2)
figure.add_trace(go.Scattergl(x=time, y=displacement_enu_m[1, :], name='North', legendgroup='n',
mode='lines', line={'color': 'green'}),
1, 2)
figure.add_trace(go.Scattergl(x=time, y=displacement_enu_m[2, :], name='Up', legendgroup='u',
mode='lines', line={'color': 'blue'}),
1, 2)
figure.add_trace(go.Scattergl(x=time, y=pose_data.position_std_enu_m[0, :], name='East', legendgroup='e',
showlegend=False, mode='lines', line={'color': 'red'}),
2, 2)
figure.add_trace(go.Scattergl(x=time, y=pose_data.position_std_enu_m[1, :], name='North', legendgroup='n',
showlegend=False, mode='lines', line={'color': 'green'}),
2, 2)
figure.add_trace(go.Scattergl(x=time, y=pose_data.position_std_enu_m[2, :], name='Up', legendgroup='u',
showlegend=False, mode='lines', line={'color': 'blue'}),
2, 2)
# Plot velocity.
figure.add_trace(go.Scattergl(x=time, y=pose_data.velocity_body_mps[0, :], name='X', legendgroup='x',
mode='lines', line={'color': 'red'}),
1, 3)
figure.add_trace(go.Scattergl(x=time, y=pose_data.velocity_body_mps[1, :], name='Y', legendgroup='y',
mode='lines', line={'color': 'green'}),
1, 3)
figure.add_trace(go.Scattergl(x=time, y=pose_data.velocity_body_mps[2, :], name='Z', legendgroup='z',
mode='lines', line={'color': 'blue'}),
1, 3)
figure.add_trace(go.Scattergl(x=time, y=pose_data.velocity_std_body_mps[0, :], name='X', legendgroup='x',
showlegend=False, mode='lines', line={'color': 'red'}),
2, 3)
figure.add_trace(go.Scattergl(x=time, y=pose_data.velocity_std_body_mps[1, :], name='Y', legendgroup='y',
showlegend=False, mode='lines', line={'color': 'green'}),
2, 3)
figure.add_trace(go.Scattergl(x=time, y=pose_data.velocity_std_body_mps[2, :], name='Z', legendgroup='z',
showlegend=False, mode='lines', line={'color': 'blue'}),
2, 3)
self._add_figure(name="pose", figure=figure, title="Vehicle Pose vs. Time")
def plot_calibration(self):
"""!
@brief Plot the calibration progress over time.
"""
if self.output_dir is None:
return
# Read the pose data.
result = self.reader.read(message_types=[CalibrationStatus], **self.params)
cal_data = result[CalibrationStatus.MESSAGE_TYPE]
if len(cal_data.p1_time) == 0:
self.logger.info('No calibration data available. Skipping calibration plot.')
return
time = cal_data.p1_time - float(self.t0)
text = ["Time: %.3f sec (%.3f sec)" % (t, t + float(self.t0)) for t in time]
# Map calibration stage enum values onto a [0, N) range for plotting.
stage_map = {e.value: i for i, e in enumerate(CalibrationStage)}
calibration_stage = [stage_map[s] for s in cal_data.calibration_stage]
# Setup the figure.
figure = make_subplots(rows=4, cols=1, print_grid=False, shared_xaxes=True,
subplot_titles=['<- Percent Complete // Stage ->', 'Mounting Angles',
'Mounting Angle Standard Deviation', 'Travel Distance'],
specs=[[{"secondary_y": True}], [{}], [{}], [{}]])
figure['layout'].update(showlegend=True, modebar_add=['v1hovermode'])
for i in range(4):
figure['layout']['xaxis%d' % (i + 1)].update(title=self.p1_time_label, showticklabels=True)
figure['layout']['yaxis1'].update(title="Percent Complete", range=[0, 100])
figure['layout']['yaxis2'].update(ticktext=['%s' % e.name for e in CalibrationStage],
tickvals=list(range(len(stage_map))))
figure['layout']['yaxis3'].update(title="Degrees")
figure['layout']['yaxis4'].update(title="Degrees")
figure['layout']['yaxis5'].update(title="Meters")
# Plot calibration stage and completion percentages.
figure.add_trace(go.Scattergl(x=time, y=cal_data.gyro_bias_percent_complete, text=text,
name='Gyro Bias Completion', hoverlabel={'namelength': -1},
mode='lines', line={'color': 'red'}),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=cal_data.accel_bias_percent_complete, text=text,
name='Accel Bias Completion', hoverlabel={'namelength': -1},
mode='lines', line={'color': 'green'}),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=cal_data.mounting_angle_percent_complete, text=text,
name='Mounting Angle Completion', hoverlabel={'namelength': -1},
mode='lines', line={'color': 'blue'}),
1, 1)
figure.add_trace(go.Scattergl(x=time, y=calibration_stage, name='Stage', text=text,
mode='lines', line={'color': 'black', 'dash': 'dash'}),
1, 1, secondary_y=True)
# Plot mounting angles.
figure.add_trace(go.Scattergl(x=time, y=cal_data.ypr_deg[0, :], name='Yaw', legendgroup='y', text=text,
mode='lines', line={'color': 'red'}),
2, 1)
figure.add_trace(go.Scattergl(x=time, y=cal_data.ypr_deg[1, :], name='Pitch', legendgroup='p', text=text,
mode='lines', line={'color': 'green'}),
2, 1)
figure.add_trace(go.Scattergl(x=time, y=cal_data.ypr_deg[2, :], name='Roll', legendgroup='r', text=text,
mode='lines', line={'color': 'blue'}),
2, 1)
figure.add_trace(go.Scattergl(x=time, y=cal_data.ypr_std_dev_deg[0, :], name='Yaw Std Dev', legendgroup='y',
text=text, mode='lines', line={'color': 'red'}, hoverlabel={'namelength': -1}),
3, 1)
figure.add_trace(go.Scattergl(x=time, y=cal_data.ypr_std_dev_deg[1, :], name='Pitch Std Dev', legendgroup='p',
text=text, mode='lines', line={'color': 'green'}, hoverlabel={'namelength': -1}),
3, 1)
figure.add_trace(go.Scattergl(x=time, y=cal_data.ypr_std_dev_deg[2, :], name='Roll Std Dev', legendgroup='r',
text=text, mode='lines', line={'color': 'blue'}, hoverlabel={'namelength': -1}),
3, 1)
thresh_time = time[np.array((0, -1))]
figure.add_trace(go.Scattergl(x=thresh_time, y=[cal_data.mounting_angle_max_std_dev_deg[0]] * 2,
name='Max Yaw Std Dev', legendgroup='y', hoverlabel={'namelength': -1},
mode='lines', line={'color': 'red', 'dash': 'dash'}),
3, 1)
figure.add_trace(go.Scattergl(x=thresh_time, y=[cal_data.mounting_angle_max_std_dev_deg[1]] * 2,
name='Max Pitch Std Dev', legendgroup='p', hoverlabel={'namelength': -1},
text=text, mode='lines', line={'color': 'green', 'dash': 'dash'}),
3, 1)
figure.add_trace(go.Scattergl(x=thresh_time, y=[cal_data.mounting_angle_max_std_dev_deg[2]] * 2,
name='Max Roll Std Dev', legendgroup='r', hoverlabel={'namelength': -1},
text=text, mode='lines', line={'color': 'blue', 'dash': 'dash'}),
3, 1)
# Plot travel distance.
figure.add_trace(go.Scattergl(x=time, y=cal_data.travel_distance_m, name='Travel Distance', text=text,
mode='lines', line={'color': 'blue'}, hoverlabel={'namelength': -1}),
4, 1)
figure.add_trace(go.Scattergl(x=thresh_time, y=[cal_data.min_travel_distance_m] * 2,
name='Min Travel Distance', text=text, hoverlabel={'namelength': -1},
mode='lines', line={'color': 'black', 'dash': 'dash'}),
4, 1)
self._add_figure(name="calibration", figure=figure, title="Calibration Status")
def plot_solution_type(self):
"""!
@brief Plot the solution type over time.
"""
if self.output_dir is None:
return
# Read the pose data.
result = self.reader.read(message_types=[PoseMessage], source_ids=self.default_source_id, **self.params)
pose_data = result[PoseMessage.MESSAGE_TYPE]
if len(pose_data.p1_time) == 0:
self.logger.info('No pose data available. Skipping solution type plot.')
return
# Setup the figure.
figure = make_subplots(rows=1, cols=1, print_grid=False, shared_xaxes=True, subplot_titles=['Solution Type'])
figure['layout']['xaxis'].update(title=self.p1_time_label)
figure['layout']['yaxis1'].update(title="Solution Type",
ticktext=['%s (%d)' % (e.name, e.value) for e in SolutionType],
tickvals=[e.value for e in SolutionType])
time = pose_data.p1_time - float(self.t0)
text = ["Time: %.3f sec (%.3f sec)" % (t, t + float(self.t0)) for t in time]
figure.add_trace(go.Scattergl(x=time, y=pose_data.solution_type, text=text, mode='markers'), 1, 1)
self._add_figure(name="solution_type", figure=figure, title="Solution Type")
def plot_stationary_status(self):
"""!
@brief Plot the staionary status over time.
"""
if self.output_dir is None:
return
# Read the pose data.
result = self.reader.read(message_types=[PoseMessage], source_ids=self.default_source_id, **self.params)
pose_data = result[PoseMessage.MESSAGE_TYPE]
if len(pose_data.p1_time) == 0:
self.logger.info('No pose data available. Skipping solution type plot.')
return
# Set up the figure.
figure = make_subplots(rows=1, cols=1, print_grid=False, shared_xaxes=True, subplot_titles=['Stationary Status'])
figure['layout']['xaxis'].update(title=self.p1_time_label)
figure['layout']['yaxis1'].update(title="Stationary Status",
ticktext=['Moving', 'Stationary'],
tickvals=[0, PoseMessage.FLAG_STATIONARY])
time = pose_data.p1_time - float(self.t0)
# Extract the stationary status from the pose data flags.
stationary_status = pose_data.flags & PoseMessage.FLAG_STATIONARY
text = ["Time: %.3f sec (%.3f sec)" % (t, t + float(self.t0)) for t in time]
figure.add_trace(go.Scattergl(x=time, y=stationary_status, text=text, mode='markers'), 1, 1)
self._add_figure(name="stationary_status", figure=figure, title="Stationary Status")
def _plot_displacement(self, source, time, solution_type, displacement_enu_m, std_enu_m,
title='Displacement'):
"""!
@brief Generate a topocentric (top-down) plot of position displacement, as well as plot of displacement over
time.
"""
if self.output_dir is None:
return
# Setup the figure.
topo_figure = make_subplots(rows=1, cols=1, print_grid=False, shared_xaxes=False,
subplot_titles=[title])
topo_figure['layout']['xaxis1'].update(title="East (m)")
topo_figure['layout']['yaxis1'].update(title="North (m)")
time_figure = make_subplots(rows=4, cols=1, print_grid=False, shared_xaxes=True,
subplot_titles=['3D', 'East', 'North', 'Up'])
time_figure['layout'].update(showlegend=True, modebar_add=['v1hovermode'])
for i in range(4):
time_figure['layout']['xaxis%d' % (i + 1)].update(title=self.p1_time_label, showticklabels=True)
time_figure['layout']['yaxis1'].update(title=f"{title} (m)")
time_figure['layout']['yaxis2'].update(title=f"{title} (m)")
time_figure['layout']['yaxis3'].update(title=f"{title} (m)")
time_figure['layout']['yaxis4'].update(title=f"{title} (m)")
# Remove invalid solutions.
valid_idx = np.logical_and(~np.isnan(time), solution_type != SolutionType.Invalid)
if not np.any(valid_idx):
self.logger.info('No valid position solutions detected. Skipping displacement plots.')
return
# Add statistics to the figure title.
format = 'Mean: %(mean).2f m, Median: %(median).2f m, Min: %(min).2f m, Max: %(max).2f m, Std Dev: %(std).2f m'
displacement_3d_m = np.linalg.norm(displacement_enu_m, axis=0)
extra_text = '[All] ' + format % {
'mean': np.mean(displacement_3d_m),
'median': np.median(displacement_3d_m),
'min': np.min(displacement_3d_m),
'max': np.max(displacement_3d_m),
'std': np.std(displacement_3d_m),
}
idx = solution_type == SolutionType.RTKFixed
if np.any(idx):
displacement_3d_m = np.linalg.norm(displacement_enu_m[:, idx], axis=0)
extra_text += '<br>[Fixed] ' + format % {
'mean': np.mean(displacement_3d_m),
'median': np.median(displacement_3d_m),
'min': np.min(displacement_3d_m),
'max': np.max(displacement_3d_m),
'std': np.std(displacement_3d_m),
}
topo_figure.update_layout(title_text=extra_text)
time_figure.update_layout(title_text=extra_text)
# Plot the data.
max_3d_diff_m = [0.0]
def _plot_data(name, idx, marker_style=None):
style = {'mode': 'markers', 'marker': {'size': 8}, 'showlegend': True, 'legendgroup': name,
'hoverlabel': {'namelength': -1}}
if marker_style is not None:
style['marker'].update(marker_style)
if np.any(idx):
text = ["Time: %.3f sec (%.3f sec)<br>Delta (ENU): (%.2f, %.2f, %.2f) m" \
"<br>Std (ENU): (%.2f, %.2f, %.2f) m" %
(t, t + float(self.t0), *delta, *std)
for t, delta, std in zip(time[idx], displacement_enu_m[:, idx].T, std_enu_m[:, idx].T)]
topo_figure.add_trace(go.Scattergl(x=displacement_enu_m[0, idx], y=displacement_enu_m[1, idx],
name=name, text=text, **style), 1, 1)
displacement_3d_m = np.linalg.norm(displacement_enu_m[:, idx], axis=0)
max_3d_diff_m[0] = max(max_3d_diff_m[0], np.nanmax(displacement_3d_m))
time_figure.add_trace(go.Scattergl(x=time[idx], y=displacement_3d_m,
name=name, text=text, **style), 1, 1)
style['showlegend'] = False
time_figure.add_trace(go.Scattergl(x=time[idx], y=displacement_enu_m[0, idx], name=name,
text=text, **style), 2, 1)
time_figure.add_trace(go.Scattergl(x=time[idx], y=displacement_enu_m[1, idx], name=name,
text=text, **style), 3, 1)
time_figure.add_trace(go.Scattergl(x=time[idx], y=displacement_enu_m[2, idx], name=name,
text=text, **style), 4, 1)
else:
# If there's no data, draw a dummy trace so it shows up in the legend anyway.
topo_figure.add_trace(go.Scattergl(x=[np.nan], y=[np.nan], name=name, visible='legendonly', **style),
1, 1)
time_figure.add_trace(go.Scattergl(x=[np.nan], y=[np.nan], name=name, visible='legendonly', **style),
1, 1)
for type, info in _SOLUTION_TYPE_MAP.items():
_plot_data(info.name, solution_type == type, marker_style=info.style)
# Set the 3D displacement Y-axis limits to start at 0 and encompass the data.
max_y = 1.2 * max_3d_diff_m[0]
if max_y == 0.0:
max_y = 1.0
time_figure['layout']['yaxis1'].update(range=[0, max_y])
name = source.replace(' ', '_').lower()
self._add_figure(name=f"{name}_top_down", figure=topo_figure, title=f"{source}: Top-Down (Topocentric)")
self._add_figure(name=f"{name}_vs_time", figure=time_figure, title=f"{source}: vs. Time")
def plot_stationary_position_error(self, truth_lla_deg):
"""!
@brief Plot position error vs. a known stationary location.
@param truth_lla_deg The truth LLA location (in degrees/meters).
"""
truth_ecef_m = np.array(geodetic2ecef(*truth_lla_deg, deg=True))
return self._plot_pose_displacement(title='Position Error', reference=truth_ecef_m)
def plot_pose_displacement(self):
"""!
@brief Generate a topocentric (top-down) plot of position displacement vs the median position, as well as plot
of displacement over time.
"""
return self._plot_pose_displacement(reference='median')
def _plot_pose_displacement(self, title='Pose Displacement', reference=None):
if self.output_dir is None:
return None
# Read the pose data.
result = self.reader.read(message_types=[PoseMessage], source_ids=self.default_source_id, **self.params)
pose_data = result[PoseMessage.MESSAGE_TYPE]
if len(pose_data.p1_time) == 0:
self.logger.info('No pose data available. Skipping displacement plots.')
return None
# Remove invalid solutions.
valid_idx = np.logical_and(~np.isnan(pose_data.p1_time), pose_data.solution_type != SolutionType.Invalid)
if not np.any(valid_idx):
self.logger.info('No valid position solutions detected. Skipping displacement plots.')
return None
time = pose_data.p1_time[valid_idx] - float(self.t0)
solution_type = pose_data.solution_type[valid_idx]
lla_deg = pose_data.lla_deg[:, valid_idx]
std_enu_m = pose_data.position_std_enu_m[:, valid_idx]
# Convert to ENU displacement with respect to the median position (we use median instead of centroid just in
# case there are one or two huge outliers).
position_ecef_m = np.array(geodetic2ecef(lat=lla_deg[0, :], lon=lla_deg[1, :], alt=lla_deg[2, :], deg=True))
if reference is None:
reference = 'median'
if isinstance(reference, str):
if reference == 'first':
reference_ecef_m = position_ecef_m[:, 0]
title_suffix = ' From First Position'
elif reference == 'median':
reference_ecef_m = np.median(position_ecef_m, axis=1)
title_suffix = ' From Median Position'
elif reference == 'median_fixed':
idx = solution_type == SolutionType.RTKFixed
if np.any(idx):
reference_ecef_m = np.median(position_ecef_m[:, idx], axis=1)
title_suffix = ' From Median Fixed Position'
else:
self.logger.warning('No fixed positions available. Using median as displacement plot reference.')
reference_ecef_m = np.median(position_ecef_m, axis=1)
title_suffix = ' From Median Position'
else:
raise ValueError('Unrecognized reference specifier.')
else:
reference_ecef_m = reference
title_suffix = ''