INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProvider/IMU/FileReader/MultiImuFile.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 179 281 63.7%
Functions: 17 20 85.0%
Branches: 202 682 29.6%
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 ✗ std::array<float, 3> imuPosAccel = { static_cast<float>(_imuPosAll[i].b_positionAccel().x()), static_cast<float>(_imuPosAll[i].b_positionAccel().y()), static_cast<float>(_imuPosAll[i].b_positionAccel().z()) };
95 ✗ if (ImGui::InputFloat3(fmt::format("Lever Accel [m]##{}", size_t(id)).c_str(), imuPosAccel.data()))
96 {
97 ✗ flow::ApplyChanges();
98 ✗ _imuPosAll[i]._b_positionAccel = Eigen::Vector3d(imuPosAccel.at(0), imuPosAccel.at(1), imuPosAccel.at(2));
99 }
100 ✗ ImGui::SameLine();
101 ✗ gui::widgets::HelpMarker("Position of the accelerometer sensor relative to the vehicle center of mass in the body coordinate frame.");
102
103 ✗ std::array<float, 3> imuPosGyro = { static_cast<float>(_imuPosAll[i].b_positionGyro().x()), static_cast<float>(_imuPosAll[i].b_positionGyro().y()), static_cast<float>(_imuPosAll[i].b_positionGyro().z()) };
104 ✗ if (ImGui::InputFloat3(fmt::format("Lever Gyro [m]##{}", size_t(id)).c_str(), imuPosGyro.data()))
105 {
106 ✗ flow::ApplyChanges();
107 ✗ _imuPosAll[i]._b_positionGyro = Eigen::Vector3d(imuPosGyro.at(0), imuPosGyro.at(1), imuPosGyro.at(2));
108 }
109 ✗ ImGui::SameLine();
110 ✗ gui::widgets::HelpMarker("Position of the gyroscope sensor relative to the vehicle center of mass in the body coordinate frame.");
111
112 ✗ std::array<float, 3> imuPosMag = { static_cast<float>(_imuPosAll[i].b_positionMag().x()), static_cast<float>(_imuPosAll[i].b_positionMag().y()), static_cast<float>(_imuPosAll[i].b_positionMag().z()) };
113 ✗ if (ImGui::InputFloat3(fmt::format("Lever Mag [m]##{}", size_t(id)).c_str(), imuPosMag.data()))
114 {
115 ✗ flow::ApplyChanges();
116 ✗ _imuPosAll[i]._b_positionMag = Eigen::Vector3d(imuPosMag.at(0), imuPosMag.at(1), imuPosMag.at(2));
117 }
118 ✗ ImGui::SameLine();
119 ✗ gui::widgets::HelpMarker("Position of the magnetometer sensor relative to the vehicle center of mass in the body coordinate frame.");
120
121 ✗ Eigen::Vector3d eulerAccel = rad2deg(trafo::quat2eulerZYX(_imuPosAll[i].p_quatAccel_b()));
122 ✗ std::array<float, 3> imuRotAccel = { static_cast<float>(eulerAccel.x()), static_cast<float>(eulerAccel.y()), static_cast<float>(eulerAccel.z()) };
123 ✗ if (ImGui::InputFloat3(fmt::format("Rotation Accel [deg]##{}", size_t(id)).c_str(), imuRotAccel.data()))
124 {
125 // (-180:180] x (-90:90] x (-180:180]
126 ✗ imuRotAccel.at(0) = std::max(imuRotAccel.at(0), -179.9999F);
127 ✗ imuRotAccel.at(0) = std::min(imuRotAccel.at(0), 180.0F);
128 ✗ imuRotAccel.at(1) = std::max(imuRotAccel.at(1), -89.9999F);
129 ✗ imuRotAccel.at(1) = std::min(imuRotAccel.at(1), 90.0F);
130 ✗ imuRotAccel.at(2) = std::max(imuRotAccel.at(2), -179.9999F);
131 ✗ imuRotAccel.at(2) = std::min(imuRotAccel.at(2), 180.0F);
132
133 ✗ flow::ApplyChanges();
134 ✗ _imuPosAll[i]._b_quatAccel_p = trafo::b_Quat_p(deg2rad(imuRotAccel.at(0)), deg2rad(imuRotAccel.at(1)), deg2rad(imuRotAccel.at(2)));
135 }
136
137 ✗ Eigen::Vector3d eulerGyro = rad2deg(trafo::quat2eulerZYX(_imuPosAll[i].p_quatGyro_b()));
138 ✗ std::array<float, 3> imuRotGyro = { static_cast<float>(eulerGyro.x()), static_cast<float>(eulerGyro.y()), static_cast<float>(eulerGyro.z()) };
139 ✗ if (ImGui::InputFloat3(fmt::format("Rotation Gyro [deg]##{}", size_t(id)).c_str(), imuRotGyro.data()))
140 {
141 // (-180:180] x (-90:90] x (-180:180]
142 ✗ imuRotGyro.at(0) = std::max(imuRotGyro.at(0), -179.9999F);
143 ✗ imuRotGyro.at(0) = std::min(imuRotGyro.at(0), 180.0F);
144 ✗ imuRotGyro.at(1) = std::max(imuRotGyro.at(1), -89.9999F);
145 ✗ imuRotGyro.at(1) = std::min(imuRotGyro.at(1), 90.0F);
146 ✗ imuRotGyro.at(2) = std::max(imuRotGyro.at(2), -179.9999F);
147 ✗ imuRotGyro.at(2) = std::min(imuRotGyro.at(2), 180.0F);
148
149 ✗ flow::ApplyChanges();
150 ✗ _imuPosAll[i]._b_quatGyro_p = trafo::b_Quat_p(deg2rad(imuRotGyro.at(0)), deg2rad(imuRotGyro.at(1)), deg2rad(imuRotGyro.at(2)));
151 }
152
153 ✗ Eigen::Vector3d eulerMag = rad2deg(trafo::quat2eulerZYX(_imuPosAll[i].p_quatMag_b()));
154 ✗ std::array<float, 3> imuRotMag = { static_cast<float>(eulerMag.x()), static_cast<float>(eulerMag.y()), static_cast<float>(eulerMag.z()) };
155 ✗ if (ImGui::InputFloat3(fmt::format("Rotation Mag [deg]##{}", size_t(id)).c_str(), imuRotMag.data()))
156 {
157 // (-180:180] x (-90:90] x (-180:180]
158 ✗ imuRotMag.at(0) = std::max(imuRotMag.at(0), -179.9999F);
159 ✗ imuRotMag.at(0) = std::min(imuRotMag.at(0), 180.0F);
160 ✗ imuRotMag.at(1) = std::max(imuRotMag.at(1), -89.9999F);
161 ✗ imuRotMag.at(1) = std::min(imuRotMag.at(1), 90.0F);
162 ✗ imuRotMag.at(2) = std::max(imuRotMag.at(2), -179.9999F);
163 ✗ imuRotMag.at(2) = std::min(imuRotMag.at(2), 180.0F);
164
165 ✗ flow::ApplyChanges();
166 ✗ _imuPosAll[i]._b_quatMag_p = trafo::b_Quat_p(deg2rad(imuRotMag.at(0)), deg2rad(imuRotMag.at(1)), deg2rad(imuRotMag.at(2)));
167 }
168
169 ✗ ImGui::TreePop();
170 }
171 }
172 ✗ }
173
174 ✗ [[nodiscard]] json NAV::MultiImuFile::save() const
175 {
176 LOG_TRACE("{}: called", nameId());
177
178 ✗ json j;
179
180 ✗ j["FileReader"] = FileReader::save();
181 ✗ j["imuPos"] = _imuPosAll;
182 ✗ j["nmeaType"] = _nmeaType;
183 ✗ j["startTime"] = _startTime;
184 ✗ j["delim"] = _delim;
185
186 ✗ return j;
187 ✗ }
188
189 2 void NAV::MultiImuFile::restore(json const& j)
190 {
191 LOG_TRACE("{}: called", nameId());
192
193
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("FileReader"))
194 {
195 2 FileReader::restore(j.at("FileReader"));
196 }
197
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("imuPos"))
198 {
199 2 j.at("imuPos").get_to(_imuPosAll);
200 }
201
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("nmeaType"))
202 {
203 2 j.at("nmeaType").get_to(_nmeaType);
204 }
205
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("startTime"))
206 {
207 2 j.at("startTime").get_to(_startTime);
208 }
209
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (j.contains("delim"))
210 {
211 2 j.at("delim").get_to(_delim);
212 }
213 2 }
214
215 2 bool NAV::MultiImuFile::initialize()
216 {
217 LOG_TRACE("{}: called", nameId());
218
219 2 return FileReader::initialize();
220 }
221
222 2 void NAV::MultiImuFile::deinitialize()
223 {
224 LOG_TRACE("{}: called", nameId());
225
226 2 FileReader::deinitialize();
227 2 }
228
229 4 bool NAV::MultiImuFile::resetNode()
230 {
231 4 _lastFiltObs.reset();
232 4 _lineCounter = 0;
233
234 4 FileReader::resetReader();
235
236
2/2
✓ Branch 5 taken 20 times.
✓ Branch 6 taken 4 times.
 24 for (auto& sensor : _messages)
237 {
238 20 sensor.clear();
239 }
240
2/2
✓ Branch 4 taken 20 times.
✓ Branch 5 taken 4 times.
 24 for (auto& cnt : _messageCnt)
241 {
242 20 cnt = 0;
243 }
244
245 4 return true;
246 }
247
248 114 void NAV::MultiImuFile::updateNumberOfOutputPins()
249 {
250
2/2
✓ Branch 1 taken 570 times.
✓ Branch 2 taken 114 times.
 684 while (outputPins.size() < _nSensors)
251 {
252
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);
253 570 _imuPosAll.resize(_nSensors);
254
255 570 _messages.resize(_nSensors);
256 570 _messageCnt.resize(_nSensors);
257 }
258 684 }
259
260 2 NAV::FileReader::FileType NAV::MultiImuFile::determineFileType()
261 {
262 LOG_TRACE("called");
263
264
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 if (good())
265 {
266 2 return FileType::ASCII;
267 }
268
269 ✗ LOG_ERROR("Could not open file {}", getFilepath().string());
270 ✗ return FileType::NONE;
271 }
272
273 2 void NAV::MultiImuFile::readHeader()
274 {
275 LOG_TRACE("called");
276
277 2 _gpzdaFound = false;
278 2 _gpggaFound = false;
279 2 const char* gpzda = "GPZDA";
280 2 const char* gpgga = "GPGGA";
281
282 2 std::string line;
283
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 auto len = tellg();
284
285 // Find first line of data
286
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))
287 {
288 34 _lineCounter++;
289
290 // Remove any trailing non text characters
291
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());
292
293
2/2
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 30 times.
 34 if ((line.find(gpzda)) != std::string::npos)
