diff --git a/src/NodeRegistry.cpp b/src/NodeRegistry.cpp index 432a9d9c..bf713eae 100644 --- a/src/NodeRegistry.cpp +++ b/src/NodeRegistry.cpp @@ -198,6 +198,7 @@ std::vector NAV::NodeRegistry::GetParentNodeDataTypes(const std::st #include "Nodes/DataProcessor/KalmanFilter/LooselyCoupledKF.hpp" #include "Nodes/DataProcessor/KalmanFilter/TightlyCoupledKF.hpp" #include "Nodes/DataProcessor/SensorCombiner/ImuFusion.hpp" +#include "Nodes/DataProcessor/DetectAndAvoid/DetectAndAvoid.hpp" // Data Provider #include "Nodes/DataProvider/CSV/CsvFile.hpp" #include "Nodes/DataProvider/GNSS/FileReader/RinexNavFile.hpp" @@ -263,6 +264,7 @@ void NAV::NodeRegistry::RegisterNodeTypes() registerNodeType(); registerNodeType(); registerNodeType(); + registerNodeType(); // registerNodeType(); registerNodeType(); // Data Provider diff --git a/src/Nodes/DataProcessor/DetectAndAvoid/DetectAndAvoid.cpp b/src/Nodes/DataProcessor/DetectAndAvoid/DetectAndAvoid.cpp new file mode 100644 index 00000000..15be9f8c --- /dev/null +++ b/src/Nodes/DataProcessor/DetectAndAvoid/DetectAndAvoid.cpp @@ -0,0 +1,102 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#include "DetectAndAvoid.hpp" + +#include +#include + +#include "NodeData/State/PosVelAtt.hpp" +#include "internal/FlowManager.hpp" +#include "internal/Node/Pin.hpp" +#include "internal/NodeManager.hpp" +namespace nm = NAV::NodeManager; +#include "internal/gui/widgets/HelpMarker.hpp" +#include "internal/gui/widgets/imgui_ex.hpp" +#include "internal/gui/widgets/InputWithUnit.hpp" +#include "internal/gui/NodeEditorApplication.hpp" +#include "NodeRegistry.hpp" +#include "util/Logger.hpp" + +NAV::DetectAndAvoid::DetectAndAvoid() + : Node(typeStatic()) +{ + LOG_TRACE("{}: called", name); + + _hasConfig = true; + _guiConfigDefaultWindowSize = { 822, 936 }; // TODO: Adapt this, once the node is finished + + nm::CreateInputPin(this, "PosVelAttIn", Pin::Type::Flow, { NAV::PosVelAtt::type() }, &DetectAndAvoid::recvPosVelAtt); + nm::CreateOutputPin(this, "PosVelAttOut", Pin::Type::Flow, { PosVelAtt::type() }); +} + +NAV::DetectAndAvoid::~DetectAndAvoid() +{ + LOG_TRACE("{}: called", nameId()); +} + +std::string NAV::DetectAndAvoid::typeStatic() +{ + return "DetectAndAvoid"; +} + +std::string NAV::DetectAndAvoid::type() const +{ + return typeStatic(); +} + +std::string NAV::DetectAndAvoid::category() +{ + return "Data Processor"; +} + +void NAV::DetectAndAvoid::guiConfig() +{ + [[maybe_unused]] float itemWidth = 470 * gui::NodeEditorApplication::windowFontRatio(); // TODO: Adapt this, once the node is finished + [[maybe_unused]] float unitWidth = 180 * gui::NodeEditorApplication::windowFontRatio(); // TODO: Adapt this, once the node is finished +} + +json NAV::DetectAndAvoid::save() const +{ + LOG_TRACE("{}: called", nameId()); + + json j; + + return j; +} + +void NAV::DetectAndAvoid::restore(json const& /* j */) // TODO: Adapt this, once the node is finished +{ + LOG_TRACE("{}: called", nameId()); +} + +bool NAV::DetectAndAvoid::initialize() +{ + LOG_TRACE("{}: called", nameId()); + + return true; +} + +void NAV::DetectAndAvoid::deinitialize() +{ + LOG_TRACE("{}: called", nameId()); +} + +void NAV::DetectAndAvoid::recvPosVelAtt(InputPin::NodeDataQueue& queue, size_t /* pinIdx */) +{ + auto posVelAtt = std::static_pointer_cast(queue.extract_front()); + + invokeCallbackWithPosVelAtt(*posVelAtt); +} + +void NAV::DetectAndAvoid::invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt) +{ + auto posVelAtt_solution = std::make_shared(); + posVelAtt_solution->insTime = posVelAtt.insTime; + invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, posVelAtt_solution); +} \ No newline at end of file diff --git a/src/Nodes/DataProcessor/DetectAndAvoid/DetectAndAvoid.hpp b/src/Nodes/DataProcessor/DetectAndAvoid/DetectAndAvoid.hpp new file mode 100644 index 00000000..cbbf404c --- /dev/null +++ b/src/Nodes/DataProcessor/DetectAndAvoid/DetectAndAvoid.hpp @@ -0,0 +1,77 @@ +// This file is part of INSTINCT, the INS Toolkit for Integrated +// Navigation Concepts and Training by the Institute of Navigation of +// the University of Stuttgart, Germany. +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at https://mozilla.org/MPL/2.0/. + +/// @file DetectAndAvoid.hpp +/// @brief Detect-And-Avoid class +/// @author M. Maier (marcel.maier@ins.uni-stuttgart.de) +/// @date 2024-10-25 + +#pragma once + +#include +#include "internal/Node/Node.hpp" +#include "NodeData/State/PosVelAtt.hpp" + +namespace NAV +{ +/// @brief Detect-And-Avoid class +class DetectAndAvoid : public Node +{ + public: + /// @brief Default constructor + DetectAndAvoid(); + /// @brief Destructor + ~DetectAndAvoid() override; + /// @brief Copy constructor + DetectAndAvoid(const DetectAndAvoid&) = delete; + /// @brief Move constructor + DetectAndAvoid(DetectAndAvoid&&) = delete; + /// @brief Copy assignment operator + DetectAndAvoid& operator=(const DetectAndAvoid&) = delete; + /// @brief Move assignment operator + DetectAndAvoid& operator=(DetectAndAvoid&&) = delete; + /// @brief String representation of the class type + [[nodiscard]] static std::string typeStatic(); + + /// @brief String representation of the class type + [[nodiscard]] std::string type() const override; + + /// @brief String representation of the class category + [[nodiscard]] static std::string category(); + + /// @brief ImGui config window which is shown on double click + /// @attention Don't forget to set _hasConfig to true in the constructor of the node + void guiConfig() override; + + /// @brief Saves the node into a json object + [[nodiscard]] json save() const override; + + /// @brief Restores the node from a json object + /// @param[in] j Json object with the node state + void restore(const json& j) override; + + private: + constexpr static size_t INPUT_PORT_INDEX_POS_VEL_ATT = 0; ///< @brief Flow (PosVelAtt) + constexpr static size_t OUTPUT_PORT_INDEX_SOLUTION = 0; ///< @brief Flow (DaaSolution) + + /// @brief Initialize the node + bool initialize() override; + + /// @brief Deinitialize the node + void deinitialize() override; + + /// @brief Invoke the callback with a PosVelAtt solution + /// @param[in] posVelAtt PosVelAtt solution + void invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt); + + /// @brief Receive Function for the PosVelAtt observation + /// @param[in] queue Queue with all the received data messages + /// @param[in] pinIdx Index of the pin the data is received on + void recvPosVelAtt(InputPin::NodeDataQueue& queue, size_t pinIdx); +}; +} // namespace NAV \ No newline at end of file