INSTINCT Code Coverage Report

Directory: src/
File: Nodes/DataLink/udpRecv.cpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 13 177 7.3%
Functions: 4 16 25.0%
Branches: 10 320 3.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 "udpRecv.hpp"
10
11 #include "Nodes/DataLink/UdpUtil.hpp"
12 #include "NodeData/GNSS/GnssObs.hpp"
13 #include "NodeData/State/PosVelAtt.hpp"
14 #include "Navigation/GNSS/Core/SatelliteIdentifier.hpp"
15 #include "Navigation/Time/InsTime.hpp"
16
17 #include "util/Assert.h"
18 #include "internal/FlowManager.hpp"
19
20 #include "util/Logger.hpp"
21 #include "internal/gui/widgets/imgui_ex.hpp"
22 #include "internal/gui/widgets/EnumCombo.hpp"
23 #include "internal/gui/NodeEditorApplication.hpp"
24
25 #include <cstdint>
26 #include <cstring>
27 #include <memory>
28 #include <boost/system/detail/error_code.hpp>
29
30 114 NAV::UdpRecv::UdpRecv()
31
4/8
✓ 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.
 114 : Node(typeStatic()), _socket(_io_context)
32 {
33 LOG_TRACE("{}: called", name);
34
35 114 _onlyRealTime = true;
36 114 _hasConfig = true;
37 114 _guiConfigDefaultWindowSize = { 261, 95 };
38
39
4/8
✓ Branch 2 taken 114 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 114 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 114 times.
✓ Branch 10 taken 114 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 456 CreateOutputPin("Data", Pin::Type::Flow, { NAV::PosVelAtt::type() });
40 228 }
41
42 228 NAV::UdpRecv::~UdpRecv()
43 {
44 LOG_TRACE("{}: called", nameId());
45 228 }
46
47 228 std::string NAV::UdpRecv::typeStatic()
48 {
49
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 456 return "UdpRecv";
50 }
51
52 ✗ std::string NAV::UdpRecv::type() const
53 {
54 ✗ return typeStatic();
55 }
56
57 114 std::string NAV::UdpRecv::category()
58 {
59
1/2
✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
 228 return "Data Link";
60 }
61
62 ✗ void NAV::UdpRecv::guiConfig()
63 {
64 ✗ ImGui::SetNextItemWidth(150 * gui::NodeEditorApplication::windowFontRatio());
65 ✗ if (ImGui::InputIntL(fmt::format("Port##{}", size_t(id)).c_str(), &_port, UdpUtil::PORT_LIMITS[0], UdpUtil::PORT_LIMITS[1]))
66 {
67 ✗ flow::ApplyChanges();
68 }
69 ✗ ImGui::SetNextItemWidth(150 * gui::NodeEditorApplication::windowFontRatio());
70 ✗ if (gui::widgets::EnumCombo(fmt::format("Output Type##{}", size_t(id)).c_str(), _outputType))
71 {
72 ✗ LOG_DEBUG("{}: Output Type changed to {}", nameId(), to_string(_outputType));
73 ✗ if (_outputType == UdpUtil::MessageType::PosVelAtt)
74 {
75 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::PosVelAtt::type() };
76 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name = NAV::PosVelAtt::type();
77 }
78 ✗ else if (_outputType == UdpUtil::MessageType::PosVel)
79 {
80 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::PosVel::type() };
81 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name = NAV::PosVel::type();
82 }
83 ✗ else if (_outputType == UdpUtil::MessageType::Pos)
84 {
85 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::Pos::type() };
86 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name = NAV::Pos::type();
87 }
88 ✗ else if (_outputType == UdpUtil::MessageType::GnssObs)
89 {
90 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::GnssObs::type() };
91 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name = NAV::GnssObs::type();
92 }
93
94 ✗ for (auto& link : outputPins.front().links)
95 {
96 ✗ if (auto* connectedPin = link.getConnectedPin())
97 {
98 ✗ outputPins.front().recreateLink(*connectedPin);
99 }
100 }
101
102 ✗ flow::ApplyChanges();
103 }
104 ✗ }
105
106 ✗ bool NAV::UdpRecv::resetNode()
107 {
108 ✗ return true;
109 }
110
111 ✗ json NAV::UdpRecv::save() const
112 {
113 LOG_TRACE("{}: called", nameId());
114
115 ✗ json j;
116 ✗ j["port"] = _port;
117 ✗ j["outputType"] = _outputType;
118
119 ✗ return j;
120 ✗ }
121
122 ✗ void NAV::UdpRecv::restore(json const& j)
123 {
124 LOG_TRACE("{}: called", nameId());
125 ✗ if (j.contains("port"))
126 {
127 ✗ j.at("port").get_to(_port);
128 }
129 ✗ if (j.contains("outputType"))
130 {
131 ✗ j.at("outputType").get_to(_outputType);
132
133 ✗ if (!outputPins.empty())
134 {
135 ✗ if (_outputType == UdpUtil::MessageType::PosVelAtt)
136 {
137 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::PosVelAtt::type() };
138 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name = NAV::PosVelAtt::type();
139 }
140 ✗ else if (_outputType == UdpUtil::MessageType::PosVel)
141 {
142 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::PosVel::type() };
143 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name = NAV::PosVel::type();
144 }
145 ✗ else if (_outputType == UdpUtil::MessageType::Pos)
146 {
147 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::Pos::type() };
148 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name = NAV::Pos::type();
149 }
150 ✗ else if (_outputType == UdpUtil::MessageType::GnssObs)
151 {
152 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::GnssObs::type() };
153 ✗ outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name = NAV::GnssObs::type();
154 }
155 }
156 }
157 ✗ }
158
159 ✗ bool NAV::UdpRecv::initialize()
160 {
161 LOG_TRACE("{}: called", nameId());
162
163 try
164 {
165 ✗ _socket = boost::asio::ip::udp::socket(_io_context, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), static_cast<uint16_t>(_port)));
166 }
167 ✗ catch (const std::exception& /* e */)
168 {
169 ✗ LOG_ERROR("{}: Port {} is already in use. Choose a different port for this instance.", nameId(), _port);
170 ✗ return false;
171 ✗ }
172
173 ✗ _running = true;
174
175 ✗ asyncReceive();
176
177 ✗ if (_isStartup)
178 {
179 ✗ _recvThread = std::thread([this]() {
180 ✗ _io_context.run();
181 ✗ });
182 }
183 else
184 {
185 ✗ _recvThread = std::thread([this]() {
186 ✗ _io_context.restart();
187 ✗ _io_context.run();
188 ✗ });
189 }
190
191 ✗ _isStartup = false;
192
193 ✗ return true;
194 }
195
196 ✗ void NAV::UdpRecv::deinitialize()
197 {
198 ✗ _running = false;
199 ✗ _io_context.stop();
200 ✗ _recvThread.join();
201 ✗ _socket.close();
202
203 LOG_TRACE("{}: called", nameId());
204 ✗ }
205
206 ✗ void NAV::UdpRecv::asyncReceive()
207 {
208 ✗ _socket.async_receive_from(
209 ✗ boost::asio::buffer(_charArray, UdpUtil::MAXIMUM_BYTES), _sender_endpoint,
210 ✗ [this](boost::system::error_code errorRcvd, std::size_t bytesRcvd) {
211 ✗ if ((!errorRcvd) && (bytesRcvd > 0))
212 {
213 ✗ UdpUtil::MessageType msgType{};
214 ✗ std::memcpy(&msgType, _charArray.data(), UdpUtil::Size::MSGTYPE);
215 LOG_DATA("{}: Received {} bytes (message type {})", nameId(), bytesRcvd, fmt::underlying(msgType));
216
217 ✗ int32_t gpsCycle{};
218 ✗ int32_t gpsWeek{};
219 ✗ double gpsTow{};
220
221 ✗ std::memcpy(&gpsCycle, _charArray.data() + UdpUtil::Offset::GPSCYCLE, UdpUtil::Size::GPSCYCLE);
222 ✗ std::memcpy(&gpsWeek, _charArray.data() + UdpUtil::Offset::GPSWEEK, UdpUtil::Size::GPSWEEK);
223 ✗ std::memcpy(&gpsTow, _charArray.data() + UdpUtil::Offset::GPSTOW, UdpUtil::Size::GPSTOW);
224
225 ✗ if (msgType == UdpUtil::MessageType::PosVelAtt)
226 {
227 ✗ if (outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name != NAV::PosVelAtt::type())
228 {
229 ✗ LOG_ERROR("{}: Change output type to 'PosVelAtt'!", nameId());
230 ✗ return;
231 }
232 ✗ auto obs = std::make_shared<PosVelAtt>();
233
234 ✗ obs->insTime = InsTime(gpsCycle, gpsWeek, gpsTow);
235
236 // Position in LLA coordinates
237 ✗ Eigen::Vector3d posLLA{};
238 ✗ std::memcpy(posLLA.data(), _charArray.data() + UdpUtil::Offset::POS, UdpUtil::Size::POS);
239
240 // Velocity in local frame
241 ✗ Eigen::Vector3d vel_n{};
242 ✗ std::memcpy(vel_n.data(), _charArray.data() + UdpUtil::Offset::VEL, UdpUtil::Size::VEL);
243
244 // Attitude
245 ✗ Eigen::Quaterniond n_Quat_b{};
246 ✗ std::memcpy(&n_Quat_b.x(), _charArray.data() + UdpUtil::Offset::QUAT, UdpUtil::Size::QUAT);
247 ✗ std::memcpy(&n_Quat_b.y(), _charArray.data() + UdpUtil::Offset::QUAT + UdpUtil::Size::QUAT, UdpUtil::Size::QUAT);
248 ✗ std::memcpy(&n_Quat_b.z(), _charArray.data() + UdpUtil::Offset::QUAT + 2 * UdpUtil::Size::QUAT, UdpUtil::Size::QUAT);
249 ✗ std::memcpy(&n_Quat_b.w(), _charArray.data() + UdpUtil::Offset::QUAT + 3 * UdpUtil::Size::QUAT, UdpUtil::Size::QUAT);
250
251 ✗ obs->setPosVelAtt_n(posLLA, vel_n, n_Quat_b);
252
253 ✗ this->invokeCallbacks(OUTPUT_PORT_INDEX_NODE_DATA, obs);
254 ✗ }
255 ✗ else if (msgType == UdpUtil::MessageType::PosVel)
256 {
257 ✗ if (outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name != NAV::PosVel::type())
258 {
259 ✗ LOG_ERROR("{}: Change output type to 'PosVel'!", nameId());
260 ✗ return;
261 }
262 ✗ auto obs = std::make_shared<PosVel>();
263
264 ✗ obs->insTime = InsTime(gpsCycle, gpsWeek, gpsTow);
265
266 // Position in LLA coordinates
267 ✗ Eigen::Vector3d posLLA{};
268 ✗ std::memcpy(posLLA.data(), _charArray.data() + UdpUtil::Offset::POS, UdpUtil::Size::POS);
269
270 // Velocity in local frame
271 ✗ Eigen::Vector3d vel_n{};
272 ✗ std::memcpy(vel_n.data(), _charArray.data() + UdpUtil::Offset::VEL, UdpUtil::Size::VEL);
273
274 ✗ obs->setPosVel_n(posLLA, vel_n);
275
276 ✗ this->invokeCallbacks(OUTPUT_PORT_INDEX_NODE_DATA, obs);
277 ✗ }
278 ✗ else if (msgType == UdpUtil::MessageType::Pos)
279 {
280 ✗ if (outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name != NAV::Pos::type())
281 {
282 ✗ LOG_ERROR("{}: Change output type to 'Pos'!", nameId());
283 ✗ return;
284 }
285 ✗ auto obs = std::make_shared<Pos>();
286
287 ✗ obs->insTime = InsTime(gpsCycle, gpsWeek, gpsTow);
288
289 // Position in LLA coordinates
290 ✗ Eigen::Vector3d posLLA{};
291 ✗ std::memcpy(posLLA.data(), _charArray.data() + UdpUtil::Offset::POS, UdpUtil::Size::POS);
292
293 ✗ obs->setPosition_lla(posLLA);
294
295 ✗ this->invokeCallbacks(OUTPUT_PORT_INDEX_NODE_DATA, obs);
296 ✗ }
297 ✗ else if (msgType == UdpUtil::MessageType::GnssObs)
298 {
299 ✗ if (outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).name != NAV::GnssObs::type())
300 {
301 ✗ LOG_ERROR("{}: Change output type to 'GnssObs'!", nameId());
302 ✗ return;
303 }
304 ✗ auto gnssObs = std::make_shared<GnssObs>();
305 ✗ gnssObs->insTime = InsTime(gpsCycle, gpsWeek, gpsTow);
306
307 ✗ size_t byteSizeGnssData{};
308 ✗ std::memcpy(&byteSizeGnssData, _charArray.data() + UdpUtil::Offset::SIZE, UdpUtil::Size::SIZE);
309 ✗ size_t sizeGnssData = byteSizeGnssData / UdpUtil::Size::SINGLE_OBSERVATION_DATA;
310 ✗ INS_ASSERT_USER_ERROR(byteSizeGnssData % UdpUtil::Size::SINGLE_OBSERVATION_DATA == 0,
311 "The UdpRecv node received a not dividable amount of bytes for the GnssObs data.");
312 LOG_DATA("{}: {} GNSS signals ({} bytes)", nameId(), sizeGnssData, byteSizeGnssData);
313 ✗ gnssObs->data.resize(sizeGnssData, GnssObs::ObservationData(SatSigId()));
314 ✗ std::memcpy(gnssObs->data.data(), _charArray.data() + UdpUtil::Offset::GNSSDATA, byteSizeGnssData);
315
316 ✗ this->invokeCallbacks(OUTPUT_PORT_INDEX_NODE_DATA, gnssObs);
317 ✗ }
318 else
319 {
320 ✗ LOG_ERROR("{}: Data type not receivable, yet.", nameId());
321 }
322 }
323 else
324 {
325 ✗ LOG_ERROR("{}: Error receiving the UDP network stream: {}, Received bytes: {}", nameId(), errorRcvd.what(), bytesRcvd);
326 }
327
328 ✗ if (_running)
329 {
330 ✗ asyncReceive();
331 }
332 });
333 ✗ }
334
335 ✗ const char* NAV::to_string(NAV::UdpUtil::MessageType value)
336 {
337 ✗ switch (value)
338 {
339 ✗ case NAV::UdpUtil::MessageType::PosVelAtt:
340 ✗ return "PosVelAtt";
341 ✗ case NAV::UdpUtil::MessageType::PosVel:
342 ✗ return "PosVel";
343 ✗ case NAV::UdpUtil::MessageType::Pos:
344 ✗ return "Pos";
345 ✗ case NAV::UdpUtil::MessageType::GnssObs:
346 ✗ return "GnssObs";
347 ✗ case NAV::UdpUtil::MessageType::COUNT:
348 ✗ return "";
349 }
350 ✗ return "";
351 }
352