294 {
295 4 _gpzdaFound = true;
296
297 4 int32_t year{};
298 4 int32_t month{};
299 4 int32_t day{};
300 4 int32_t hour{};
301 4 int32_t min{};
302 4 long double sec{};
303
304 // Convert line into stream
305
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 std::stringstream lineStream(line);
306 4 std::string cell;
307
308 // Split line at comma
309
2/2
✓ Branch 5 taken 20 times.
✓ Branch 6 taken 4 times.
 24 for (const auto& col : _headerColumns)
310 {
311
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, ','))
312 {
313 // Remove any trailing non text characters
314
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());
315
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
 20 while (cell.empty())
316 {
317 ✗ std::getline(lineStream, cell, ',');
318 }
319
320
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")
321 {
322
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());
323 4 continue;
324 }
325
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")
326 {
327
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));
328
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));
329
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));
330 4 continue;
331 }
332
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")
333 {
334
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 day = std::stoi(cell);
335 4 continue;
336 }
337
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")
338 {
339
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 month = std::stoi(cell);
340 4 continue;
341 }
342
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 if (col == "year")
343 {
344
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 year = std::stoi(cell);
345 }
346
347
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
348
349
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 len = tellg();
350 4 continue;
351 4 }
352 }
353 4 }
354
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 32 times.
 34 if (line.find(gpgga) != std::string::npos)
