INSTINCT Code Coverage Report

Directory: src/
File: Nodes/Experimental/DataProvider/IMU/NetworkStream/SkydelNetworkStream.cpp
Date: 2025-06-02 15:19:59
Exec Total Coverage
Lines: 0 160 0.0%
Functions: 0 13 0.0%
Branches: 0 188 0.0%
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 "SkydelNetworkStream.hpp"
10
11 #include <thread>
12 #include <string>
13 #include <vector>
14 #include <chrono>
15 #include <cmath>
16 #include <sstream>
17
18 #include "util/Logger.hpp"
19 #include "internal/NodeManager.hpp"
20 #include "internal/FlowManager.hpp"
21 #include "NodeData/IMU/ImuObs.hpp"
22 #include "util/Time/TimeBase.hpp"
23 #include "Navigation/Transformations/CoordinateFrames.hpp"
24 #include "NodeData/State/PosVelAtt.hpp"
25 #include "internal/gui/widgets/HelpMarker.hpp"
26
27 namespace nm = NAV::NodeManager;
28 using boost::asio::ip::udp;
29
30 namespace NAV::experimental
31 {
32
33 ✗ SkydelNetworkStream::SkydelNetworkStream()
34 ✗ : Imu(typeStatic()), _senderEndpoint(udp::v4(), 4444), _socket(_ioservice, _senderEndpoint)
35 {
36 ✗ _onlyRealTime = true;
37 ✗ _hasConfig = true;
38 ✗ _guiConfigDefaultWindowSize = { 305, 70 };
39
40 ✗ nm::CreateOutputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() });
41 ✗ nm::CreateOutputPin(this, "PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() });
42 ✗ }
43
44 ✗ SkydelNetworkStream::~SkydelNetworkStream()
45 {
46 LOG_TRACE("{}: called", nameId());
47 ✗ }
48
49 ✗ std::string SkydelNetworkStream::typeStatic()
50 {
51 ✗ return "SkydelNetworkStream";
52 }
53
54 ✗ std::string SkydelNetworkStream::type() const
55 {
56 ✗ return typeStatic();
57 }
58
59 ✗ std::string SkydelNetworkStream::category()
60 {
61 ✗ return "Data Provider";
62 }
63
64 ✗ void SkydelNetworkStream::guiConfig()
65 {
66 ✗ std::string str;
67
68 ✗ if (_startCounter < _startNow)
69 {
70 ✗ str = "(loading)";
71 }
72 else
73 {
74 ✗ std::ostringstream strs;
75 ✗ strs << _dataRate;
76 ✗ str = strs.str();
77 ✗ }
78
79 ✗ ImGui::LabelText(str.c_str(), "data rate [Hz]");
80 ✗ ImGui::SameLine();
81 ✗ gui::widgets::HelpMarker("The data rate can be adjusted in Skydel: Settings/Plug-ins/<Plug-in-name>/Plug-in UI. Make sure to enable either WiFi or a LAN connection. Enabling both can lead to loss of data, because Skydel only knows one ip address.");
82 ✗ }
83
84 ✗ bool SkydelNetworkStream::resetNode()
85 {
86 ✗ return true;
87 }
88
89 ✗ void SkydelNetworkStream::do_receive()
90 {
91 ✗ _socket.async_receive_from(
92 ✗ boost::asio::buffer(_data, _maxLength), _senderEndpoint,
93 ✗ [this](boost::system::error_code errorRcvd, std::size_t bytesRcvd) {
94 ✗ if ((!errorRcvd) && (bytesRcvd > 0))
95 {
96 // Splitting the incoming string analogous to 'ImuFile.cpp'
97 ✗ std::stringstream lineStream(std::string(_data.begin(), _data.end()));
98 ✗ std::string cell;
99 ✗ auto obsG = std::make_shared<PosVelAtt>();
100 ✗ auto obs = std::make_shared<ImuObs>(this->_imuPos);
101
102 // Inits for simulated measurement variables
103 ✗ uint64_t timeSinceStartup = 0;
104
105 ✗ double posX = 0.0;
106 ✗ double posY = 0.0;
107 ✗ double posZ = 0.0;
108 ✗ double attRoll = 0.0;
109 ✗ double attPitch = 0.0;
110 ✗ double attYaw = 0.0;
111
112 ✗ double accelX = 0.0;
113 ✗ double accelY = 0.0;
114 ✗ double accelZ = 0.0;
115 ✗ double gyroX = 0.0;
116 ✗ double gyroY = 0.0;
117 ✗ double gyroZ = 0.0;
118
119 ✗ for (size_t i = 0; i < 13; i++)
120 {
121 // Reading string from csv
122 ✗ if (std::getline(lineStream, cell, ','))
123 {
124 ✗ switch (i)
125 {
126 ✗ case 0:
127 ✗ timeSinceStartup = static_cast<uint64_t>(std::stod(cell) * 1e6); // [ns] = [ms] * 1e6
128 ✗ break;
129 ✗ case 1:
130 ✗ posX = std::stod(cell);
131 ✗ break;
132 ✗ case 2:
133 ✗ posY = std::stod(cell);
134 ✗ break;
135 ✗ case 3:
136 ✗ posZ = std::stod(cell);
137 ✗ break;
138 ✗ case 4:
139 ✗ attRoll = std::stod(cell);
140 ✗ break;
141 ✗ case 5:
142 ✗ attPitch = std::stod(cell);
143 ✗ break;
144 ✗ case 6:
145 ✗ attYaw = std::stod(cell);
146 ✗ break;
147 ✗ case 7:
148 ✗ accelX = std::stod(cell);
149 ✗ break;
150 ✗ case 8:
151 ✗ accelY = std::stod(cell);
152 ✗ break;
153 ✗ case 9:
154 ✗ accelZ = std::stod(cell);
155 ✗ break;
156 ✗ case 10:
157 ✗ gyroX = std::stod(cell);
158 ✗ break;
159 ✗ case 11:
160 ✗ gyroY = std::stod(cell);
161 ✗ break;
162 ✗ case 12:
163 ✗ gyroZ = std::stod(cell);
164 ✗ break;
165
166 ✗ default:
167 ✗ LOG_ERROR("Error in network stream: Cell index is out of bounds");
168 ✗ break;
169 }
170 }
171 else
172 {
173 ✗ LOG_ERROR("Error in IMU stream: Reading a string from csv failed");
174 ✗ return;
175 }
176 }
177
178 // Set GNSS values
179 ✗ Eigen::Vector3d e_position{ posX, posY, posZ };
180 ✗ Eigen::Vector3d lla_position = trafo::ecef2lla_WGS84(e_position);
181 ✗ Eigen::Quaterniond e_Quat_b;
182 ✗ e_Quat_b = trafo::e_Quat_n(lla_position(0), lla_position(1)) * trafo::n_Quat_b(attRoll, attPitch, attYaw);
183
184 ✗ obsG->setPosition_e(e_position);
185 ✗ Eigen::Vector3d velDummy{ 0, 0, 0 }; // TODO: Add velocity output in Skydel API and NetStream
186 ✗ obsG->setVelocity_e(velDummy);
187 ✗ obsG->setAttitude_e_Quat_b(e_Quat_b); // Attitude MUST BE set after Position, because the n- to e-sys trafo depends on lla_position
188
189 // Set IMU values
190 ✗ obs->p_acceleration = { accelX, accelY, accelZ };
191 ✗ obs->p_angularRate = { gyroX, gyroY, gyroZ };
192 // TODO: Add magnetometer model to Skydel API 'InstinctDataStream'
193
194 ✗ InsTime currentTime = util::time::GetCurrentInsTime();
195 ✗ if (!currentTime.empty())
196 {
197 ✗ obs->insTime = currentTime;
198 ✗ obsG->insTime = currentTime;
199 }
200
201 ✗ if (_lastMessageTime)
202 {
203 // FIXME: This seems like a bug in clang-tidy. Check if it is working in future versions of clang-tidy
204 // NOLINTNEXTLINE(hicpp-use-nullptr, modernize-use-nullptr)
205 ✗ if (timeSinceStartup - _lastMessageTime >= static_cast<uint64_t>(1.5 / _dataRate * 1e9))
206 {
207 ✗ LOG_WARN("{}: Potentially lost a message. Previous message was at {} and current message at {} which is a time difference of {} seconds.", nameId(),
208 _lastMessageTime, timeSinceStartup, static_cast<double>(timeSinceStartup - _lastMessageTime) * 1e-9);
209 }
210 }
211 ✗ _lastMessageTime = timeSinceStartup;
212
213 ✗ this->invokeCallbacks(OUTPUT_PORT_INDEX_GNSS_OBS, obsG);
214 ✗ this->invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs);
215
216 // Data rate (for visualization in GUI)
217 ✗ _packageCount++;
218
219 ✗ if (_startCounter < _startNow)
220 {
221 ✗ _packageCount = 0;
222 ✗ _startCounter++;
223 }
224
225 ✗ if (_packageCount == 1)
226 {
227 ✗ _startPoint = std::chrono::steady_clock::now();
228 }
229 ✗ else if (_packageCount == _packagesNumber)
230 {
231 ✗ std::chrono::duration<double> elapsed_seconds = std::chrono::steady_clock::now() - _startPoint;
232 ✗ _dataRate = static_cast<double>(_packagesNumber - 1) / elapsed_seconds.count();
233
234 // Dynamic adaptation of data rate to a human-readable display update rate in GUI (~ 1 Hz)
235 ✗ if ((_dataRate > 2) && (_dataRate < 1001)) // restriction on 'reasonable' sensor data rates (Skydel max. is 1000 Hz)
236 {
237 ✗ _packagesNumber = static_cast<int>(_dataRate);
238 }
239 ✗ else if (_dataRate >= 1001)
240 {
241 ✗ _packagesNumber = 1000;
242 }
243
244 ✗ _packageCount = 0;
245
246 LOG_DATA("Elapsed Seconds = {}", elapsed_seconds.count());
247 LOG_DATA("SkydelNetworkStream: dataRate = {}", _dataRate);
248 }
249 ✗ }
250 else
251 {
252 ✗ LOG_ERROR("Error receiving the network stream from Skydel");
253 }
254
255 ✗ if (!_stop)
256 {
257 ✗ do_receive();
258 }
259 });
260 ✗ }
261
262 ✗ bool SkydelNetworkStream::initialize()
263 {
264 LOG_TRACE("{}: called", nameId());
265
266 ✗ _stop = false;
267 ✗ _packageCount = 0;
268 ✗ _startCounter = 0;
269 ✗ _packagesNumber = 2;
270
271 ✗ _lastMessageTime = 0;
272
273 ✗ do_receive();
274
275 ✗ if (_isStartup)
276 {
277 ✗ _testThread = std::thread([this]() {
278 ✗ _ioservice.run();
279 ✗ });
280 }
281 else
282 {
283 ✗ _testThread = std::thread([this]() {
284 ✗ _ioservice.restart();
285 ✗ _ioservice.run();
286 ✗ });
287 }
288
289 ✗ _isStartup = false;
290
291 ✗ return true;
292 }
293
294 ✗ void SkydelNetworkStream::deinitialize()
295 {
296 LOG_TRACE("{}: called", nameId());
297
298 ✗ _stop = true;
299 ✗ _ioservice.stop();
300 ✗ _testThread.join();
301 ✗ }
302
303 } // namespace NAV::experimental
304