DAA

src/NodeRegistry.cpp

@@ -198,6 +198,7 @@ std::vector<std::string> 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<SinglePointPositioning>();
     registerNodeType<ImuIntegrator>();
     registerNodeType<LooselyCoupledKF>();
+    registerNodeType<DetectAndAvoid>();
     // registerNodeType<TightlyCoupledKF>();
     registerNodeType<ImuFusion>();
     // Data Provider

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 <imgui.h>
+#include <imgui_internal.h>
+
+#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<const PosVelAtt>(queue.extract_front());
+
+    invokeCallbackWithPosVelAtt(*posVelAtt);
+}
+
+void NAV::DetectAndAvoid::invokeCallbackWithPosVelAtt(const PosVelAtt& posVelAtt)
+{
+    auto posVelAtt_solution = std::make_shared<PosVelAtt>();
+    posVelAtt_solution->insTime = posVelAtt.insTime;
+    invokeCallbacks(OUTPUT_PORT_INDEX_SOLUTION, posVelAtt_solution);
+}

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 ([email protected])
+/// @date 2024-10-25
+
+#pragma once
+
+#include <cstddef>
+#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