355 {
356 2 _gpggaFound = true;
357 }
358
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))
359 {
360
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);
361
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
362 2 break;
363 }
364
1/2
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
 32 len = tellg();
365 }
366
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)
367 {
368 ✗ 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());
369 ✗ _nmeaType = NmeaType::GPGGA;
370 }
371
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)
372 {
373 ✗ 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());
374 ✗ _nmeaType = NmeaType::GPZDA;
375 }
376
1/2
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
 2 _delim = _gpzdaFound ? ',' : ' ';
377 2 }
378
379 15174 std::shared_ptr<const NAV::NodeData> NAV::MultiImuFile::pollData(size_t pinIdx, bool peek)
380 {
381 15174 std::shared_ptr<ImuObs> obs = nullptr;
382
383
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
384 {
385
1/2
✓ Branch 1 taken 8515 times.
✗ Branch 2 not taken.
 8515 obs = _messages.at(pinIdx).begin()->second;
386
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
387 {
388
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());
389 }
390 }
391 else
392 {
393 // Read line
394 6659 std::string line;
395
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))
396 {
397 7582 _lineCounter++;
398
399 // Remove any starting non text characters
400
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); }));
401
402
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7582 times.
 7582 if (line.empty())
403 {
404 933 continue;
405 }
406
407 // Convert line into stream
408
1/2
✓ Branch 2 taken 7582 times.
✗ Branch 3 not taken.
 7582 std::stringstream lineStream(line);
