diff --git a/python/examples/extract_wheel_speed_data.py b/python/examples/extract_vehicle_speed_data.py similarity index 69% rename from python/examples/extract_wheel_speed_data.py rename to python/examples/extract_vehicle_speed_data.py index c13a77e9..1732e24c 100755 --- a/python/examples/extract_wheel_speed_data.py +++ b/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)) diff --git a/python/fusion_engine_client/analysis/data_loader.py b/python/fusion_engine_client/analysis/data_loader.py index c9b116e0..23d3c25d 100644 --- a/python/fusion_engine_client/analysis/data_loader.py +++ b/python/fusion_engine_client/analysis/data_loader.py @@ -1,10 +1,8 @@ -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 @@ -12,7 +10,7 @@ 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 @@ -20,6 +18,11 @@ 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,7 +652,7 @@ 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) ->\ @@ -657,7 +660,7 @@ def _convert_time(self, gps_to_p1, """! @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,