| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | /// @file MultiImuFile.hpp | ||
| 10 | /// @brief File reader for Multi-IMU data log files | ||
| 11 | /// @author M. Maier (marcel.maier@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2022-06-24 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include "internal/Node/Node.hpp" | ||
| 17 | #include "Nodes/DataProvider/Protocol/FileReader.hpp" | ||
| 18 | #include "NodeData/IMU/ImuPos.hpp" | ||
| 19 | #include "internal/gui/widgets/TimeEdit.hpp" | ||
| 20 | |||
| 21 | #include "NodeData/IMU/ImuObs.hpp" | ||
| 22 | |||
| 23 | namespace NAV | ||
| 24 | { | ||
| 25 | /// File reader for Multi-IMU data log files | ||
| 26 | class MultiImuFile : public Node, public FileReader | ||
| 27 | { | ||
| 28 | public: | ||
| 29 | /// @brief Default constructor | ||
| 30 | MultiImuFile(); | ||
| 31 | /// @brief Destructor | ||
| 32 | ~MultiImuFile() override; | ||
| 33 | /// @brief Copy constructor | ||
| 34 | MultiImuFile(const MultiImuFile&) = delete; | ||
| 35 | /// @brief Move constructor | ||
| 36 | MultiImuFile(MultiImuFile&&) = delete; | ||
| 37 | /// @brief Copy assignment operator | ||
| 38 | MultiImuFile& operator=(const MultiImuFile&) = delete; | ||
| 39 | /// @brief Move assignment operator | ||
| 40 | MultiImuFile& operator=(MultiImuFile&&) = delete; | ||
| 41 | |||
| 42 | /// @brief String representation of the Class Type | ||
| 43 | [[nodiscard]] static std::string typeStatic(); | ||
| 44 | |||
| 45 | /// @brief String representation of the Class Type | ||
| 46 | [[nodiscard]] std::string type() const override; | ||
| 47 | |||
| 48 | /// @brief String representation of the Class Category | ||
| 49 | [[nodiscard]] static std::string category(); | ||
| 50 | |||
| 51 | /// @brief ImGui config window which is shown on double click | ||
| 52 | /// @attention Don't forget to set _hasConfig to true in the constructor of the node | ||
| 53 | void guiConfig() override; | ||
| 54 | |||
| 55 | /// @brief Saves the node into a json object | ||
| 56 | [[nodiscard]] json save() const override; | ||
| 57 | |||
| 58 | /// @brief Restores the node from a json object | ||
| 59 | /// @param[in] j Json object with the node state | ||
| 60 | void restore(const json& j) override; | ||
| 61 | |||
| 62 | /// @brief Resets the node. Moves the read cursor to the start | ||
| 63 | bool resetNode() override; | ||
| 64 | |||
| 65 | /// Position and rotation information for conversion from platform to body frame | ||
| 66 | [[nodiscard]] const ImuPos& imuPosition() const { return _imuPos; } | ||
| 67 | |||
| 68 | protected: | ||
| 69 | /// Position and rotation information for conversion from platform to body frame | ||
| 70 | ImuPos _imuPos; | ||
| 71 | |||
| 72 | private: | ||
| 73 | constexpr static size_t OUTPUT_PORT_INDEX_IMU_OBS = 0; ///< @brief Flow (ImuObs) | ||
| 74 | |||
| 75 | /// @brief Initialize the node | ||
| 76 | bool initialize() override; | ||
| 77 | |||
| 78 | /// @brief Deinitialize the node | ||
| 79 | void deinitialize() override; | ||
| 80 | |||
| 81 | /// @brief Adds/Deletes Output Pins depending on the variable _nOutputPins | ||
| 82 | void updateNumberOfOutputPins(); | ||
| 83 | |||
| 84 | /// @brief Function to determine the File Type | ||
| 85 | /// @return The File path which was recognized | ||
| 86 | NAV::FileReader::FileType determineFileType() override; | ||
| 87 | |||
| 88 | /// @brief Function to read the Header of a file | ||
| 89 | void readHeader() override; | ||
| 90 | |||
| 91 | /// @brief Polls data from the file | ||
| 92 | /// @param[in] pinIdx Index of the pin the data is requested on | ||
| 93 | /// @param[in] peek Specifies if the data should be peeked (without moving the read cursor) or read | ||
| 94 | /// @return The read observation | ||
| 95 | [[nodiscard]] std::shared_ptr<const NodeData> pollData(size_t pinIdx, bool peek); | ||
| 96 | |||
| 97 | /// Number of connected sensors | ||
| 98 | size_t _nSensors = 5; | ||
| 99 | |||
| 100 | /// @brief GPS data rate [Hz] | ||
| 101 | double _gpsRate = 1; | ||
| 102 | |||
| 103 | /// @brief Read messages. | ||
| 104 | /// Vector idx: Sensor Id, | ||
| 105 | /// - Map Key: InsTime | ||
| 106 | /// - Map Value: IMU Observation | ||
| 107 | std::vector<std::map<InsTime, std::shared_ptr<ImuObs>>> _messages; | ||
| 108 | |||
| 109 | /// @brief Counter for lines | ||
| 110 | size_t _lineCounter{}; | ||
| 111 | |||
| 112 | /// @brief Counter for messages | ||
| 113 | std::vector<size_t> _messageCnt; | ||
| 114 | |||
| 115 | /// @brief Delimiter: ',' for GPZDA and ' ' for GPGGA messages (NMEA) | ||
| 116 | char _delim = ','; | ||
| 117 | |||
| 118 | /// @brief First 'gpsSecond', s.t. measurements start at time = 0 | ||
| 119 | double _startupGpsSecond{}; | ||
| 120 | |||
| 121 | /// Time Format to input the init time with | ||
| 122 | gui::widgets::TimeEditFormat _startTimeEditFormat; | ||
| 123 | |||
| 124 | /// @brief Absolute start time | ||
| 125 | InsTime _startTime{ 2000, 1, 1, 0, 0, 0 }; | ||
| 126 | |||
| 127 | /// @brief Container of column names | ||
| 128 | std::vector<std::string> _columns{ "sensorId", "gpsSecond", "timeNumerator", "timeDenominator", "accelX", "accelY", "accelZ", "gyroX", "gyroY", "gyroZ" }; | ||
| 129 | |||
| 130 | /// @brief Container of header column names | ||
| 131 | std::vector<std::string> _headerColumns{ "nmeaMsgType", "UTC_HMS", "day", "month", "year" }; | ||
| 132 | |||
| 133 | /// @brief Container for individual sensor orientations of a Multi-IMU | ||
| 134 | std::vector<ImuPos> _imuPosAll; | ||
| 135 | |||
| 136 | /// @brief Previous observation (for timestamp) | ||
| 137 | InsTime _lastFiltObs; | ||
| 138 | |||
| 139 | /// @brief Flag that indicates whether a 'GPZDA' message was found in the Multi-IMU-Logs header | ||
| 140 | bool _gpzdaFound = false; | ||
| 141 | /// @brief Flag that indicates whether a 'GPGGA' message was found in the Multi-IMU-Logs header | ||
| 142 | bool _gpggaFound = false; | ||
| 143 | |||
| 144 | /// Types of NMEA messages available | ||
| 145 | enum class NmeaType : uint8_t | ||
| 146 | { | ||
| 147 | GPGGA, ///< NMEA message type | ||
| 148 | GPZDA, ///< NMEA message type | ||
| 149 | COUNT, ///< Number of items in the enum | ||
| 150 | }; | ||
| 151 | /// @brief Converts the enum to a string | ||
| 152 | /// @param[in] value Enum value to convert into text | ||
| 153 | /// @return String representation of the enum | ||
| 154 | friend constexpr const char* to_string(NmeaType value); | ||
| 155 | |||
| 156 | /// Selected NMEA type in the GUI | ||
| 157 | NmeaType _nmeaType = NmeaType::GPZDA; | ||
| 158 | }; | ||
| 159 | |||
| 160 | /// @brief Converts the enum to a string | ||
| 161 | /// @param[in] value Enum value to convert into text | ||
| 162 | /// @return String representation of the enum | ||
| 163 | ✗ | constexpr const char* to_string(NAV::MultiImuFile::NmeaType value) | |
| 164 | { | ||
| 165 | ✗ | switch (value) | |
| 166 | { | ||
| 167 | ✗ | case NAV::MultiImuFile::NmeaType::GPGGA: | |
| 168 | ✗ | return "GPGGA"; | |
| 169 | ✗ | case NAV::MultiImuFile::NmeaType::GPZDA: | |
| 170 | ✗ | return "GPZDA"; | |
| 171 | ✗ | case NAV::MultiImuFile::NmeaType::COUNT: | |
| 172 | ✗ | return ""; | |
| 173 | } | ||
| 174 | ✗ | return ""; | |
| 175 | } | ||
| 176 | |||
| 177 | } // namespace NAV | ||
| 178 |