INSTINCT Code Coverage Report

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