0.4.1
Loading...
Searching...
No Matches
Combiner.hpp
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/// @file Combiner.hpp
10/// @brief Calculates differences between signals
11/// @author T. Topp (topp@ins.uni-stuttgart.de)
12/// @date 2024-01-29
13
14#pragma once
15
16#include <limits>
17#include <memory>
18#include <unordered_set>
19#include <map>
20
22#include "NodeData/NodeData.hpp"
25
27
40
44
45namespace NAV
46{
47/// @brief Calculates differences between signals
48class Combiner : public Node, public CommonLog
49{
50 public:
51 /// @brief Default constructor
52 Combiner();
53 /// @brief Destructor
54 ~Combiner() override;
55 /// @brief Copy constructor
56 Combiner(const Combiner&) = delete;
57 /// @brief Move constructor
58 Combiner(Combiner&&) = delete;
59 /// @brief Copy assignment operator
60 Combiner& operator=(const Combiner&) = delete;
61 /// @brief Move assignment operator
63
64 /// @brief String representation of the Class Type
65 [[nodiscard]] static std::string typeStatic();
66
67 /// @brief String representation of the Class Type
68 [[nodiscard]] std::string type() const override;
69
70 /// @brief String representation of the Class Category
71 [[nodiscard]] static std::string category();
72
73 /// @brief ImGui config window which is shown on double click
74 /// @attention Don't forget to set _hasConfig to true in the constructor of the node
75 void guiConfig() override;
76
77 /// @brief Saves the node into a json object
78 [[nodiscard]] json save() const override;
79
80 /// @brief Restores the node from a json object
81 /// @param[in] j Json object with the node state
82 void restore(const json& j) override;
83
84 private:
85 constexpr static size_t OUTPUT_PORT_INDEX_DYN_DATA = 0; ///< @brief Flow (DynamicData)
86
87 /// Possible data identifiers to connect
102
103 /// Combination of data
105 {
106 /// Term of a combination equation
107 struct Term
108 {
109 double factor = 1.0; ///< Factor to multiply the term with
110 size_t pinIndex = 0; ///< Pin Index
111 std::variant<size_t, std::string> dataSelection = size_t(0); ///< Data Index or Data identifier
112
113 PolynomialRegressor<double> polyReg{ 1, 2 }; ///< Polynomial Regressor to interpolate data
114 ScrollingBuffer<std::shared_ptr<const NodeData>> rawData{ 2 }; ///< Last raw data to add if we send
115
116 /// @brief Get a string description of the combination
117 /// @param node Combiner node pointer
118 /// @param descriptors Data descriptors
119 [[nodiscard]] std::string description(const Combiner* node, const std::vector<std::string>& descriptors) const
120 {
121 if (std::holds_alternative<size_t>(dataSelection) && std::get<size_t>(dataSelection) < descriptors.size())
122 {
123 return fmt::format("{} {} ({})", factor == 1.0 ? "+" : (factor == -1.0 ? "-" : fmt::format("{:.2f}", factor)),
124 descriptors.at(std::get<size_t>(dataSelection)), node->inputPins.at(pinIndex).name);
125 }
126 if (std::holds_alternative<std::string>(dataSelection))
127 {
128 return fmt::format("{} {} ({})", factor == 1.0 ? "+" : (factor == -1.0 ? "-" : fmt::format("{:.2f}", factor)),
129 std::get<std::string>(dataSelection), node->inputPins.at(pinIndex).name);
130 }
131 return fmt::format("N/A ({})", node->inputPins.at(pinIndex).name);
132 }
133 };
134
135 /// List of terms making up the combination
136 std::vector<Term> terms{ Term{ .factor = +1.0, .pinIndex = 0 },
137 Term{ .factor = -1.0, .pinIndex = 1 } };
138
139 /// @brief Get a string description of the combination
140 /// @param node Combiner node pointer
141 [[nodiscard]] std::string description(const Combiner* node) const
142 {
143 std::string desc;
144 for (const auto& term : terms)
145 {
146 auto descriptors = node->getDataDescriptors(term.pinIndex);
147 auto termDescription = term.description(node, descriptors);
148
149 if (!desc.empty())
150 {
151 if (termDescription.starts_with("+ ") || termDescription.starts_with("- "))
152 {
153 desc += " ";
154 }
155 else
156 {
157 desc += " + ";
158 }
159 }
160 desc += termDescription;
161 }
162
163 return desc;
164 }
165 };
166
167 /// Combinations to calculate
168 std::vector<Combination> _combinations{ Combination() };
169
170 /// Pin data
171 struct PinData
172 {
173 /// Time of the last observation processed
175 /// Min time between messages
176 double minTimeStep = std::numeric_limits<double>::infinity();
177 /// Extra data descriptors for dynamic data
178 std::vector<std::string> dynDataDescriptors;
179 };
180
181 /// Data per pin
182 std::vector<PinData> _pinData;
183
184 /// Reference pin
185 size_t _refPinIdx = 0;
186
187 /// Output missing combinations with NaN instead of removing
189
190 bool _noOutputIfTimeDiffLarge = true; ///< Wether to not output a term if the interpolation time point is too far away
191 double _maxTimeDiffMultiplierFrequency = 2.1; ///< Multiply frequency with this to get maximum allowed time difference to interpolate to
192
193 bool _noOutputIfTimeStepLarge = true; ///< Wether to not output a term if the time step to interpolate in between is large
194 double _maxTimeStepMultiplierFrequency = 5.0; ///< Multiply frequency with this to get maximum allowed time step of incoming observations
195
196 /// Send request information
198 {
199 size_t combIndex = 0; ///< Combination Index
200 std::unordered_set<size_t> termIndices; ///< Term indices, which are already calculated
201 double result = 0.0; ///< Calculation result
202 bool termNullopt = false; ///< True if one of the terms values returned std::nullopt
203 std::vector<std::pair<std::string, std::shared_ptr<const NodeData>>> rawData; ///< List of the raw data of all terms contributing to the result
204 };
205
206 /// Chronological list of send request
207 std::map<InsTime, std::vector<SendRequest>> _sendRequests;
208
209 /// @brief Function to call to add a new pin
210 /// @param[in, out] node Pointer to this node
211 static void pinAddCallback(Node* node);
212 /// @brief Function to call to delete a pin
213 /// @param[in, out] node Pointer to this node
214 /// @param[in] pinIdx Input pin index to delete
215 static void pinDeleteCallback(Node* node, size_t pinIdx);
216
217 /// @brief Dynamic input pins
218 /// @attention This should always be the last variable in the header, because it accesses others through the function callbacks
220
221 /// @brief Initialize the node
222 bool initialize() override;
223
224 /// @brief Deinitialize the node
225 void deinitialize() override;
226
227 /// @brief Returns a list of descriptors for the pin
228 /// @param pinIndex Pin Index to look for the descriptor
229 [[nodiscard]] std::vector<std::string> getDataDescriptors(size_t pinIndex) const;
230
231 /// @brief Receive Data Function
232 /// @param[in] queue Queue with all the received data messages
233 /// @param[in] pinIdx Index of the pin the data is received on
234 void receiveData(InputPin::NodeDataQueue& queue, size_t pinIdx);
235
236 /// @brief Write info to a json object
237 /// @param[out] j Json output
238 /// @param[in] data Object to read info from
239 friend void to_json(json& j, const Combination& data);
240 /// @brief Read info from a json object
241 /// @param[in] j Json variable to read info from
242 /// @param[out] data Output object
243 friend void from_json(const json& j, Combination& data);
244 /// @brief Write info to a json object
245 /// @param[out] j Json output
246 /// @param[in] data Object to read info from
247 friend void to_json(json& j, const Combination::Term& data);
248 /// @brief Read info from a json object
249 /// @param[in] j Json variable to read info from
250 /// @param[out] data Output object
251 friend void from_json(const json& j, Combination::Term& data);
252};
253
254} // namespace NAV
Common logging variables like time into run and local positions.
Inputs pins which can be added dynamically.
nlohmann::json json
json namespace
GNSS measurement combinations.
GNSS Observation messages.
Data storage class for simulated IMU observations.
Data storage class for one VectorNavImu observation.
Parent Class for all IMU Observations.
Loosely-coupled Kalman Filter INS/GNSS errors.
Tightly-coupled Kalman Filter INS/GNSS errors.
The class is responsible for all time-related tasks.
Data storage class for one KVH Imu observation.
Abstract NodeData Class.
Node Class.
Polynomial curve fitting.
Position, Velocity and Attitude Storage Class.
RTKLIB Pos Observation Class.
A buffer which is overwriting itself from the start when full.
SPP Algorithm output.
Unordered map wrapper.
Binary Outputs from VectorNav Sensors.
static void pinAddCallback(Node *node)
Function to call to add a new pin.
Definition Combiner.cpp:427
bool initialize() override
Initialize the node.
Definition Combiner.cpp:459
static constexpr size_t OUTPUT_PORT_INDEX_DYN_DATA
Flow (DynamicData)
Definition Combiner.hpp:85
Combiner & operator=(Combiner &&)=delete
Move assignment operator.
gui::widgets::DynamicInputPins _dynamicInputPins
Dynamic input pins.
Definition Combiner.hpp:219
friend void from_json(const json &j, Combination &data)
Read info from a json object.
Definition Combiner.cpp:74
std::vector< std::string > getDataDescriptors(size_t pinIndex) const
Returns a list of descriptors for the pin.
Definition Combiner.cpp:490
double _maxTimeDiffMultiplierFrequency
Multiply frequency with this to get maximum allowed time difference to interpolate to.
Definition Combiner.hpp:191
Combiner(const Combiner &)=delete
Copy constructor.
static std::vector< std::string > _dataIdentifier
Possible data identifiers to connect.
Definition Combiner.hpp:88
void guiConfig() override
ImGui config window which is shown on double click.
Definition Combiner.cpp:112
void deinitialize() override
Deinitialize the node.
Definition Combiner.cpp:485
bool _outputMissingAsNaN
Output missing combinations with NaN instead of removing.
Definition Combiner.hpp:188
static void pinDeleteCallback(Node *node, size_t pinIdx)
Function to call to delete a pin.
Definition Combiner.cpp:434
void receiveData(InputPin::NodeDataQueue &queue, size_t pinIdx)
Receive Data Function.
Definition Combiner.cpp:505
double _maxTimeStepMultiplierFrequency
Multiply frequency with this to get maximum allowed time step of incoming observations.
Definition Combiner.hpp:194
bool _noOutputIfTimeStepLarge
Wether to not output a term if the time step to interpolate in between is large.
Definition Combiner.hpp:193
size_t _refPinIdx
Reference pin.
Definition Combiner.hpp:185
static std::string category()
String representation of the Class Category.
Definition Combiner.cpp:107
Combiner(Combiner &&)=delete
Move constructor.
std::string type() const override
String representation of the Class Type.
Definition Combiner.cpp:102
std::map< InsTime, std::vector< SendRequest > > _sendRequests
Chronological list of send request.
Definition Combiner.hpp:207
static std::string typeStatic()
String representation of the Class Type.
Definition Combiner.cpp:97
std::vector< PinData > _pinData
Data per pin.
Definition Combiner.hpp:182
Combiner()
Default constructor.
Definition Combiner.cpp:79
void restore(const json &j) override
Restores the node from a json object.
Definition Combiner.cpp:413
bool _noOutputIfTimeDiffLarge
Wether to not output a term if the interpolation time point is too far away.
Definition Combiner.hpp:190
friend void to_json(json &j, const Combination &data)
Write info to a json object.
Definition Combiner.cpp:65
~Combiner() override
Destructor.
Definition Combiner.cpp:92
Combiner & operator=(const Combiner &)=delete
Copy assignment operator.
json save() const override
Saves the node into a json object.
Definition Combiner.cpp:397
std::vector< Combination > _combinations
Combinations to calculate.
Definition Combiner.hpp:168
CommonLog(const CommonLog &)=delete
Copy constructor.
static std::string type()
Returns the type of the data class.
static std::string type()
Returns the type of the data class.
Definition GnssObs.hpp:150
static std::string type()
Returns the type of the data class.
static std::string type()
Returns the type of the data class.
static std::string type()
Returns the type of the data class.
Definition ImuObs.hpp:33
TsDeque< std::shared_ptr< const NAV::NodeData > > NodeDataQueue
Node data queue type.
Definition Pin.hpp:707
static std::string type()
Returns the type of the data class.
static std::string type()
Returns the type of the data class.
The class is responsible for all time-related tasks.
Definition InsTime.hpp:710
static std::string type()
Returns the type of the data class.
Definition KvhObs.hpp:39
Node(std::string name)
Constructor.
Definition Node.cpp:30
std::vector< InputPin > inputPins
List of input pins.
Definition Node.hpp:397
Polynomial Curve Fitting.
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 type()
Returns the type of the data class.
A buffer which is overwriting itself from the start when full.
static std::string type()
Returns the type of the data class.
static std::string type()
Returns the type of the data class.
Term of a combination equation.
Definition Combiner.hpp:108
double factor
Factor to multiply the term with.
Definition Combiner.hpp:109
ScrollingBuffer< std::shared_ptr< const NodeData > > rawData
Last raw data to add if we send.
Definition Combiner.hpp:114
std::string description(const Combiner *node, const std::vector< std::string > &descriptors) const
Get a string description of the combination.
Definition Combiner.hpp:119
std::variant< size_t, std::string > dataSelection
Data Index or Data identifier.
Definition Combiner.hpp:111
PolynomialRegressor< double > polyReg
Polynomial Regressor to interpolate data.
Definition Combiner.hpp:113
Combination of data.
Definition Combiner.hpp:105
std::vector< Term > terms
List of terms making up the combination.
Definition Combiner.hpp:136
std::string description(const Combiner *node) const
Get a string description of the combination.
Definition Combiner.hpp:141
double minTimeStep
Min time between messages.
Definition Combiner.hpp:176
InsTime lastTime
Time of the last observation processed.
Definition Combiner.hpp:174
std::vector< std::string > dynDataDescriptors
Extra data descriptors for dynamic data.
Definition Combiner.hpp:178
Send request information.
Definition Combiner.hpp:198
double result
Calculation result.
Definition Combiner.hpp:201
std::unordered_set< size_t > termIndices
Term indices, which are already calculated.
Definition Combiner.hpp:200
bool termNullopt
True if one of the terms values returned std::nullopt.
Definition Combiner.hpp:202
std::vector< std::pair< std::string, std::shared_ptr< const NodeData > > > rawData
List of the raw data of all terms contributing to the result.
Definition Combiner.hpp:203
size_t combIndex
Combination Index.
Definition Combiner.hpp:199
Inputs pins which can be added dynamically.