INSTINCT Code Coverage Report

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