Update from PR comments.

python/examples/extract_wheel_speed_data.py → python/examples/extract_vehicle_speed_data.py

@@ -49,12 +49,21 @@
 
     # Read satellite data from the file.
     reader = DataLoader(input_path)
-    result = reader.read(message_types=[WheelSpeedOutput, RawWheelSpeedOutput], show_progress=True)
+    result = reader.read(
+        message_types=[
+            WheelSpeedOutput,
+            RawWheelSpeedOutput,
+            VehicleSpeedOutput,
+            RawVehicleSpeedOutput],
+        show_progress=True)
+    if all(len(d.messages) == 0 for d in result.values()):
+        logger.warning('No speed data found in log file.')
+        sys.exit(2)
+
     wheel_speed_data = result[WheelSpeedOutput.MESSAGE_TYPE]
     raw_wheel_speed_data = result[RawWheelSpeedOutput.MESSAGE_TYPE]
-    if len(wheel_speed_data.messages) == 0 and len(raw_wheel_speed_data.messages) == 0:
-        logger.warning('No wheel speed data found in log file.')
-        sys.exit(2)
+    vehicle_speed_data = result[WheelSpeedOutput.MESSAGE_TYPE]
+    raw_vehicle_speed_data = result[RawWheelSpeedOutput.MESSAGE_TYPE]
 
     # Generate a CSV file for corrected wheel speed data.
     if len(wheel_speed_data.messages) != 0:
@@ -96,4 +105,38 @@
     else:
         logger.info("No raw wheel speed data.")
 
+    # Generate a CSV file for corrected vehicle speed data.
+    if len(vehicle_speed_data.messages) != 0:
+        path = os.path.join(output_dir, 'vehicle_speed_data.csv')
+        logger.info("Generating '%s'." % path)
+        with open(path, 'w') as f:
+            f.write('P1 Time (sec), GPS Time (sec), Vehicle Speed (m/s), Gear\n')
+            for message in vehicle_speed_data.messages:
+                gps_time = reader.convert_to_gps_time(message.p1_time)
+                f.write(
+                    '%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %d\n' %
+                    (float(message.p1_time),
+                     float(gps_time),
+                     message.vehicle_speed_mps,
+                     message.gear))
+    else:
+        logger.info("No corrected vehicle speed data.")
+
+    # Generate a CSV file for raw vehicle speed data.
+    if len(raw_vehicle_speed_data.messages) != 0:
+        path = os.path.join(output_dir, 'raw_vehicle_speed_data.csv')
+        logger.info("Generating '%s'." % path)
+        with open(path, 'w') as f:
+            f.write('P1 Time (sec), GPS Time (sec), Vehicle Speed (m/s), Gear\n')
+            for message in vehicle_speed_data.messages:
+                gps_time = reader.convert_to_gps_time(message.p1_time)
+                f.write(
+                    '%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %d\n' %
+                    (float(message.p1_time),
+                     float(gps_time),
+                     message.vehicle_speed_mps,
+                     message.gear))
+    else:
+        logger.info("No raw vehicle speed data.")
+
     logger.info("Output stored in '%s'." % os.path.abspath(output_dir))

python/fusion_engine_client/analysis/data_loader.py

@@ -1,25 +1,28 @@
-from typing import Dict, Iterable, Tuple, Union
+from enum import Enum, auto
+from typing import Dict, Iterable, Union
 
 from collections import deque
-import copy
 from datetime import datetime, timezone
-import io
-import os
 
 from gpstime import gpstime, unix2gps
 import numpy as np
 import scipy as sp
 
 from ..messages import *
 from ..messages.timestamp import is_gps_time
-from ..parsers.file_index import FileIndex, FileIndexBuilder
+from ..parsers.file_index import FileIndex
 from ..parsers.mixed_log_reader import MixedLogReader
 from ..utils import trace as logging
 from ..utils.trace import SilentLogger
 from ..utils.enum_utils import IntEnum
 from ..utils.time_range import TimeRange
 
 
+class TimeConversionType:
+    P1_TO_GPS = auto()
+    GPS_TO_P1 = auto()
+
+
 class MessageData(object):
     def __init__(self, message_type, params):
         self.message_type = message_type
@@ -649,15 +652,15 @@ def get_log_reader(self) -> MixedLogReader:
     def get_input_path(self):
         return self.reader.input_file.name
 
-    def _convert_time(self, gps_to_p1,
+    def _convert_time(self, conversion_type: TimeConversionType,
                       times: Union[Iterable[Union[datetime, gpstime, Timestamp, float]],
                                    Union[datetime, gpstime, Timestamp, float]],
                       assume_utc: bool = False) ->\
             np.ndarray:
         """!
         @brief Convert UTC or GPS timestamps to P1 time or Convert UTC or P1 timestamps to GPS time.
 
-        @param gps_to_p1 If `True`, convert to P1 time. If `False`, convert to GPS time.
+        @param conversion_type If `GPS_TO_P1`, convert to P1 time. If `P1_TO_GPS`, convert to GPS time.
         @param times A list of one or more timestamps to be converted, using any of the following formats:
                - `datetime` - A UTC or local timezone date and time
                - `gpstime` - A GPS timestamp
@@ -742,7 +745,7 @@ def _to_gps_or_p1(value):
 
         # Now, find all values that are GPS time (i.e., big enough that we assume they're not P1 times already) and
         # convert them to P1 time or vice versa.
-        if gps_to_p1:
+        if conversion_type == TimeConversionType.GPS_TO_P1:
             gps_idx = is_gps_time(time_sec)
             if np.any(gps_idx):
                 if p1_ref_sec is None:
@@ -754,8 +757,8 @@ def _to_gps_or_p1(value):
                     # instead use SciPy's function.
                     f = sp.interpolate.interp1d(gps_ref_sec, p1_ref_sec, fill_value='extrapolate')
                     time_sec[gps_idx] = f(time_sec[gps_idx])
-        else:
-            p1_idx = not is_gps_time(time_sec)
+        elif conversion_type == TimeConversionType.P1_TO_GPS:
+            p1_idx = np.logical_not(is_gps_time(time_sec))
             if np.any(p1_idx):
                 if p1_idx is None:
                     time_sec[p1_idx] = np.nan
@@ -792,7 +795,7 @@ def convert_to_p1_time(self,
 
         @return A numpy array containing P1 time values (in seconds), or `nan` if the value could not be converted.
         """
-        return self._convert_time(True, times, assume_utc)
+        return self._convert_time(conversion_type=TimeConversionType.GPS_TO_P1, times=times, assume_utc=assume_utc)
 
     def convert_to_gps_time(self,
                             times: Union[Iterable[Union[datetime, gpstime, Timestamp, float]],
@@ -817,7 +820,7 @@ def convert_to_gps_time(self,
 
         @return A numpy array containing GPS time values (in seconds), or `nan` if the value could not be converted.
         """
-        return self._convert_time(False, times, assume_utc)
+        return self._convert_time(conversion_type=TimeConversionType.P1_TO_GPS, times=times, assume_utc=assume_utc)
 
     @classmethod
     def time_align_data(cls, data: dict, mode: TimeAlignmentMode = TimeAlignmentMode.INSERT,