INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProvider/IMU/FileReader/MultiImuFile.cpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 179 249 71.9%
Functions: 17 20 85.0%
Branches: 202 530 38.1%
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 #include "MultiImuFile.hpp"
10
11 #include "util/Logger.hpp"
12
13 #include "Navigation/Transformations/CoordinateFrames.hpp"
14 #include "Navigation/Transformations/Units.hpp"
15
16 #include "internal/NodeManager.hpp"
17 namespace nm = NAV::NodeManager;
18 #include "internal/FlowManager.hpp"
19 #include "internal/gui/widgets/EnumCombo.hpp"
20 #include "internal/gui/widgets/HelpMarker.hpp"
21 #include "internal/gui/NodeEditorApplication.hpp"
22
23 114 NAV::MultiImuFile::MultiImuFile()
24
7/14
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 114 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 114 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 114 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 114 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 114 times.
✗ Branch 25 not taken.
 570 : Node(typeStatic())
25 {
26 LOG_TRACE("{}: called", name);
27
28 114 _hasConfig = true;
29 114 _guiConfigDefaultWindowSize = { 528, 379 };
30
31
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 114 updateNumberOfOutputPins();
32 114 }
33
34 232 NAV::MultiImuFile::~MultiImuFile()
35 {
36 LOG_TRACE("{}: called", nameId());
37 232 }
38
39 226 std::string NAV::MultiImuFile::typeStatic()
40 {
41
1/2
✓ Branch 1 taken 226 times.
✗ Branch 2 not taken.
 452 return "MultiImuFile";
42 }
43
44 ✗ std::string NAV::MultiImuFile::type() const
45 {
46 ✗ return typeStatic();
47 }
48
49 112 std::string NAV::MultiImuFile::category()
50 {
51
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 return "Data Provider";
52 }
53
54 ✗ void NAV::MultiImuFile::guiConfig()
55 {
56 ✗ float columnWidth = 130.0F * gui::NodeEditorApplication::windowFontRatio();
57
58 ✗ if (FileReader::guiConfig(".txt", { ".txt" }, size_t(id), nameId()))
59 {
60 ✗ flow::ApplyChanges();
61 ✗ doDeinitialize();
62 }
63
64 ✗ ImGui::SetNextItemWidth(columnWidth);
65
66 ✗ if (gui::widgets::EnumCombo(fmt::format("NMEA message type##{}", size_t(id)).c_str(), _nmeaType))
67 {
68 ✗ LOG_DEBUG("{}: nmeaType changed to {}", nameId(), fmt::underlying(_nmeaType));
69
70 ✗ flow::ApplyChanges();
71 ✗ doDeinitialize();
72 }
73 ✗ ImGui::SameLine();
74 ✗ gui::widgets::HelpMarker("Until June 2023, NMEA messages in the Multi-IMU file's header were of the 'GPGGA' type. Since this type does not provide an absolute time reference, it was changed to 'GPZDA'.\n\n");
75
76 ✗ if (_nmeaType == NmeaType::GPGGA)
77 {
78 ✗ if (gui::widgets::TimeEdit(fmt::format("{}", size_t(id)).c_str(), _startTime, _startTimeEditFormat))
79 {
80 ✗ LOG_DEBUG("{}: startTime changed to {}", nameId(), _startTime);
81 ✗ flow::ApplyChanges();
82 }
83 }
84
85 ✗ ImGui::Separator();
86 // Set Imu Position and Rotation (from 'Imu::guiConfig();')
87 ✗ bool showRotation = true;
88 ✗ for (size_t i = 0; i < _nSensors; ++i)
89 {
90 ✗ ImGui::SetNextItemOpen(showRotation, ImGuiCond_FirstUseEver);
91 ✗ if (i == 0) { showRotation = false; }
92 ✗ if (ImGui::TreeNode(fmt::format("Imu #{} Position & Rotation##{}", i + 1, size_t(id)).c_str()))
93 {
94 ✗ ImGui::BeginDisabled(); // FIXME Not properly simulated and accounted for in the algorithms
95 ✗ std::array<float, 3> imuPos = { static_cast<float>(_imuPosAll[i].b_positionIMU_p().x()), static_cast<float>(_imuPosAll[i].b_positionIMU_p().y()), static_cast<float>(_imuPosAll[i].b_positionIMU_p().z()) };
96 ✗ if (ImGui::InputFloat3(fmt::format("Position [m]##{}", size_t(id)).c_str(), imuPos.data()))
97 {
98 ✗ flow::ApplyChanges();
99 ✗ _imuPosAll[i]._b_positionIMU_p = Eigen::Vector3d(imuPos.at(0), imuPos.at(1), imuPos.at(2));
100 }
101 ✗ ImGui::EndDisabled();
102 ✗ ImGui::SameLine();
103 ✗ gui::widgets::HelpMarker("Position of the IMU sensor relative to the vehicle center of mass in the body coordinate frame.");
104
105 ✗ Eigen::Vector3d eulerAnglesIMU = rad2deg(trafo::quat2eulerZYX(_imuPosAll[i].p_quat_b()));
106 ✗ std::array<float, 3> imuRot = { static_cast<float>(eulerAnglesIMU.x()), static_cast<float>(eulerAnglesIMU.y()), static_cast<float>(eulerAnglesIMU.z()) };
107 ✗ if (ImGui::InputFloat3(fmt::format("Rotation [deg]##{}", size_t(id)).c_str(), imuRot.data()))
108 {
109 // (-180:180] x (-90:90] x (-180:180]
110 ✗ imuRot.at(0) = std::max(imuRot.at(0), -179.9999F);
111 ✗ imuRot.at(0) = std::min(imuRot.at(0), 180.0F);
112 ✗ imuRot.at(1) = std::max(imuRot.at(1), -89.9999F);
113 ✗ imuRot.at(1) = std::min(imuRot.at(1), 90.0F);
114 ✗ imuRot.at(2) = std::max(imuRot.at(2), -179.9999F);
115 ✗ imuRot.at(2) = std::min(imuRot.at(2), 180.0F);
116
117 ✗ flow::ApplyChanges();
118 ✗ _imuPosAll[i]._b_quat_p = trafo::b_Quat_p(deg2rad(imuRot.at(0)), deg2rad(imuRot.at(1)), deg2rad(imuRot.at(2)));
119 }
120
121 ✗ ImGui::TreePop();
122 }
123 }
124 ✗ }
125
126 ✗ [[nodiscard]] json NAV::MultiImuFile::save() const
127 {
128 LOG_TRACE("{}: called", nameId());
129
130 ✗ json j;
131
132 ✗ j["FileReader"] = FileReader::save();
133 ✗ j["imuPos"] = _imuPosAll;
134 ✗ j["nmeaType"] = _nmeaType;
135 ✗ j["startTime"] = _startTime;
136 ✗ j["delim"] = _delim;
137
138 ✗ return j;
139 ✗ }
140
141 2 void NAV::MultiImuFile::restore(json const& j)
142 {
143 LOG_TRACE("{}: called", nameId());
144
145
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("FileReader"))
146 {
147 2 FileReader::restore(j.at("FileReader"));
148 }
149
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("imuPos"))
150 {
151 2 j.at("imuPos").get_to(_imuPosAll);
152 }
153
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("nmeaType"))
154 {
155 2 j.at("nmeaType").get_to(_nmeaType);
156 }
157
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("startTime"))
158 {
159 2 j.at("startTime").get_to(_startTime);
160 }
161
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("delim"))
162 {
163 2 j.at("delim").get_to(_delim);
164 }
165 2 }
166
167 2 bool NAV::MultiImuFile::initialize()
168 {
169 LOG_TRACE("{}: called", nameId());
170
171 2 return FileReader::initialize();
172 }
173
174 2 void NAV::MultiImuFile::deinitialize()
175 {
176 LOG_TRACE("{}: called", nameId());
177
178 2 FileReader::deinitialize();
179 2 }
180
181 4 bool NAV::MultiImuFile::resetNode()
182 {
183 4 _lastFiltObs.reset();
184 4 _lineCounter = 0;
185
186 4 FileReader::resetReader();
187
188
2/2
✓ Branch 5 taken 20 times.
✓ Branch 6 taken 4 times.
 24 for (auto& sensor : _messages)
189 {
190 20 sensor.clear();
191 }
192
2/2
✓ Branch 4 taken 20 times.
✓ Branch 5 taken 4 times.
 24 for (auto& cnt : _messageCnt)
193 {
194 20 cnt = 0;
195 }
196
197 4 return true;
198 }
199
200 114 void NAV::MultiImuFile::updateNumberOfOutputPins()
201 {
202
2/2
✓ Branch 1 taken 570 times.
✓ Branch 2 taken 114 times.
 684 while (outputPins.size() < _nSensors)
203 {
204
4/8
✓ Branch 1 taken 570 times.
✗ Branch 2 not taken.
✓ Branch 8 taken 570 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 570 times.
✓ Branch 13 taken 570 times.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
 2280 nm::CreateOutputPin(this, fmt::format("ImuObs {}", outputPins.size() + 1).c_str(), Pin::Type::Flow, { NAV::ImuObs::type() }, &MultiImuFile::pollData);
205 570 _imuPosAll.resize(_nSensors);
206
207 570 _messages.resize(_nSensors);
208 570 _messageCnt.resize(_nSensors);
209 }
210 684 }
211
212 2 NAV::FileReader::FileType NAV::MultiImuFile::determineFileType()
213 {
214 LOG_TRACE("called");
215
216
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (good())
217 {
218 2 return FileType::ASCII;
219 }
220
221 ✗ LOG_ERROR("Could not open file {}", getFilepath().string());
222 ✗ return FileType::NONE;
223 }
224
225 2 void NAV::MultiImuFile::readHeader()
226 {
227 LOG_TRACE("called");
228
229 2 _gpzdaFound = false;
230 2 _gpggaFound = false;
231 2 const char* gpzda = "GPZDA";
232 2 const char* gpgga = "GPGGA";
233
234 2 std::string line;
235
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 auto len = tellg();
236
237 // Find first line of data
238
3/6
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 34 times.
✗ Branch 7 not taken.
 34 while (getline(line))
239 {
240 34 _lineCounter++;
241
242 // Remove any trailing non text characters
243
2/4
✓ Branch 3 taken 34 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 34 times.
✗ Branch 8 not taken.
 653 line.erase(std::ranges::find_if(line, [](int ch) { return std::iscntrl(ch); }), line.end());
244
245
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 30 times.
 34 if ((line.find(gpzda)) != std::string::npos)
246 {
247 4 _gpzdaFound = true;
248
249 4 int32_t year{};
250 4 int32_t month{};
251 4 int32_t day{};
252 4 int32_t hour{};
253 4 int32_t min{};
254 4 long double sec{};
255
256 // Convert line into stream
257
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 std::stringstream lineStream(line);
258 4 std::string cell;
259
260 // Split line at comma
261
2/2
✓ Branch 5 taken 20 times.
✓ Branch 6 taken 4 times.
 24 for (const auto& col : _headerColumns)
262 {
263
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
 20 if (std::getline(lineStream, cell, ','))
264 {
265 // Remove any trailing non text characters
266
2/4
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 108 cell.erase(std::ranges::find_if(cell, [](int ch) { return std::iscntrl(ch); }), cell.end());
267
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
 20 while (cell.empty())
268 {
269 ✗ std::getline(lineStream, cell, ',');
270 }
271
272
3/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 16 times.
 20 if (col == "nmeaMsgType")
273 {
274
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 LOG_DEBUG("{}: nmeaMsgType read.", nameId());
275 4 continue;
276 }
277
3/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 12 times.
 16 if (col == "UTC_HMS")
278 {
279
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 hour = std::stoi(cell.substr(0, 2));
280
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 min = std::stoi(cell.substr(2, 2));
281
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 sec = std::stold(cell.substr(4, 5));
282 4 continue;
283 }
284
3/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 8 times.
 12 if (col == "day")
285 {
286
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 day = std::stoi(cell);
287 4 continue;
288 }
289
3/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 4 times.
 8 if (col == "month")
290 {
291
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 month = std::stoi(cell);
292 4 continue;
293 }
294
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 if (col == "year")
295 {
296
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 year = std::stoi(cell);
297 }
298
299
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 _startTime = InsTime(year, month, day, hour, min, sec + 1. / _gpsRate, UTC); // GPS has a rate of 1 Hz, actual starttime is one message after the last GPZDA msg in the header
300
301
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 len = tellg();
302 4 continue;
303 4 }
304 }
305 4 }
306
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 32 times.
 34 if (line.find(gpgga) != std::string::npos)
307 {
308 2 _gpggaFound = true;
309 }
310
9/10
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✓ Branch 9 taken 20 times.
✓ Branch 10 taken 11 times.
✓ Branch 11 taken 1 times.
✓ Branch 12 taken 1 times.
✓ Branch 13 taken 10 times.
✓ Branch 14 taken 2 times.
✓ Branch 15 taken 30 times.
 64 else if ((std::find_if(line.begin(), line.begin() + 1, [](int ch) { return std::isdigit(ch); }) != (std::begin(line) + 1)) && (_gpggaFound || _gpzdaFound))
311 {
312
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
 2 LOG_DEBUG("{}: Found first line of data: {}", nameId(), line);
313
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 seekg(len, std::ios_base::beg); // Reset the read cursor, otherwise we skip the first message
314 2 break;
315 }
316
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 len = tellg();
317 }
318
4/6
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
 2 if (_gpggaFound && _nmeaType == NmeaType::GPZDA && !_gpzdaFound)
319 {
320 ✗ LOG_WARN("{}: NMEA message type was set to 'GPZDA', but the file contains only 'GPGGA'. Make sure that the absolute time reference is set correctly.", nameId());
321 ✗ _nmeaType = NmeaType::GPGGA;
322 }
323
2/4
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
 2 if (_gpzdaFound && _nmeaType == NmeaType::GPGGA)
324 {
325 ✗ LOG_WARN("{}: NMEA message type was set to 'GPGGA', but the file contains 'GPZDA'. The absolute time reference was read from the header, not the GUI input.", nameId());
326 ✗ _nmeaType = NmeaType::GPZDA;
327 }
328
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 _delim = _gpzdaFound ? ',' : ' ';
329 2 }
330
331 15174 std::shared_ptr<const NAV::NodeData> NAV::MultiImuFile::pollData(size_t pinIdx, bool peek)
332 {
333 15174 std::shared_ptr<ImuObs> obs = nullptr;
334
335
3/4
✓ Branch 1 taken 15174 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8515 times.
✓ Branch 5 taken 6659 times.
 15174 if (!_messages.at(pinIdx).empty()) // Another pin was reading a message for this pin
336 {
337
1/2
✓ Branch 1 taken 8515 times.
✗ Branch 2 not taken.
 8515 obs = _messages.at(pinIdx).begin()->second;
338
2/2
✓ Branch 0 taken 7582 times.
✓ Branch 1 taken 933 times.
 8515 if (!peek) // When peeking, we leave the message in the buffer, so we can remove it when polling
339 {
340
3/6
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7582 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 7582 times.
✗ Branch 9 not taken.
 7582 _messages.at(pinIdx).erase(_messages.at(pinIdx).begin());
341 }
342 }
343 else
344 {
345 // Read line
346 6659 std::string line;
347
4/6
✓ Branch 1 taken 7592 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7592 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7582 times.
✓ Branch 7 taken 10 times.
 7592 while (getline(line))
348 {
349 7582 _lineCounter++;
350
351 // Remove any starting non text characters
352
2/4
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 7582 times.
✗ Branch 8 not taken.
 15164 line.erase(line.begin(), std::ranges::find_if(line, [](int ch) { return std::isgraph(ch); }));
353
354
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7582 times.
 7582 if (line.empty())
355 {
356 933 continue;
357 }
358
359 // Convert line into stream
360
1/2
✓ Branch 2 taken 7582 times.
✗ Branch 3 not taken.
 7582 std::stringstream lineStream(line);
361 7582 std::string cell;
362
363 7582 size_t sensorId{};
364 7582 double gpsSecond{};
365 7582 double timeNumerator{};
366 7582 double timeDenominator{};
367 7582 std::optional<double> accelX;
368 7582 std::optional<double> accelY;
369 7582 std::optional<double> accelZ;
370 7582 std::optional<double> gyroX;
371 7582 std::optional<double> gyroY;
372 7582 std::optional<double> gyroZ;
373
374 // Split line at comma
375
2/2
✓ Branch 5 taken 75820 times.
✓ Branch 6 taken 7582 times.
 83402 for (const auto& col : _columns)
376 {
377
3/6
✓ Branch 1 taken 75820 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 75820 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 75820 times.
✗ Branch 7 not taken.
 75820 if (std::getline(lineStream, cell, _delim))
378 {
379 // Remove any trailing non text characters
380
2/4
✓ Branch 3 taken 75820 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 75820 times.
✗ Branch 8 not taken.
 378417 cell.erase(std::ranges::find_if(cell, [](int ch) { return std::iscntrl(ch); }), cell.end());
381
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 75820 times.
 75820 while (cell.empty())
382 {
383 ✗ std::getline(lineStream, cell, ' ');
384 }
385
386
3/4
✓ Branch 1 taken 75820 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 68238 times.
 75820 if (col == "sensorId")
387 {
388
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 sensorId = std::stoul(cell); // NOLINT(clang-diagnostic-implicit-int-conversion)
389 }
390
3/4
✓ Branch 1 taken 68238 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 60656 times.
 68238 else if (col == "gpsSecond")
391 {
392
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 gpsSecond = std::stod(cell); // [s]
393
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 7580 times.
 7582 if (_startupGpsSecond == 0)
394 {
395 2 _startupGpsSecond = gpsSecond;
396 }
397 }
398
3/4
✓ Branch 1 taken 60656 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 53074 times.
 60656 else if (col == "timeNumerator")
399 {
400
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 timeNumerator = std::stod(cell);
401 }
402
3/4
✓ Branch 1 taken 53074 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 45492 times.
 53074 else if (col == "timeDenominator")
403 {
404
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 timeDenominator = std::stod(cell);
405 }
406
3/4
✓ Branch 1 taken 45492 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 37910 times.
 45492 else if (col == "accelX")
407 {
408
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 accelX = 0.001 * std::stod(cell); // [m/s²]
409 }
410
3/4
✓ Branch 1 taken 37910 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 30328 times.
 37910 else if (col == "accelY")
411 {
412
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 accelY = 0.001 * std::stod(cell); // [m/s²]
413 }
414
3/4
✓ Branch 1 taken 30328 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 22746 times.
 30328 else if (col == "accelZ")
415 {
416
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 accelZ = 0.001 * std::stod(cell); // [m/s²]
417 }
418
3/4
✓ Branch 1 taken 22746 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 15164 times.
 22746 else if (col == "gyroX")
419 {
420
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 gyroX = deg2rad(std::stod(cell) / 131); // [deg/s]
421 }
422
3/4
✓ Branch 1 taken 15164 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 7582 times.
 15164 else if (col == "gyroY")
423 {
424
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 gyroY = deg2rad(std::stod(cell)) / 131; // [deg/s]
425 }
426
2/4
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
✗ Branch 4 not taken.
 7582 else if (col == "gyroZ")
427 {
428
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 gyroZ = deg2rad(std::stod(cell)) / 131; // [deg/s]
429 }
430 }
431 }
432
433 7582 auto timeStamp = gpsSecond + timeNumerator / timeDenominator - _startupGpsSecond;
434
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7582 times.
 7582 if (!peek)
435 {
436 ✗ LOG_DEBUG("line: {}", line);
437 ✗ LOG_DEBUG("timeStamp: {}", timeStamp);
438 }
439
440
1/2
✓ Branch 2 taken 7582 times.
✗ Branch 3 not taken.
 7582 obs = std::make_shared<ImuObs>(_imuPosAll[sensorId - 1]);
441
442
2/4
✓ Branch 2 taken 7582 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 7582 times.
✗ Branch 6 not taken.
 7582 obs->insTime = _startTime + std::chrono::duration<double>(timeStamp);
443
444
4/8
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7582 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 7582 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 7582 times.
 7582 if (!accelX || !accelY || !accelZ)
445 {
446 ✗ LOG_ERROR("{}: Fields 'accelX', 'accelY', 'accelZ' are needed.", nameId());
447 ✗ return nullptr;
448 }
449
4/8
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7582 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 7582 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 7582 times.
 7582 if (!gyroX || !gyroY || !gyroZ)
450 {
451 ✗ LOG_ERROR("{}: Fields 'gyroX', 'gyroY', 'gyroZ' are needed.", nameId());
452 ✗ return nullptr;
453 }
454
455
4/8
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7582 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7582 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7582 times.
✗ Branch 11 not taken.
 7582 obs->p_acceleration = { accelX.value(), accelY.value(), accelZ.value() };
456
4/8
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7582 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7582 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7582 times.
✗ Branch 11 not taken.
 7582 obs->p_angularRate = { gyroX.value(), gyroY.value(), gyroZ.value() };
457
458
2/2
✓ Branch 0 taken 933 times.
✓ Branch 1 taken 6649 times.
 7582 if (sensorId - 1 != pinIdx)
459 {
460 // Write message into buffer to process later on correct pin
461
2/4
✓ Branch 1 taken 933 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 933 times.
✗ Branch 7 not taken.
 933 _messages.at(sensorId - 1).insert(std::make_pair(obs->insTime, obs));
462
463 933 continue; // Read next line in file and search for correct sensor
464 }
465
1/2
✓ Branch 0 taken 6649 times.
✗ Branch 1 not taken.
 6649 if (peek)
466 {
467 // Write message into buffer to process later in poll step
468
2/4
✓ Branch 1 taken 6649 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 6649 times.
✗ Branch 7 not taken.
 6649 _messages.at(pinIdx).insert(std::make_pair(obs->insTime, obs));
469 }
470 6649 break;
471 15164 }
472 6659 }
473
474 // Calls all the callbacks
475
6/6
✓ Branch 1 taken 15164 times.
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 7582 times.
✓ Branch 4 taken 7582 times.
✓ Branch 5 taken 7582 times.
✓ Branch 6 taken 7592 times.
 15174 if (obs && !peek)
476 {
477
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 _messageCnt.at(pinIdx)++;
478
479 // Detect jumps back in time
480
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
 7582 if (obs->insTime < _lastFiltObs)
481 {
482 ✗ LOG_ERROR("{}: Jumped back in time on line {} (at {}), by {} s", nameId(), _lineCounter, obs->insTime.toYMDHMS(), static_cast<double>((obs->insTime - _lastFiltObs).count()));
483 ✗ return obs;
484 }
485 7582 _lastFiltObs = obs->insTime;
486
487
1/2
✓ Branch 2 taken 7582 times.
✗ Branch 3 not taken.
 7582 invokeCallbacks(pinIdx, obs);
488 }
489
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 15164 times.
 15174 if (obs == nullptr)
490 {
491
4/8
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 10 times.
✗ Branch 12 not taken.
 10 LOG_DEBUG("{}: Finished reading on pinIdx {}. Read a total of {} messages.", nameId(), pinIdx, _messageCnt.at(pinIdx));
492 }
493 15174 return obs;
494 15174 }
495