0.5.0
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
41
45
46namespace NAV
47{
48/// @brief Calculates differences between signals
49class Combiner : public Node, public CommonLog
50{
51 public:
52 /// @brief Default constructor
53 Combiner();
54 /// @brief Destructor
55 ~Combiner() override;
56 /// @brief Copy constructor
57 Combiner(const Combiner&) = delete;
58 /// @brief Move constructor
59 Combiner(Combiner&&) = delete;
60 /// @brief Copy assignment operator
61 Combiner& operator=(const Combiner&) = delete;
62 /// @brief Move assignment operator
64
65 /// @brief String representation of the Class Type
66 [[nodiscard]] static std::string typeStatic();
67
68 /// @brief String representation of the Class Type
69 [[nodiscard]] std::string type() const override;
70
71 /// @brief String representation of the Class Category
72 [[nodiscard]] static std::string category();
73
74 /// @brief ImGui config window which is shown on double click
75 /// @attention Don't forget to set _hasConfig to true in the constructor of the node
76 void guiConfig() override;
77
78 /// @brief Saves the node into a json object
79 [[nodiscard]] json save() const override;
80
81 /// @brief Restores the node from a json object
82 /// @param[in] j Json object with the node state
83 void restore(const json& j) override;
84
85 private:
86 constexpr static size_t OUTPUT_PORT_INDEX_DYN_DATA = 0; ///< @brief Flow (DynamicData)
87
88 /// Possible data identifiers to connect
104
105 /// Combination of data
107 {
108 /// Term of a combination equation
109 struct Term
110 {
111 double factor = 1.0; ///< Factor to multiply the term with
112 size_t pinIndex = 0; ///< Pin Index
113 std::variant<size_t, std::string> dataSelection = size_t(0); ///< Data Index or Data identifier
114
115 PolynomialRegressor<double> polyReg{ 1, 2 }; ///< Polynomial Regressor to interpolate data
116 ScrollingBuffer<std::shared_ptr<const NodeData>> rawData{ 2 }; ///< Last raw data to add if we send
117
118 /// @brief Get a string description of the combination
119 /// @param node Combiner node pointer
120 /// @param descriptors Data descriptors
121 [[nodiscard]] std::string description(const Combiner* node, const std::vector<std::string>& descriptors) const
122 {
123 if (std::holds_alternative<size_t>(dataSelection) && std::get<size_t>(dataSelection) < descriptors.size())
124 {
125 return fmt::format("{} {} ({})", factor == 1.0 ? "+" : (factor == -1.0 ? "-" : fmt::format("{:.2f}", factor)),
126 descriptors.at(std::get<size_t>(dataSelection)), node->inputPins.at(pinIndex).name);
127 }
128 if (std::holds_alternative<std::string>(dataSelection))
129 {
130 return fmt::format("{} {} ({})", factor == 1.0 ? "+" : (factor == -1.0 ? "-" : fmt::format("{:.2f}", factor)),
131 std::get<std::string>(dataSelection), node->inputPins.at(pinIndex).name);
132 }
133 return fmt::format("N/A ({})", node->inputPins.at(pinIndex).name);
134 }
135 };
136
137 /// List of terms making up the combination
138 std::vector<Term> terms{ Term{ .factor = +1.0, .pinIndex = 0 },
139 Term{ .factor = -1.0, .pinIndex = 1 } };
140
141 /// @brief Get a string description of the combination
142 /// @param node Combiner node pointer
143 [[nodiscard]] std::string description(const Combiner* node) const
144 {
145 std::string desc;
146 for (const auto& term : terms)
147 {
148 auto descriptors = node->getDataDescriptors(term.pinIndex);
149 auto termDescription = term.description(node, descriptors);
150
151 if (!desc.empty())
152 {
153 if (termDescription.starts_with("+ ") || termDescription.starts_with("- "))
154 {
155 desc += " ";
156 }
157 else
158 {
159 desc += " + ";
160 }
161 }
162 desc += termDescription;
163 }
164
165 return desc;
166 }
167 };
168
169 /// Combinations to calculate
170 std::vector<Combination> _combinations{ Combination() };
171
172 /// Pin data
173 struct PinData
174 {
175 /// Time of the last observation processed
177 /// Min time between messages
178 double minTimeStep = std::numeric_limits<double>::infinity();
179 /// Extra data descriptors for dynamic data
180 std::vector<std::string> dynDataDescriptors;
181 };
182
183 /// Data per pin
184 std::vector<PinData> _pinData;
185
186 /// Reference pin
187 size_t _refPinIdx = 0;
188
189 /// Output missing combinations with NaN instead of removing
191
192 bool _noOutputIfTimeDiffLarge = true; ///< Wether to not output a term if the interpolation time point is too far away
193 double _maxTimeDiffMultiplierFrequency = 2.1; ///< Multiply frequency with this to get maximum allowed time difference to interpolate to
194
195 bool _noOutputIfTimeStepLarge = true; ///< Wether to not output a term if the time step to interpolate in between is large
196 double _maxTimeStepMultiplierFrequency = 5.0; ///< Multiply frequency with this to get maximum allowed time step of incoming observations
197
198 /// Send request information
200 {
201 size_t combIndex = 0; ///< Combination Index
202 std::unordered_set<size_t> termIndices; ///< Term indices, which are already calculated
203 double result = 0.0; ///< Calculation result
204 bool termNullopt = false; ///< True if one of the terms values returned std::nullopt
205 std::vector<std::pair<std::string, std::shared_ptr<const NodeData>>> rawData; ///< List of the raw data of all terms contributing to the result
206 };
207
208 /// Chronological list of send request
209 std::map<InsTime, std::vector<SendRequest>> _sendRequests;
210
211 /// @brief Function to call to add a new pin
212 /// @param[in, out] node Pointer to this node
213 static void pinAddCallback(Node* node);
214 /// @brief Function to call to delete a pin
215 /// @param[in, out] node Pointer to this node
216 /// @param[in] pinIdx Input pin index to delete
217 static void pinDeleteCallback(Node* node, size_t pinIdx);
218
219 /// @brief Dynamic input pins
220 /// @attention This should always be the last variable in the header, because it accesses others through the function callbacks
222
223 /// @brief Initialize the node
224 bool initialize() override;
225
226 /// @brief Deinitialize the node
227 void deinitialize() override;
228
229 /// @brief Returns a list of descriptors for the pin
230 /// @param pinIndex Pin Index to look for the descriptor
231 [[nodiscard]] std::vector<std::string> getDataDescriptors(size_t pinIndex) const;
232
233 /// @brief Receive Data Function
234 /// @param[in] queue Queue with all the received data messages
235 /// @param[in] pinIdx Index of the pin the data is received on
236 void receiveData(InputPin::NodeDataQueue& queue, size_t pinIdx);
237
238 /// @brief Write info to a json object
239 /// @param[out] j Json output
240 /// @param[in] data Object to read info from
241 friend void to_json(json& j, const Combination& data);
242 /// @brief Read info from a json object
243 /// @param[in] j Json variable to read info from
244 /// @param[out] data Output object
245 friend void from_json(const json& j, Combination& data);
246 /// @brief Write info to a json object
247 /// @param[out] j Json output
248 /// @param[in] data Object to read info from
249 friend void to_json(json& j, const Combination::Term& data);
250 /// @brief Read info from a json object
251 /// @param[in] j Json variable to read info from
252 /// @param[out] data Output object
253 friend void from_json(const json& j, Combination::Term& data);
254};
255
256} // 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.
RTK Node/Algorithm output.
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:86
Combiner & operator=(Combiner &&)=delete
Move assignment operator.
gui::widgets::DynamicInputPins _dynamicInputPins
Dynamic input pins.
Definition Combiner.hpp:221
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:193
Combiner(const Combiner &)=delete
Copy constructor.
static std::vector< std::string > _dataIdentifier
Possible data identifiers to connect.
Definition Combiner.hpp:89
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:190
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:196
bool _noOutputIfTimeStepLarge
Wether to not output a term if the time step to interpolate in between is large.
Definition Combiner.hpp:195
size_t _refPinIdx
Reference pin.
Definition Combiner.hpp:187
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:209
static std::string typeStatic()
String representation of the Class Type.
Definition Combiner.cpp:97
std::vector< PinData > _pinData
Data per pin.
Definition Combiner.hpp:184
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:192
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:170
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.
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:110
double factor
Factor to multiply the term with.
Definition Combiner.hpp:111
ScrollingBuffer< std::shared_ptr< const NodeData > > rawData
Last raw data to add if we send.
Definition Combiner.hpp:116
std::string description(const Combiner *node, const std::vector< std::string > &descriptors) const
Get a string description of the combination.
Definition Combiner.hpp:121
std::variant< size_t, std::string > dataSelection
Data Index or Data identifier.
Definition Combiner.hpp:113
PolynomialRegressor< double > polyReg
Polynomial Regressor to interpolate data.
Definition Combiner.hpp:115
Combination of data.
Definition Combiner.hpp:107
std::vector< Term > terms
List of terms making up the combination.
Definition Combiner.hpp:138
std::string description(const Combiner *node) const
Get a string description of the combination.
Definition Combiner.hpp:143
double minTimeStep
Min time between messages.
Definition Combiner.hpp:178
InsTime lastTime
Time of the last observation processed.
Definition Combiner.hpp:176
std::vector< std::string > dynDataDescriptors
Extra data descriptors for dynamic data.
Definition Combiner.hpp:180
Send request information.
Definition Combiner.hpp:200
double result
Calculation result.
Definition Combiner.hpp:203
std::unordered_set< size_t > termIndices
Term indices, which are already calculated.
Definition Combiner.hpp:202
bool termNullopt
True if one of the terms values returned std::nullopt.
Definition Combiner.hpp:204
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:205
size_t combIndex
Combination Index.
Definition Combiner.hpp:201
Inputs pins which can be added dynamically.