409 7582 std::string cell;
410
411 7582 size_t sensorId{};
412 7582 double gpsSecond{};
413 7582 double timeNumerator{};
414 7582 double timeDenominator{};
415 7582 std::optional<double> accelX;
416 7582 std::optional<double> accelY;
417 7582 std::optional<double> accelZ;
418 7582 std::optional<double> gyroX;
419 7582 std::optional<double> gyroY;
420 7582 std::optional<double> gyroZ;
421
422 // Split line at comma
423
2/2
✓ Branch 5 taken 75820 times.
✓ Branch 6 taken 7582 times.
 83402 for (const auto& col : _columns)
424 {
425
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))
426 {
427 // Remove any trailing non text characters
428
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());
429
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 75820 times.
 75820 while (cell.empty())
430 {
431 ✗ std::getline(lineStream, cell, ' ');
432 }
433
434
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")
435 {
436
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 sensorId = std::stoul(cell); // NOLINT(clang-diagnostic-implicit-int-conversion)
437 }
438
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")
439 {
440
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 gpsSecond = std::stod(cell); // [s]
441
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 7580 times.
 7582 if (_startupGpsSecond == 0)
442 {
443 2 _startupGpsSecond = gpsSecond;
444 }
445 }
446
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")
447 {
448
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 timeNumerator = std::stod(cell);
449 }
450
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")
451 {
452
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 timeDenominator = std::stod(cell);
453 }
454
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")
455 {
456
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 accelX = 0.001 * std::stod(cell); // [m/s²]
457 }
458
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")
459 {
460
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 accelY = 0.001 * std::stod(cell); // [m/s²]
461 }
462
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")
463 {
464
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 accelZ = 0.001 * std::stod(cell); // [m/s²]
465 }
466
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")
467 {
468
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 gyroX = deg2rad(std::stod(cell) / 131); // [deg/s]
469 }
470
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")
471 {
472
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 gyroY = deg2rad(std::stod(cell)) / 131; // [deg/s]
473 }
474
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")
475 {
476
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 gyroZ = deg2rad(std::stod(cell)) / 131; // [deg/s]
477 }
478 }
479 }
480
481 7582 auto timeStamp = gpsSecond + timeNumerator / timeDenominator - _startupGpsSecond;
482
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 7582 times.
 7582 if (!peek)
483 {
484 ✗ LOG_DEBUG("line: {}", line);
485 ✗ LOG_DEBUG("timeStamp: {}", timeStamp);
486 }
487
488
1/2
✓ Branch 2 taken 7582 times.
✗ Branch 3 not taken.
 7582 obs = std::make_shared<ImuObs>(_imuPosAll[sensorId - 1]);
489
490
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);
491
492
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)
493 {
494 ✗ LOG_ERROR("{}: Fields 'accelX', 'accelY', 'accelZ' are needed.", nameId());
495 ✗ return nullptr;
496 }
497
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)
498 {
499 ✗ LOG_ERROR("{}: Fields 'gyroX', 'gyroY', 'gyroZ' are needed.", nameId());
500 ✗ return nullptr;
501 }
502
503
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() };
504
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() };
505
506
2/2
✓ Branch 0 taken 933 times.
✓ Branch 1 taken 6649 times.
 7582 if (sensorId - 1 != pinIdx)
507 {
508 // Write message into buffer to process later on correct pin
509
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));
510
511 933 continue; // Read next line in file and search for correct sensor
512 }
513
1/2
✓ Branch 0 taken 6649 times.
✗ Branch 1 not taken.
 6649 if (peek)
514 {
515 // Write message into buffer to process later in poll step
516
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));
517 }
518 6649 break;
519 15164 }
520 6659 }
521
522 // Calls all the callbacks
523
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)
524 {
525
1/2
✓ Branch 1 taken 7582 times.
✗ Branch 2 not taken.
 7582 _messageCnt.at(pinIdx)++;
526
527 // Detect jumps back in time
528
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 7582 times.
 7582 if (obs->insTime < _lastFiltObs)
529 {
530 ✗ LOG_ERROR("{}: Jumped back in time on line {} (at {}), by {} s", nameId(), _lineCounter, obs->insTime.toYMDHMS(), static_cast<double>((obs->insTime - _lastFiltObs).count()));
531 ✗ return obs;
532 }
533 7582 _lastFiltObs = obs->insTime;
534
535
1/2
✓ Branch 2 taken 7582 times.
✗ Branch 3 not taken.
 7582 invokeCallbacks(pinIdx, obs);
536 }
537
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 15164 times.
 15174 if (obs == nullptr)
538 {
539
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));
540 }
541 15174 return obs;
542 15174 }
543