INSTINCT Code Coverage Report

Directory: src/
File: Nodes/Experimental/DataProvider/IMU/NetworkStream/SkydelNetworkStream.cpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 0 160 0.0%
Functions: 0 13 0.0%
Branches: 0 198 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 ✗ double posX = 0.0;
104 ✗ double posY = 0.0;
105 ✗ double posZ = 0.0;
106 ✗ double attRoll = 0.0;
107 ✗ double attPitch = 0.0;
108 ✗ double attYaw = 0.0;
109
110 ✗ double accelX = 0.0;
111 ✗ double accelY = 0.0;
112 ✗ double accelZ = 0.0;
113 ✗ double gyroX = 0.0;
114 ✗ double gyroY = 0.0;
115 ✗ double gyroZ = 0.0;
116
117 ✗ for (size_t i = 0; i < 13; i++)
118 {
119 // Reading string from csv
120 ✗ if (std::getline(lineStream, cell, ','))
121 {
122 ✗ switch (i)
123 {
124 ✗ case 0:
125 ✗ obs->timeSinceStartup = std::stod(cell) * 1e6; // [ns] = [ms] * 1e6
126 ✗ break;
127 ✗ case 1:
128 ✗ posX = std::stod(cell);
129 ✗ break;
130 ✗ case 2:
131 ✗ posY = std::stod(cell);
132 ✗ break;
133 ✗ case 3:
134 ✗ posZ = std::stod(cell);
135 ✗ break;
136 ✗ case 4:
137 ✗ attRoll = std::stod(cell);
138 ✗ break;
139 ✗ case 5:
140 ✗ attPitch = std::stod(cell);
141 ✗ break;
142 ✗ case 6:
143 ✗ attYaw = std::stod(cell);
144 ✗ break;
145 ✗ case 7:
146 ✗ accelX = std::stod(cell);
147 ✗ break;
148 ✗ case 8:
149 ✗ accelY = std::stod(cell);
150 ✗ break;
151 ✗ case 9:
152 ✗ accelZ = std::stod(cell);
153 ✗ break;
154 ✗ case 10:
155 ✗ gyroX = std::stod(cell);
156 ✗ break;
157 ✗ case 11:
158 ✗ gyroY = std::stod(cell);
159 ✗ break;
160 ✗ case 12:
161 ✗ gyroZ = std::stod(cell);
162 ✗ break;
163
164 ✗ default:
165 ✗ LOG_ERROR("Error in network stream: Cell index is out of bounds");
166 ✗ break;
167 }
168 }
169 else
170 {
171 ✗ LOG_ERROR("Error in IMU stream: Reading a string from csv failed");
172 ✗ return;
173 }
174 }
175
176 // Set GNSS values
177 ✗ Eigen::Vector3d e_position{ posX, posY, posZ };
178 ✗ Eigen::Vector3d lla_position = trafo::ecef2lla_WGS84(e_position);
179 ✗ Eigen::Quaterniond e_Quat_b;
180 ✗ e_Quat_b = trafo::e_Quat_n(lla_position(0), lla_position(1)) * trafo::n_Quat_b(attRoll, attPitch, attYaw);
181
182 ✗ obsG->setPosition_e(e_position);
183 ✗ Eigen::Vector3d velDummy{ 0, 0, 0 }; // TODO: Add velocity output in Skydel API and NetStream
184 ✗ obsG->setVelocity_e(velDummy);
185 ✗ 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
186
187 // Set IMU values
188 ✗ obs->p_acceleration = { accelX, accelY, accelZ };
189 ✗ obs->p_angularRate = { gyroX, gyroY, gyroZ };
190 // TODO: Add magnetometer model to Skydel API 'InstinctDataStream'
191
192 ✗ InsTime currentTime = util::time::GetCurrentInsTime();
193 ✗ if (!currentTime.empty())
194 {
195 ✗ obs->insTime = currentTime;
196 ✗ obsG->insTime = currentTime;
197 }
198
199 ✗ if (obs->timeSinceStartup.has_value())
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 (obs->timeSinceStartup.value() - _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, obs->timeSinceStartup.value(), static_cast<double>(obs->timeSinceStartup.value() - _lastMessageTime) * 1e-9);
209 }
210 }
211 ✗ _lastMessageTime = obs->timeSinceStartup.value();
212 }
213
214 ✗ this->invokeCallbacks(OUTPUT_PORT_INDEX_GNSS_OBS, obsG);
215 ✗ this->invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs);
216
217 // Data rate (for visualization in GUI)
218 ✗ _packageCount++;
219
220 ✗ if (_startCounter < _startNow)
221 {
222 ✗ _packageCount = 0;
223 ✗ _startCounter++;
224 }
225
226 ✗ if (_packageCount == 1)
227 {
228 ✗ _startPoint = std::chrono::steady_clock::now();
229 }
230 ✗ else if (_packageCount == _packagesNumber)
231 {
232 ✗ std::chrono::duration<double> elapsed_seconds = std::chrono::steady_clock::now() - _startPoint;
233 ✗ _dataRate = static_cast<double>(_packagesNumber - 1) / elapsed_seconds.count();
234
235 // Dynamic adaptation of data rate to a human-readable display update rate in GUI (~ 1 Hz)
236 ✗ if ((_dataRate > 2) && (_dataRate < 1001)) // restriction on 'reasonable' sensor data rates (Skydel max. is 1000 Hz)
237 {
238 ✗ _packagesNumber = static_cast<int>(_dataRate);
239 }
240 ✗ else if (_dataRate >= 1001)
241 {
242 ✗ _packagesNumber = 1000;
243 }
244
245 ✗ _packageCount = 0;
246
247 LOG_DATA("Elapsed Seconds = {}", elapsed_seconds.count());
248 LOG_DATA("SkydelNetworkStream: dataRate = {}", _dataRate);
249 }
250 ✗ }
251 else
252 {
253 ✗ LOG_ERROR("Error receiving the network stream from Skydel");
254 }
255
256 ✗ if (!_stop)
257 {
258 ✗ do_receive();
259 }
260 });
261 ✗ }
262
263 ✗ bool SkydelNetworkStream::initialize()
264 {
265 LOG_TRACE("{}: called", nameId());
266
267 ✗ _stop = false;
268 ✗ _packageCount = 0;
269 ✗ _startCounter = 0;
270 ✗ _packagesNumber = 2;
271
272 ✗ _lastMessageTime = 0;
273
274 ✗ do_receive();
275
276 ✗ if (_isStartup)
277 {
278 ✗ _testThread = std::thread([this]() {
279 ✗ _ioservice.run();
280 ✗ });
281 }
282 else
283 {
284 ✗ _testThread = std::thread([this]() {
285 ✗ _ioservice.restart();
286 ✗ _ioservice.run();
287 ✗ });
288 }
289
290 ✗ _isStartup = false;
291
292 ✗ return true;
293 }
294
295 ✗ void SkydelNetworkStream::deinitialize()
296 {
297 LOG_TRACE("{}: called", nameId());
298
299 ✗ _stop = true;
300 ✗ _ioservice.stop();
301 ✗ _testThread.join();
302 ✗ }
303
304 } // namespace NAV::experimental
305