INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataProvider/State/PosVelAttFile.cpp
Date: 2025-06-06 13:17:47
Exec Total Coverage
Lines: 13 286 4.5%
Functions: 4 16 25.0%
Branches: 11 982 1.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 "PosVelAttFile.hpp"
10
11 #include "util/Logger.hpp"
12 #include "util/StringUtil.hpp"
13
14 #include "Navigation/Transformations/CoordinateFrames.hpp"
15 #include "Navigation/Transformations/Units.hpp"
16
17 #include "internal/NodeManager.hpp"
18 #include <Eigen/src/Core/DiagonalMatrix.h>
19 namespace nm = NAV::NodeManager;
20 #include "internal/FlowManager.hpp"
21
22 #include "NodeData/State/PosVelAtt.hpp"
23
24 112 NAV::PosVelAttFile::PosVelAttFile()
25
3/6
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 112 times.
✗ Branch 9 not taken.
 112 : Node(typeStatic())
26 {
27 LOG_TRACE("{}: called", name);
28
29 112 _hasConfig = true;
30 112 _guiConfigDefaultWindowSize = { 488, 248 };
31
32
4/8
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 336 times.
✓ Branch 9 taken 112 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 560 nm::CreateOutputPin(this, "PosVelAtt", Pin::Type::Flow, { Pos::type(), PosVel::type(), PosVelAtt::type() }, &PosVelAttFile::pollData);
33
2/4
✓ Branch 2 taken 112 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
 224 nm::CreateOutputPin(this, "Header Columns", Pin::Type::Object, { "std::vector<std::string>" }, &_headerColumns);
34 224 }
35
36 224 NAV::PosVelAttFile::~PosVelAttFile()
37 {
38 LOG_TRACE("{}: called", nameId());
39 224 }
40
41 224 std::string NAV::PosVelAttFile::typeStatic()
42 {
43
1/2
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
 448 return "PosVelAttFile";
44 }
45
46 ✗ std::string NAV::PosVelAttFile::type() const
47 {
48 ✗ return typeStatic();
49 }
50
51 112 std::string NAV::PosVelAttFile::category()
52 {
53
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 224 return "Data Provider";
54 }
55
56 ✗ void NAV::PosVelAttFile::guiConfig()
57 {
58 ✗ if (auto res = FileReader::guiConfig(".csv,.*", { ".csv" }, size_t(id), nameId()))
59 {
60 ✗ LOG_DEBUG("{}: Path changed to {}", nameId(), _path);
61 ✗ flow::ApplyChanges();
62 ✗ if (res == FileReader::PATH_CHANGED)
63 {
64 ✗ doReinitialize();
65 }
66 else
67 {
68 ✗ doDeinitialize();
69 }
70 }
71
72 // Header info
73 ✗ if (ImGui::BeginTable(fmt::format("##PvaHeaders ({})", id.AsPointer()).c_str(), 4,
74 ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg))
75 {
76 ✗ ImGui::TableSetupColumn("Time", ImGuiTableColumnFlags_WidthFixed);
77 ✗ ImGui::TableSetupColumn("Position", ImGuiTableColumnFlags_WidthFixed);
78 ✗ ImGui::TableSetupColumn("Velocity", ImGuiTableColumnFlags_WidthFixed);
79 ✗ ImGui::TableSetupColumn("Attitude", ImGuiTableColumnFlags_WidthFixed);
80 ✗ ImGui::TableHeadersRow();
81
82 ✗ auto TextColoredIfExists = [this](int index, const char* text) {
83 ✗ ImGui::TableSetColumnIndex(index);
84 ✗ if (std::ranges::find(_headerColumns, text) != _headerColumns.end())
85 {
86 ✗ ImGui::TextUnformatted(text);
87 }
88 else
89 {
90 ✗ ImGui::TextDisabled("%s", text);
91 }
92 ✗ };
93
94 ✗ ImGui::TableNextRow();
95 ✗ TextColoredIfExists(0, "GpsCycle");
96 ✗ TextColoredIfExists(1, "Pos ECEF X [m]");
97 ✗ TextColoredIfExists(2, "X velocity ECEF [m/s]");
98 ✗ TextColoredIfExists(3, "n_Quat_b w");
99 ✗ ImGui::TableNextRow();
100 ✗ TextColoredIfExists(0, "GpsWeek");
101 ✗ TextColoredIfExists(1, "Pos ECEF Y [m]");
102 ✗ TextColoredIfExists(2, "Y velocity ECEF [m/s]");
103 ✗ TextColoredIfExists(3, "n_Quat_b x");
104 ✗ ImGui::TableNextRow();
105 ✗ TextColoredIfExists(0, "GpsToW [s]");
106 ✗ TextColoredIfExists(1, "Pos ECEF Z [m]");
107 ✗ TextColoredIfExists(2, "Z velocity ECEF [m/s]");
108 ✗ TextColoredIfExists(3, "n_Quat_b y");
109 ✗ ImGui::TableNextRow();
110 ✗ TextColoredIfExists(1, "Latitude [deg]");
111 ✗ TextColoredIfExists(2, "Vel N [m/s]");
112 ✗ TextColoredIfExists(3, "n_Quat_b z");
113 ✗ ImGui::TableNextRow();
114 ✗ TextColoredIfExists(1, "Longitude [deg]");
115 ✗ TextColoredIfExists(2, "Vel E [m/s]");
116 ✗ TextColoredIfExists(3, "Roll [deg]");
117 ✗ ImGui::TableNextRow();
118 ✗ TextColoredIfExists(1, "Altitude [m]");
119 ✗ TextColoredIfExists(2, "Vel D [m/s]");
120 ✗ TextColoredIfExists(3, "Pitch [deg]");
121 ✗ ImGui::TableNextRow();
122 ✗ TextColoredIfExists(3, "Yaw [deg]");
123
124 ✗ ImGui::EndTable();
125 }
126 ✗ }
127
128 ✗ [[nodiscard]] json NAV::PosVelAttFile::save() const
129 {
130 LOG_TRACE("{}: called", nameId());
131
132 ✗ json j;
133
134 ✗ j["FileReader"] = FileReader::save();
135 ✗ j["pinType"] = outputPins[OUTPUT_PORT_INDEX_PVA].dataIdentifier;
136
137 ✗ return j;
138 ✗ }
139
140 ✗ void NAV::PosVelAttFile::restore(json const& j)
141 {
142 LOG_TRACE("{}: called", nameId());
143
144 ✗ if (j.contains("FileReader"))
145 {
146 ✗ FileReader::restore(j.at("FileReader"));
147 }
148 ✗ if (j.contains("pinType"))
149 {
150 ✗ j.at("pinType").get_to(outputPins[OUTPUT_PORT_INDEX_PVA].dataIdentifier);
151 }
152 ✗ }
153
154 ✗ bool NAV::PosVelAttFile::initialize()
155 {
156 LOG_TRACE("{}: called", nameId());
157
158 ✗ if (FileReader::initialize())
159 {
160 ✗ for (auto& col : _headerColumns)
161 {
162 ✗ str::replace(col, "GpsTow [s]", "GpsToW [s]");
163 }
164
165 ✗ auto hasCol = [&](const char* text) {
166 ✗ return std::ranges::find(_headerColumns, text) != _headerColumns.end();
167 ✗ };
168
169 ✗ if (!hasCol("GpsCycle") || !hasCol("GpsWeek") || !hasCol("GpsToW [s]"))
170 {
171 ✗ LOG_ERROR("{}: A PosVelAtt File needs a time. Please add columns 'GpsCycle', 'GpsWeek' and 'GpsToW [s]'.", nameId());
172 ✗ return false;
173 }
174 ✗ if (!(hasCol("Pos ECEF X [m]") && hasCol("Pos ECEF Y [m]") && hasCol("Pos ECEF Z [m]"))
175 ✗ && !(hasCol("Latitude [deg]") && hasCol("Longitude [deg]") && hasCol("Altitude [m]")))
176 {
177 ✗ LOG_ERROR("{}: A PosVelAtt File needs a position. Please provide"
178 " either 'Pos ECEF X [m]', 'Pos ECEF Y [m]', 'Pos ECEF Z [m]'"
179 " or 'Latitude [deg]', 'Longitude [deg]', 'Altitude [m]'.",
180 nameId());
181 ✗ return false;
182 }
183 ✗ _fileContent = FileContent::Pos;
184 ✗ outputPins[OUTPUT_PORT_INDEX_PVA].dataIdentifier = std::vector{ Pos::type() };
185
186 ✗ if ((hasCol("Vel ECEF X [m/s]") && hasCol("Vel ECEF Y [m/s]") && hasCol("Vel ECEF Z [m/s]"))
187 ✗ || (hasCol("Vel N [m/s]") && hasCol("Vel E [m/s]") && hasCol("Vel D [m/s]"))
188 ✗ || (hasCol("X velocity ECEF [m/s]") && hasCol("Y velocity ECEF [m/s]") && hasCol("Z velocity ECEF [m/s]")))
189 {
190 ✗ _fileContent = FileContent::PosVel;
191 ✗ outputPins[OUTPUT_PORT_INDEX_PVA].dataIdentifier = std::vector{ PosVel::type() };
192
193 ✗ if ((hasCol("n_Quat_b w") && hasCol("n_Quat_b x") && hasCol("n_Quat_b y") && hasCol("n_Quat_b z"))
194 ✗ || (hasCol("Roll [deg]") && hasCol("Pitch [deg]") && hasCol("Yaw [deg]")))
195 {
196 ✗ _fileContent = FileContent::PosVelAtt;
197 ✗ outputPins[OUTPUT_PORT_INDEX_PVA].dataIdentifier = std::vector{ PosVelAtt::type() };
198 }
199 }
200
201 ✗ for (auto& link : outputPins[OUTPUT_PORT_INDEX_PVA].links)
202 {
203 ✗ if (auto* pin = link.getConnectedPin())
204 {
205 ✗ outputPins[OUTPUT_PORT_INDEX_PVA].recreateLink(*pin);
206 }
207 }
208
209 ✗ return true;
210 }
211
212 ✗ outputPins[OUTPUT_PORT_INDEX_PVA].dataIdentifier = { Pos::type(), PosVel::type(), PosVelAtt::type() };
213 ✗ return false;
214 ✗ }
215
216 ✗ void NAV::PosVelAttFile::deinitialize()
217 {
218 LOG_TRACE("{}: called", nameId());
219
220 ✗ FileReader::deinitialize();
221 ✗ }
222
223 ✗ bool NAV::PosVelAttFile::resetNode()
224 {
225 ✗ FileReader::resetReader();
226
227 ✗ return true;
228 }
229
230 ✗ std::shared_ptr<const NAV::NodeData> NAV::PosVelAttFile::pollData()
231 {
232 ✗ std::shared_ptr<Pos> obs;
233 ✗ switch (_fileContent)
234 {
235 ✗ case FileContent::Pos:
236 ✗ obs = std::make_shared<Pos>();
237 ✗ break;
238 ✗ case FileContent::PosVel:
239 ✗ obs = std::make_shared<PosVel>();
240 ✗ break;
241 ✗ case FileContent::PosVelAtt:
242 ✗ obs = std::make_shared<PosVelAtt>();
243 ✗ break;
244 }
245
246 // Read line
247 ✗ std::string line;
248 ✗ getline(line);
249 // Remove any starting non text characters
250 ✗ line.erase(line.begin(), std::ranges::find_if(line, [](int ch) { return std::isgraph(ch); }));
251
252 ✗ if (line.empty())
253 {
254 ✗ return nullptr;
255 }
256
257 // Convert line into stream
258 ✗ std::stringstream lineStream(line);
259 ✗ std::string cell;
260
261 ✗ std::optional<uint16_t> gpsCycle = 0;
262 ✗ std::optional<uint16_t> gpsWeek;
263 ✗ std::optional<long double> gpsToW;
264 ✗ std::optional<double> e_position_x;
265 ✗ std::optional<double> e_position_y;
266 ✗ std::optional<double> e_position_z;
267 ✗ std::optional<double> e_positionStdDev_x;
268 ✗ std::optional<double> e_positionStdDev_y;
269 ✗ std::optional<double> e_positionStdDev_z;
270 ✗ std::optional<double> lla_position_x;
271 ✗ std::optional<double> lla_position_y;
272 ✗ std::optional<double> lla_position_z;
273 ✗ std::optional<double> n_positionStdDev_n;
274 ✗ std::optional<double> n_positionStdDev_e;
275 ✗ std::optional<double> n_positionStdDev_d;
276 ✗ std::optional<double> e_velocity_x;
277 ✗ std::optional<double> e_velocity_y;
278 ✗ std::optional<double> e_velocity_z;
279 ✗ std::optional<double> e_velocityStdDev_x;
280 ✗ std::optional<double> e_velocityStdDev_y;
281 ✗ std::optional<double> e_velocityStdDev_z;
282 ✗ std::optional<double> n_velocity_n;
283 ✗ std::optional<double> n_velocity_e;
284 ✗ std::optional<double> n_velocity_d;
285 ✗ std::optional<double> n_velocityStdDev_n;
286 ✗ std::optional<double> n_velocityStdDev_e;
287 ✗ std::optional<double> n_velocityStdDev_d;
288 ✗ std::optional<double> n_Quat_b_w;
289 ✗ std::optional<double> n_Quat_b_x;
290 ✗ std::optional<double> n_Quat_b_y;
291 ✗ std::optional<double> n_Quat_b_z;
292 ✗ std::optional<double> roll;
293 ✗ std::optional<double> pitch;
294 ✗ std::optional<double> yaw;
295
296 // Split line at comma
297 ✗ for (const auto& column : _headerColumns)
298 {
299 ✗ if (std::getline(lineStream, cell, ','))
300 {
301 // Remove any trailing non text characters
302 ✗ cell.erase(std::ranges::find_if(cell, [](int ch) { return std::iscntrl(ch); }), cell.end());
303 ✗ if (cell.empty())
304 {
305 ✗ continue;
306 }
307
308 ✗ if (column == "GpsCycle") { gpsCycle = static_cast<uint16_t>(std::stoul(cell)); }
309 ✗ else if (column == "GpsWeek") { gpsWeek = static_cast<uint16_t>(std::stoul(cell)); }
310 ✗ else if (column == "GpsToW [s]") { gpsToW = std::stold(cell); }
311
312 ✗ else if (column == "Pos ECEF X [m]") { e_position_x = std::stod(cell); }
313 ✗ else if (column == "Pos ECEF Y [m]") { e_position_y = std::stod(cell); }
314 ✗ else if (column == "Pos ECEF Z [m]") { e_position_z = std::stod(cell); }
315 ✗ else if (column == "Pos StdDev ECEF X [m]") { e_positionStdDev_x = std::stod(cell); }
316 ✗ else if (column == "Pos StdDev ECEF Y [m]") { e_positionStdDev_y = std::stod(cell); }
317 ✗ else if (column == "Pos StdDev ECEF Z [m]") { e_positionStdDev_z = std::stod(cell); }
318
319 ✗ else if (column == "Latitude [deg]") { lla_position_x = deg2rad(std::stod(cell)); }
320 ✗ else if (column == "Longitude [deg]") { lla_position_y = deg2rad(std::stod(cell)); }
321 ✗ else if (column == "Altitude [m]") { lla_position_z = std::stod(cell); }
322 ✗ else if (column == "Pos StdDev N [m]") { n_positionStdDev_n = deg2rad(std::stod(cell)); }
323 ✗ else if (column == "Pos StdDev E [m]") { n_positionStdDev_e = deg2rad(std::stod(cell)); }
324 ✗ else if (column == "Pos StdDev D [m]") { n_positionStdDev_d = std::stod(cell); }
325
326 ✗ else if (column == "X velocity ECEF [m/s]") { e_velocity_x = std::stod(cell); }
327 ✗ else if (column == "Y velocity ECEF [m/s]") { e_velocity_y = std::stod(cell); }
328 ✗ else if (column == "Z velocity ECEF [m/s]") { e_velocity_z = std::stod(cell); }
329 ✗ else if (column == "Vel StdDev ECEF X [m/s]") { e_velocityStdDev_x = std::stod(cell); }
330 ✗ else if (column == "Vel StdDev ECEF Y [m/s]") { e_velocityStdDev_y = std::stod(cell); }
331 ✗ else if (column == "Vel StdDev ECEF Z [m/s]") { e_velocityStdDev_z = std::stod(cell); }
332
333 ✗ else if (column == "Vel N [m/s]") { n_velocity_n = std::stod(cell); }
334 ✗ else if (column == "Vel E [m/s]") { n_velocity_e = std::stod(cell); }
335 ✗ else if (column == "Vel D [m/s]") { n_velocity_d = std::stod(cell); }
336 ✗ else if (column == "Vel StdDev N [m/s]") { n_velocityStdDev_n = std::stod(cell); }
337 ✗ else if (column == "Vel StdDev E [m/s]") { n_velocityStdDev_e = std::stod(cell); }
338 ✗ else if (column == "Vel StdDev D [m/s]") { n_velocityStdDev_d = std::stod(cell); }
339
340 ✗ else if (column == "n_Quat_b w") { n_Quat_b_w = std::stod(cell); }
341 ✗ else if (column == "n_Quat_b x") { n_Quat_b_x = std::stod(cell); }
342 ✗ else if (column == "n_Quat_b y") { n_Quat_b_y = std::stod(cell); }
343 ✗ else if (column == "n_Quat_b z") { n_Quat_b_z = std::stod(cell); }
344
345 ✗ else if (column == "Roll [deg]") { roll = deg2rad(std::stod(cell)); }
346 ✗ else if (column == "Pitch [deg]") { pitch = deg2rad(std::stod(cell)); }
347 ✗ else if (column == "Yaw [deg]") { yaw = deg2rad(std::stod(cell)); }
348 }
349 }
350
351 ✗ if (gpsCycle.has_value() && gpsWeek.has_value() && gpsToW.has_value())
352 {
353 ✗ obs->insTime = InsTime(gpsCycle.value(), gpsWeek.value(), gpsToW.value());
354 }
355 else
356 {
357 ✗ LOG_WARN("{}: A PosVelAtt File needs a time.", nameId());
358 ✗ return nullptr;
359 }
360
361 ✗ if (_fileContent == FileContent::PosVel || _fileContent == FileContent::PosVelAtt)
362 {
363 ✗ if (e_velocity_x.has_value() && e_velocity_y.has_value() && e_velocity_z.has_value())
364 {
365 ✗ auto lla_pos = lla_position_x.has_value() && lla_position_y.has_value() && lla_position_z.has_value()
366 ✗ ? Eigen::Vector3d(*lla_position_x, *lla_position_y, *lla_position_z)
367 ✗ : trafo::ecef2lla_WGS84(Eigen::Vector3d(*e_position_x, *e_position_y, *e_position_z));
368 ✗ auto n_vel = trafo::n_Quat_e(lla_pos.x(), lla_pos.y()) * Eigen::Vector3d(*e_velocity_x, *e_velocity_y, *e_velocity_z);
369 ✗ n_velocity_n = n_vel.x();
370 ✗ n_velocity_e = n_vel.y();
371 ✗ n_velocity_d = n_vel.z();
372 }
373 ✗ else if (n_velocity_n.has_value() && n_velocity_e.has_value() && n_velocity_d.has_value())
374 {
375 ✗ auto lla_pos = lla_position_x.has_value() && lla_position_y.has_value() && lla_position_z.has_value()
376 ✗ ? Eigen::Vector3d(*lla_position_x, *lla_position_y, *lla_position_z)
377 ✗ : trafo::ecef2lla_WGS84(Eigen::Vector3d(*e_position_x, *e_position_y, *e_position_z));
378 ✗ auto e_vel = trafo::e_Quat_n(lla_pos.x(), lla_pos.y()) * Eigen::Vector3d(*e_velocity_x, *e_velocity_y, *e_velocity_z);
379 ✗ e_velocity_x = e_vel.x();
380 ✗ e_velocity_y = e_vel.y();
381 ✗ e_velocity_z = e_vel.z();
382 }
383
384 ✗ if (e_position_x.has_value() && e_position_y.has_value() && e_position_z.has_value()
385 ✗ && e_velocity_x.has_value() && e_velocity_y.has_value() && e_velocity_z.has_value())
386 {
387 ✗ if (auto posVel = std::reinterpret_pointer_cast<PosVel>(obs))
388 {
389 ✗ if (e_positionStdDev_x.has_value() && e_positionStdDev_y.has_value() && e_positionStdDev_z.has_value()
390 ✗ && e_velocityStdDev_x.has_value() && e_velocityStdDev_y.has_value() && e_velocityStdDev_z.has_value())
391 {
392 ;
393 ✗ posVel->setPosVelAndCov_e(Eigen::Vector3d{ e_position_x.value(), e_position_y.value(), e_position_z.value() },
394 ✗ Eigen::Vector3d{ e_velocity_x.value(), e_velocity_y.value(), e_velocity_z.value() },
395 ✗ (Eigen::Vector6d() << e_positionStdDev_x.value(), e_positionStdDev_y.value(), e_positionStdDev_z.value(),
396 ✗ e_velocityStdDev_x.value(), e_velocityStdDev_y.value(), e_velocityStdDev_z.value())
397 ✗ .finished()
398 ✗ .asDiagonal()
399 ✗ .toDenseMatrix());
400 }
401 else
402 {
403 ✗ posVel->setPosition_e(Eigen::Vector3d{ e_position_x.value(), e_position_y.value(), e_position_z.value() });
404 ✗ posVel->setVelocity_e(Eigen::Vector3d{ e_velocity_x.value(), e_velocity_y.value(), e_velocity_z.value() });
405 }
406 ✗ }
407 }
408 ✗ else if (lla_position_x.has_value() && lla_position_y.has_value() && lla_position_z.has_value()
409 ✗ && n_velocity_n.has_value() && n_velocity_e.has_value() && n_velocity_d.has_value())
410 {
411 ✗ if (auto posVel = std::reinterpret_pointer_cast<PosVel>(obs))
412 {
413 ✗ if (n_positionStdDev_n.has_value() && n_positionStdDev_e.has_value() && n_positionStdDev_d.has_value()
414 ✗ && n_velocityStdDev_n.has_value() && n_velocityStdDev_e.has_value() && n_velocityStdDev_d.has_value())
415 {
416 ✗ posVel->setPosVelAndCov_n(Eigen::Vector3d{ lla_position_x.value(), lla_position_y.value(), lla_position_z.value() },
417 ✗ Eigen::Vector3d{ n_velocity_n.value(), n_velocity_e.value(), n_velocity_d.value() },
418 ✗ (Eigen::Vector6d() << n_positionStdDev_n.value(), n_positionStdDev_e.value(), n_positionStdDev_d.value(),
419 ✗ n_velocityStdDev_n.value(), n_velocityStdDev_e.value(), n_velocityStdDev_d.value())
420 ✗ .finished()
421 ✗ .asDiagonal()
422 ✗ .toDenseMatrix());
423 }
424 else
425 {
426 ✗ posVel->setPosition_lla(Eigen::Vector3d{ lla_position_x.value(), lla_position_y.value(), lla_position_z.value() });
427 ✗ posVel->setVelocity_n(Eigen::Vector3d{ n_velocity_n.value(), n_velocity_e.value(), n_velocity_d.value() });
428 }
429 ✗ }
430 }
431 else
432 {
433 ✗ LOG_WARN("{}: A PosVel/PosVelAtt file needs a position and velocity.", nameId());
434 ✗ return nullptr;
435 }
436 ✗ }
437 ✗ else if (_fileContent == FileContent::Pos)
438 {
439 ✗ if (e_position_x.has_value() && e_position_y.has_value() && e_position_z.has_value())
440 {
441 ✗ if (e_positionStdDev_x.has_value() && e_positionStdDev_y.has_value() && e_positionStdDev_z.has_value())
442 {
443 ✗ obs->setPositionAndCov_e(Eigen::Vector3d{ e_position_x.value(), e_position_y.value(), e_position_z.value() },
444 ✗ Eigen::DiagonalMatrix<double, 3>{ e_positionStdDev_x.value(), e_positionStdDev_y.value(), e_positionStdDev_z.value() }.toDenseMatrix());
445 }
446 else
447 {
448 ✗ obs->setPosition_e(Eigen::Vector3d{ e_position_x.value(), e_position_y.value(), e_position_z.value() });
449 }
450 }
451 ✗ else if (lla_position_x.has_value() && lla_position_y.has_value() && lla_position_z.has_value())
452 {
453 ✗ if (n_positionStdDev_n.has_value() && n_positionStdDev_e.has_value() && n_positionStdDev_d.has_value())
454 {
455 ✗ obs->setPositionAndCov_n(Eigen::Vector3d{ lla_position_x.value(), lla_position_y.value(), lla_position_z.value() },
456 ✗ Eigen::DiagonalMatrix<double, 3>{ n_positionStdDev_n.value(), n_positionStdDev_e.value(), n_positionStdDev_d.value() }.toDenseMatrix());
457 }
458 else
459 {
460 ✗ obs->setPosition_lla(Eigen::Vector3d{ lla_position_x.value(), lla_position_y.value(), lla_position_z.value() });
461 }
462 }
463 else
464 {
465 ✗ LOG_WARN("{}: A Pos file needs a position.", nameId());
466 ✗ return nullptr;
467 }
468 }
469
470 ✗ if (_fileContent == FileContent::PosVelAtt)
471 {
472 ✗ if (n_Quat_b_w.has_value() && n_Quat_b_x.has_value() && n_Quat_b_y.has_value() && n_Quat_b_z.has_value())
473 {
474 ✗ if (auto posVelAtt = std::reinterpret_pointer_cast<PosVelAtt>(obs))
475 {
476 ✗ posVelAtt->setAttitude_n_Quat_b(Eigen::Quaterniond{ n_Quat_b_w.value(), n_Quat_b_x.value(), n_Quat_b_y.value(), n_Quat_b_z.value() });
477 ✗ }
478 }
479 ✗ else if (roll.has_value() && pitch.has_value() && yaw.has_value())
480 {
481 ✗ if (auto posVelAtt = std::reinterpret_pointer_cast<PosVelAtt>(obs))
482 {
483 ✗ posVelAtt->setAttitude_n_Quat_b(trafo::n_Quat_b(roll.value(), pitch.value(), yaw.value()));
484 ✗ }
485 }
486 }
487
488 ✗ invokeCallbacks(OUTPUT_PORT_INDEX_PVA, obs);
489 ✗ return obs;
490 ✗ }
491