INSTINCT Code Coverage Report

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/FlowManager.hpp"
20			#include "NodeData/IMU/ImuObs.hpp"
21			#include "util/Time/TimeBase.hpp"
22			#include "Navigation/Transformations/CoordinateFrames.hpp"
23			#include "NodeData/State/PosVelAtt.hpp"
24			#include "internal/gui/widgets/HelpMarker.hpp"
25
26			using boost::asio::ip::udp;
27
28			namespace NAV::experimental
29			{
30
31		✗	SkydelNetworkStream::SkydelNetworkStream()
32		✗	: Imu(typeStatic()), _senderEndpoint(udp::v4(), 4444), _socket(_ioservice, _senderEndpoint)
33			{
34		✗	_onlyRealTime = true;
35		✗	_hasConfig = true;
36		✗	_guiConfigDefaultWindowSize = { 305, 70 };
37
38		✗	CreateOutputPin("ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() });
39		✗	CreateOutputPin("PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() });
40		✗	}
41
42		✗	SkydelNetworkStream::~SkydelNetworkStream()
43			{
44			LOG_TRACE("{}: called", nameId());
45		✗	}
46
47		✗	std::string SkydelNetworkStream::typeStatic()
48			{
49		✗	return "SkydelNetworkStream";
50			}
51
52		✗	std::string SkydelNetworkStream::type() const
53			{
54		✗	return typeStatic();
55			}
56
57		✗	std::string SkydelNetworkStream::category()
58			{
59		✗	return "Data Provider";
60			}
61
62		✗	void SkydelNetworkStream::guiConfig()
63			{
64		✗	std::string str;
65
66		✗	if (_startCounter < _startNow)
67			{
68		✗	str = "(loading)";
69			}
70			else
71			{
72		✗	std::ostringstream strs;
73		✗	strs << _dataRate;
74		✗	str = strs.str();
75		✗	}
76
77		✗	ImGui::LabelText(str.c_str(), "data rate [Hz]");
78		✗	ImGui::SameLine();
79		✗	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.");
80		✗	}
81
82		✗	bool SkydelNetworkStream::resetNode()
83			{
84		✗	return true;
85			}
86
87		✗	void SkydelNetworkStream::do_receive()
88			{
89		✗	_socket.async_receive_from(
90		✗	boost::asio::buffer(_data, _maxLength), _senderEndpoint,
91		✗	[this](boost::system::error_code errorRcvd, std::size_t bytesRcvd) {
92		✗	if ((!errorRcvd) && (bytesRcvd > 0))
93			{
94			// Splitting the incoming string analogous to 'ImuFile.cpp'
95		✗	std::stringstream lineStream(std::string(_data.begin(), _data.end()));
96		✗	std::string cell;
97		✗	auto obsG = std::make_shared<PosVelAtt>();
98		✗	auto obs = std::make_shared<ImuObs>(this->_imuPos);
99
100			// Inits for simulated measurement variables
101		✗	uint64_t timeSinceStartup = 0;
102
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		✗	timeSinceStartup = static_cast<uint64_t>(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 (_lastMessageTime)
200			{
201			// FIXME: This seems like a bug in clang-tidy. Check if it is working in future versions of clang-tidy
202			// NOLINTNEXTLINE(hicpp-use-nullptr, modernize-use-nullptr)
203		✗	if (timeSinceStartup - _lastMessageTime >= static_cast<uint64_t>(1.5 / _dataRate * 1e9))
204			{
205		✗	LOG_WARN("{}: Potentially lost a message. Previous message was at {} and current message at {} which is a time difference of {} seconds.", nameId(),
206			_lastMessageTime, timeSinceStartup, static_cast<double>(timeSinceStartup - _lastMessageTime) * 1e-9);
207			}
208			}
209		✗	_lastMessageTime = timeSinceStartup;
210
211		✗	this->invokeCallbacks(OUTPUT_PORT_INDEX_GNSS_OBS, obsG);
212		✗	this->invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs);
213
214			// Data rate (for visualization in GUI)
215		✗	_packageCount++;
216
217		✗	if (_startCounter < _startNow)
218			{
219		✗	_packageCount = 0;
220		✗	_startCounter++;
221			}
222
223		✗	if (_packageCount == 1)
224			{
225		✗	_startPoint = std::chrono::steady_clock::now();
226			}
227		✗	else if (_packageCount == _packagesNumber)
228			{
229		✗	std::chrono::duration<double> elapsed_seconds = std::chrono::steady_clock::now() - _startPoint;
230		✗	_dataRate = static_cast<double>(_packagesNumber - 1) / elapsed_seconds.count();
231
232			// Dynamic adaptation of data rate to a human-readable display update rate in GUI (~ 1 Hz)
233		✗	if ((_dataRate > 2) && (_dataRate < 1001)) // restriction on 'reasonable' sensor data rates (Skydel max. is 1000 Hz)
234			{
235		✗	_packagesNumber = static_cast<int>(_dataRate);
236			}
237		✗	else if (_dataRate >= 1001)
238			{
239		✗	_packagesNumber = 1000;
240			}
241
242		✗	_packageCount = 0;
243
244			LOG_DATA("Elapsed Seconds = {}", elapsed_seconds.count());
245			LOG_DATA("SkydelNetworkStream: dataRate = {}", _dataRate);
246			}
247		✗	}
248			else
249			{
250		✗	LOG_ERROR("Error receiving the network stream from Skydel");
251			}
252
253		✗	if (!_stop)
254			{
255		✗	do_receive();
256			}
257			});
258		✗	}
259
260		✗	bool SkydelNetworkStream::initialize()
261			{
262			LOG_TRACE("{}: called", nameId());
263
264		✗	_stop = false;
265		✗	_packageCount = 0;
266		✗	_startCounter = 0;
267		✗	_packagesNumber = 2;
268
269		✗	_lastMessageTime = 0;
270
271		✗	do_receive();
272
273		✗	if (_isStartup)
274			{
275		✗	_testThread = std::thread([this]() {
276		✗	_ioservice.run();
277		✗	});
278			}
279			else
280			{
281		✗	_testThread = std::thread([this]() {
282		✗	_ioservice.restart();
283		✗	_ioservice.run();
284		✗	});
285			}
286
287		✗	_isStartup = false;
288
289		✗	return true;
290			}
291
292		✗	void SkydelNetworkStream::deinitialize()
293			{
294			LOG_TRACE("{}: called", nameId());
295
296		✗	_stop = true;
297		✗	_ioservice.stop();
298		✗	_testThread.join();
299		✗	}
300
301			} // namespace NAV::experimental
302