0.5.0
Loading...
Searching...
No Matches
udpRecv.cpp
Go to the documentation of this file.
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
16
18#include "util/Assert.h"
19namespace nm = NAV::NodeManager;
21
22#include "util/Logger.hpp"
26
27#include <cstdint>
28#include <cstring>
29#include <memory>
30#include <boost/system/detail/error_code.hpp>
31
34{
35 LOG_TRACE("{}: called", name);
36
37 _onlyRealTime = true;
38 _hasConfig = true;
39 _guiConfigDefaultWindowSize = { 261, 95 };
40
42}
43
45{
46 LOG_TRACE("{}: called", nameId());
47}
48
50{
51 return "UdpRecv";
52}
53
54std::string NAV::UdpRecv::type() const
55{
56 return typeStatic();
57}
58
60{
61 return "Data Link";
62}
63
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 {
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));
76 {
79 }
81 {
84 }
86 {
87 outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::Pos::type() };
89 }
91 {
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
105 }
106}
107
109{
110 return true;
111}
112
114{
115 LOG_TRACE("{}: called", nameId());
116
117 json j;
118 j["port"] = _port;
119 j["outputType"] = _outputType;
120
121 return j;
122}
123
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 {
138 {
141 }
143 {
146 }
148 {
149 outputPins.at(OUTPUT_PORT_INDEX_NODE_DATA).dataIdentifier = { NAV::Pos::type() };
151 }
153 {
156 }
157 }
158 }
159}
160
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
199{
200 _running = false;
201 _io_context.stop();
202 _recvThread.join();
203 _socket.close();
204
205 LOG_TRACE("{}: called", nameId());
206}
207
209{
210 _socket.async_receive_from(
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 {
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
256 }
257 else if (msgType == UdpUtil::MessageType::PosVel)
258 {
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
279 }
280 else if (msgType == UdpUtil::MessageType::Pos)
281 {
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
298 }
299 else if (msgType == UdpUtil::MessageType::GnssObs)
300 {
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;
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
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
338{
339 switch (value)
340 {
342 return "PosVelAtt";
344 return "PosVel";
346 return "Pos";
348 return "GnssObs";
350 return "";
351 }
352 return "";
353}
Assertion helpers.
#define INS_ASSERT_USER_ERROR(_EXP, _MSG)
Assert function with message.
Definition Assert.h:21
Combo representing an enumeration.
Save/Load the Nodes.
nlohmann::json json
json namespace
GNSS Observation messages.
The class is responsible for all time-related tasks.
Utility class for logging to console and file.
#define LOG_DEBUG
Debug information. Should not be called on functions which receive observations (spamming)
Definition Logger.hpp:67
#define LOG_DATA
All output which occurs repeatedly every time observations are received.
Definition Logger.hpp:29
#define LOG_ERROR
Error occurred, which stops part of the program to work, but not everything.
Definition Logger.hpp:73
#define LOG_TRACE
Detailled info to trace the execution of the program. Should not be called on functions which receive...
Definition Logger.hpp:65
Manages all Nodes.
Position, Velocity and Attitude Storage Class.
Structs identifying a unique satellite.
Utility for the UDP Send and Receive nodes.
static std::string type()
Returns the type of the data class.
Definition GnssObs.hpp:150
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
ImVec2 _guiConfigDefaultWindowSize
Definition Node.hpp:410
std::vector< OutputPin > outputPins
List of output pins.
Definition Node.hpp:399
Node(std::string name)
Constructor.
Definition Node.cpp:30
std::string nameId() const
Node name and id.
Definition Node.cpp:253
std::string name
Name of the Node.
Definition Node.hpp:395
bool _onlyRealTime
Whether the node can run in post-processing or only real-time.
Definition Node.hpp:419
void invokeCallbacks(size_t portIndex, const std::shared_ptr< const NodeData > &data)
Calls all registered callbacks on the specified output port.
Definition Node.cpp:180
bool _hasConfig
Flag if the config window should be shown.
Definition Node.hpp:413
static std::string type()
Returns the type of the data class.
Definition PosVelAtt.hpp:29
static std::string type()
Returns the type of the data class.
Definition PosVel.hpp:27
static std::string type()
Returns the type of the data class.
Definition Pos.hpp:36
static std::string category()
String representation of the Class Category.
Definition udpRecv.cpp:59
UdpRecv()
Default constructor.
Definition udpRecv.cpp:32
void asyncReceive()
Polls the next data.
Definition udpRecv.cpp:208
void guiConfig() override
ImGui config window which is shown on double click.
Definition udpRecv.cpp:64
UdpUtil::MessageType _outputType
The selected output type in the GUI.
Definition udpRecv.hpp:89
bool initialize() override
Initialize the node.
Definition udpRecv.cpp:161
~UdpRecv() override
Destructor.
Definition udpRecv.cpp:44
boost::asio::io_context _io_context
Asynchronous receive fct.
Definition udpRecv.hpp:92
bool _isStartup
Startup handler: used in 'initialize()' to differentiate between startup and re-initialization.
Definition udpRecv.hpp:104
std::array< char, UdpUtil::MAXIMUM_BYTES > _charArray
The array that contains the data from the UDP stream.
Definition udpRecv.hpp:110
bool resetNode() override
Resets the node. Moves the read cursor to the start.
Definition udpRecv.cpp:108
std::thread _recvThread
Receiver thread.
Definition udpRecv.hpp:99
bool _running
Flag that indicates the running data link.
Definition udpRecv.hpp:102
void restore(const json &j) override
Restores the node from a json object.
Definition udpRecv.cpp:124
static constexpr size_t OUTPUT_PORT_INDEX_NODE_DATA
Object (NodeData)
Definition udpRecv.hpp:74
json save() const override
Saves the node into a json object.
Definition udpRecv.cpp:113
std::string type() const override
String representation of the Class Type.
Definition udpRecv.cpp:54
boost::asio::ip::udp::socket _socket
Boost udp socket.
Definition udpRecv.hpp:94
static std::string typeStatic()
String representation of the Class Type.
Definition udpRecv.cpp:49
int _port
UDP port number.
Definition udpRecv.hpp:86
void deinitialize() override
Deinitialize the node.
Definition udpRecv.cpp:198
boost::asio::ip::udp::endpoint _sender_endpoint
Boost udp endpoint.
Definition udpRecv.hpp:96
static float windowFontRatio()
Ratio to multiply for GUI window elements.
ImGui extensions.
bool InputIntL(const char *label, int *v, int v_min, int v_max, int step, int step_fast, ImGuiInputTextFlags flags)
Shows a value limited InputText GUI element for 'int'.
Definition imgui_ex.cpp:242
OutputPin * CreateOutputPin(Node *node, const char *name, Pin::Type pinType, const std::vector< std::string > &dataIdentifier, OutputPin::PinData data=static_cast< void * >(nullptr), int idx=-1)
Create an Output Pin object.
static constexpr size_t POS
Offset of the position.
Definition UdpUtil.hpp:75
static constexpr size_t GPSCYCLE
Offset of the GPS cycle.
Definition UdpUtil.hpp:69
static constexpr size_t GPSTOW
Offset of the GPS tow.
Definition UdpUtil.hpp:73
static constexpr size_t SIZE
Offset of the GNSS data size.
Definition UdpUtil.hpp:82
static constexpr size_t QUAT
Offset of the quaternion.
Definition UdpUtil.hpp:79
static constexpr size_t GNSSDATA
Offset of the GNSS data.
Definition UdpUtil.hpp:84
static constexpr size_t VEL
Offset of the velocity.
Definition UdpUtil.hpp:77
static constexpr size_t GPSWEEK
Offset of the GPS week.
Definition UdpUtil.hpp:71
static constexpr size_t GPSCYCLE
Size of a GPS cycle.
Definition UdpUtil.hpp:41
static constexpr size_t SIZE
Size of the size of a GNSS observation.
Definition UdpUtil.hpp:61
static constexpr size_t QUAT
Size of a Quaternion element.
Definition UdpUtil.hpp:51
static constexpr size_t SINGLE_OBSERVATION_DATA
Size of a single GNSS observation.
Definition UdpUtil.hpp:63
static constexpr size_t MSGTYPE
Size of the message type.
Definition UdpUtil.hpp:39
static constexpr size_t POS
Size of a Pos (LLA)
Definition UdpUtil.hpp:47
static constexpr size_t GPSTOW
Size of a GPS TOW.
Definition UdpUtil.hpp:45
static constexpr size_t GPSWEEK
Size of a GPS week.
Definition UdpUtil.hpp:43
static constexpr size_t VEL
Size of a Vel (NED)
Definition UdpUtil.hpp:49
static constexpr unsigned int MAXIMUM_BYTES
Network data stream maximum buffer size in [bytes] (Maximum payload size of a UDP package)
Definition UdpUtil.hpp:22
static constexpr std::array< int, 2 > PORT_LIMITS
Range a port can be in [0, 2^16-1].
Definition UdpUtil.hpp:24
MessageType
Enum specifying the type of the output message.
Definition UdpUtil.hpp:28
@ PosVel
Extract PosVel data.
Definition UdpUtil.hpp:30
@ GnssObs
Extract GnssObs data.
Definition UdpUtil.hpp:32
@ COUNT
Number of items in the enum.
Definition UdpUtil.hpp:33
@ PosVelAtt
Extract PosVelAtt data.
Definition UdpUtil.hpp:29
@ Pos
Extract Pos data.
Definition UdpUtil.hpp:31
void ApplyChanges()
Signals that there have been changes to the flow.
bool EnumCombo(const char *label, T &enumeration, size_t startIdx=0)
Combo representing an enumeration.
Definition EnumCombo.hpp:30
const char * to_string(gui::widgets::PositionWithFrame::ReferenceFrame refFrame)
Converts the enum to a string.
Stores the satellites observations.
Definition GnssObs.hpp:46
@ Flow
NodeData Trigger.
Definition Pin.hpp:52
Identifies a satellite signal (satellite frequency and number)
Asynchronous data link - receiver node.