| 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 "internal/NodeManager.hpp" | ||
| 12 | namespace nm = NAV::NodeManager; | ||
| 13 | #include "internal/FlowManager.hpp" | ||
| 14 | |||
| 15 | #include "internal/gui/widgets/imgui_ex.hpp" | ||
| 16 | #include "internal/gui/widgets/HelpMarker.hpp" | ||
| 17 | #include "internal/gui/NodeEditorApplication.hpp" | ||
| 18 | |||
| 19 | #include "util/Time/TimeBase.hpp" | ||
| 20 | |||
| 21 | #include "util/Logger.hpp" | ||
| 22 | |||
| 23 | 112 | NAV::UdpRecv::UdpRecv() | |
| 24 |
4/8✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 112 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 112 times.
✗ Branch 12 not taken.
|
112 | : Node(typeStatic()), _socket(_io_context) |
| 25 | { | ||
| 26 | LOG_TRACE("{}: called", name); | ||
| 27 | |||
| 28 | 112 | _onlyRealTime = true; | |
| 29 | 112 | _hasConfig = true; | |
| 30 | 112 | _guiConfigDefaultWindowSize = { 202, 66 }; | |
| 31 | |||
| 32 |
4/8✓ Branch 2 taken 112 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 112 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 112 times.
✓ Branch 10 taken 112 times.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
448 | nm::CreateOutputPin(this, "PosVelAtt", Pin::Type::Flow, { NAV::PosVelAtt::type() }); |
| 33 | 224 | } | |
| 34 | |||
| 35 | 224 | NAV::UdpRecv::~UdpRecv() | |
| 36 | { | ||
| 37 | LOG_TRACE("{}: called", nameId()); | ||
| 38 | 224 | } | |
| 39 | |||
| 40 | 224 | std::string NAV::UdpRecv::typeStatic() | |
| 41 | { | ||
| 42 |
1/2✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
|
448 | return "UdpRecv"; |
| 43 | } | ||
| 44 | |||
| 45 | ✗ | std::string NAV::UdpRecv::type() const | |
| 46 | { | ||
| 47 | ✗ | return typeStatic(); | |
| 48 | } | ||
| 49 | |||
| 50 | 112 | std::string NAV::UdpRecv::category() | |
| 51 | { | ||
| 52 |
1/2✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
|
224 | return "Data Link"; |
| 53 | } | ||
| 54 | |||
| 55 | ✗ | void NAV::UdpRecv::guiConfig() | |
| 56 | { | ||
| 57 | ✗ | ImGui::SetNextItemWidth(150 * gui::NodeEditorApplication::windowFontRatio()); | |
| 58 | ✗ | if (ImGui::InputIntL(fmt::format("Port##{}", size_t(id)).c_str(), &_port, PORT_LIMITS[0], PORT_LIMITS[1])) | |
| 59 | { | ||
| 60 | ✗ | flow::ApplyChanges(); | |
| 61 | } | ||
| 62 | ✗ | } | |
| 63 | |||
| 64 | ✗ | bool NAV::UdpRecv::resetNode() | |
| 65 | { | ||
| 66 | ✗ | return true; | |
| 67 | } | ||
| 68 | |||
| 69 | ✗ | json NAV::UdpRecv::save() const | |
| 70 | { | ||
| 71 | LOG_TRACE("{}: called", nameId()); | ||
| 72 | |||
| 73 | ✗ | json j; | |
| 74 | ✗ | j["port"] = _port; | |
| 75 | |||
| 76 | ✗ | return j; | |
| 77 | ✗ | } | |
| 78 | |||
| 79 | ✗ | void NAV::UdpRecv::restore(json const& j) | |
| 80 | { | ||
| 81 | LOG_TRACE("{}: called", nameId()); | ||
| 82 | ✗ | if (j.contains("port")) | |
| 83 | { | ||
| 84 | ✗ | j.at("port").get_to(_port); | |
| 85 | } | ||
| 86 | ✗ | } | |
| 87 | |||
| 88 | ✗ | bool NAV::UdpRecv::initialize() | |
| 89 | { | ||
| 90 | LOG_TRACE("{}: called", nameId()); | ||
| 91 | |||
| 92 | try | ||
| 93 | { | ||
| 94 | ✗ | _socket = boost::asio::ip::udp::socket(_io_context, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), static_cast<uint16_t>(_port))); | |
| 95 | } | ||
| 96 | ✗ | catch (const std::exception& /* e */) | |
| 97 | { | ||
| 98 | ✗ | LOG_ERROR("{}: Port {} is already in use. Choose a different port for this instance.", nameId(), _port); | |
| 99 | ✗ | return false; | |
| 100 | ✗ | } | |
| 101 | |||
| 102 | ✗ | _running = true; | |
| 103 | |||
| 104 | ✗ | asyncReceive(); | |
| 105 | |||
| 106 | ✗ | if (_isStartup) | |
| 107 | { | ||
| 108 | ✗ | _recvThread = std::thread([this]() { | |
| 109 | ✗ | _io_context.run(); | |
| 110 | ✗ | }); | |
| 111 | } | ||
| 112 | else | ||
| 113 | { | ||
| 114 | ✗ | _recvThread = std::thread([this]() { | |
| 115 | ✗ | _io_context.restart(); | |
| 116 | ✗ | _io_context.run(); | |
| 117 | ✗ | }); | |
| 118 | } | ||
| 119 | |||
| 120 | ✗ | _isStartup = false; | |
| 121 | |||
| 122 | ✗ | return true; | |
| 123 | } | ||
| 124 | |||
| 125 | ✗ | void NAV::UdpRecv::deinitialize() | |
| 126 | { | ||
| 127 | ✗ | _running = false; | |
| 128 | ✗ | _io_context.stop(); | |
| 129 | ✗ | _recvThread.join(); | |
| 130 | ✗ | _socket.close(); | |
| 131 | |||
| 132 | LOG_TRACE("{}: called", nameId()); | ||
| 133 | ✗ | } | |
| 134 | |||
| 135 | ✗ | void NAV::UdpRecv::asyncReceive() | |
| 136 | { | ||
| 137 | ✗ | _socket.async_receive_from( | |
| 138 | ✗ | boost::asio::buffer(_data, _maxBytes), _sender_endpoint, | |
| 139 | ✗ | [this](boost::system::error_code errorRcvd, std::size_t bytesRcvd) { | |
| 140 | ✗ | if ((!errorRcvd) && (bytesRcvd > 0)) | |
| 141 | { | ||
| 142 | ✗ | auto obs = std::make_shared<PosVelAtt>(); | |
| 143 | |||
| 144 | // Position in LLA coordinates | ||
| 145 | ✗ | Eigen::Vector3d posLLA{ _data.at(0), _data.at(1), _data.at(2) }; | |
| 146 | |||
| 147 | // Velocity in local frame | ||
| 148 | ✗ | Eigen::Vector3d vel_n{ _data.at(3), _data.at(4), _data.at(5) }; | |
| 149 | |||
| 150 | // Attitude | ||
| 151 | ✗ | Eigen::Quaterniond n_Quat_b{}; | |
| 152 | ✗ | n_Quat_b.x() = _data.at(6); | |
| 153 | ✗ | n_Quat_b.y() = _data.at(7); | |
| 154 | ✗ | n_Quat_b.z() = _data.at(8); | |
| 155 | ✗ | n_Quat_b.w() = _data.at(9); | |
| 156 | |||
| 157 | // Time | ||
| 158 | ✗ | auto gpsC = static_cast<int32_t>(_data.at(10)); | |
| 159 | ✗ | auto gpsW = static_cast<int32_t>(_data.at(11)); | |
| 160 | ✗ | auto gpsT = static_cast<long double>(_data.at(12)); | |
| 161 | |||
| 162 | ✗ | obs->setState_n(posLLA, vel_n, n_Quat_b); | |
| 163 | ✗ | obs->insTime = InsTime(gpsC, gpsW, gpsT); | |
| 164 | |||
| 165 | ✗ | this->invokeCallbacks(OUTPUT_PORT_INDEX_NODE_DATA, obs); | |
| 166 | ✗ | } | |
| 167 | else | ||
| 168 | { | ||
| 169 | ✗ | LOG_ERROR("Error receiving the UDP network stream."); | |
| 170 | } | ||
| 171 | |||
| 172 | ✗ | if (_running) | |
| 173 | { | ||
| 174 | ✗ | asyncReceive(); | |
| 175 | } | ||
| 176 | ✗ | }); | |
| 177 | ✗ | } | |
| 178 |