[skip ci] ideas on composite..

include/core/sensor/spec_state_sensor.h

@@ -53,13 +53,7 @@ class SpecStateSensor : public SpecState
         std::string print(const std::string& _spaces = "") const override;
 };
 
-class SpecSensorComposite : public SpecCompositeTemplate<SpecStateSensor>
-{
-    public:
-        using SpecCompositeTemplate::SpecCompositeTemplate;
-
-        SpecComposite getSpecComposite() const;
-};
+typedef TemplateComposite<SpecStateSensor> SpecSensorComposite;
 
 inline bool SpecStateSensor::isDynamic() const { return dynamic_; }
 

include/core/state_block/spec_composite.h

@@ -65,27 +65,38 @@ class SpecState
         virtual std::string print(const std::string& _spaces = "") const;
 };
 
-template <typename SPEC>
-class SpecCompositeTemplate : public std::unordered_map<char, SPEC>
+template <typename T>
+class TemplateComposite : public std::unordered_map<char, T>
 {
     public:
-        using unordered_map<char, SPEC>::unordered_map;
+        using unordered_map<char, T>::unordered_map;
 
-        SpecCompositeTemplate(const YAML::Node& _n);
-        virtual ~SpecCompositeTemplate() = default;
+        TemplateComposite(const YAML::Node& _n);
+        virtual ~TemplateComposite() = default;
 
         VectorComposite getState() const;
 
-        void emplace(char _key, const SPEC& _spec);
+        template <typename OTHER_T>
+        TemplateComposite<OTHER_T> cast() const
+        {
+            TemplateComposite<OTHER_T> casted_composite;
+            for (auto pair : *this)
+            {
+                casted_composite.emplace(pair.first, static_cast<OTHER_T>(pair.second));
+            }
+            return casted_composite;
+        }
+
+        void emplace(char _key, const T& _spec);
 
         std::string print() const;
 };
 
-typedef SpecCompositeTemplate<SpecState> SpecComposite;
+typedef TemplateComposite<SpecState> SpecComposite;
 
 
-template <typename SPEC>
-SpecCompositeTemplate<SPEC>::SpecCompositeTemplate(const YAML::Node& _n)
+template <typename T>
+TemplateComposite<T>::TemplateComposite(const YAML::Node& _n)
 {
     if (not _n.IsMap())
     {
@@ -93,18 +104,18 @@ SpecCompositeTemplate<SPEC>::SpecCompositeTemplate(const YAML::Node& _n)
     }
     for (auto spec_pair : _n)
     {
-        this->emplace(spec_pair.first.as<char>(), SPEC(spec_pair.second));
+        this->emplace(spec_pair.first.as<char>(), T(spec_pair.second));
     }
 }
 
-template <typename SPEC>
-void SpecCompositeTemplate<SPEC>::emplace(char _key, const SPEC& _spec)
+template <typename T>
+void TemplateComposite<T>::emplace(char _key, const T& _spec)
 {
     this->insert({_key,_spec});
 }
 
-template <typename SPEC>
-VectorComposite SpecCompositeTemplate<SPEC>::getState() const
+template <typename T>
+VectorComposite TemplateComposite<T>::getState() const
 {
     VectorComposite states;
     for (auto spec_pair : *this)
@@ -114,8 +125,14 @@ VectorComposite SpecCompositeTemplate<SPEC>::getState() const
     return states;
 }
 
-template <typename SPEC>
-std::string SpecCompositeTemplate<SPEC>::print() const
+// template <typename T, typename OTHER_T>
+// TemplateComposite<OTHER_T> TemplateComposite<T>::cast() const
+// {
+//     return TemplateComposite<OTHER_T>();
+// }
+
+template <typename T>
+std::string TemplateComposite<T>::print() const
 {
     std::string output;
     

src/sensor/sensor_base.cpp

@@ -73,6 +73,10 @@ SensorBase::SensorBase(const std::string& _type,
         if (prior.isDynamic())
             setDriftStd(key, prior.getDriftStd());
     }
+
+    WOLF_INFO(_priors.print());
+    auto base_prior = _priors.cast<SpecState>();
+    WOLF_INFO(base_prior.print());
 }
 
 SensorBase::~SensorBase()

src/sensor/spec_state_sensor.cpp

@@ -27,19 +27,6 @@
 namespace wolf
 {
 
-SpecComposite SpecSensorComposite::getSpecComposite() const
-{
-    SpecComposite spec_composite;
-    for (auto spec_sensor_pair : *this)
-    {
-        spec_composite.emplace(spec_sensor_pair.first, SpecState(spec_sensor_pair.second.getType(), 
-                                                                 spec_sensor_pair.second.getState(),
-                                                                 spec_sensor_pair.second.getMode(),
-                                                                 spec_sensor_pair.second.getNoiseStd()));
-    }
-    return spec_composite;
-}
-
 SpecStateSensor::SpecStateSensor(const std::string&     _type,
                                  const Eigen::VectorXd& _state,
                                  const std::string&     _mode,

src/state_block/has_state_blocks.cpp

@@ -57,10 +57,13 @@ HasStateBlocks::HasStateBlocks(const SpecComposite& _specs)
 
 StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_key, const StateBlockPtr& _sb, ProblemPtr _problem)
 {
-    assert(state_block_map_.count(_sb_key) == 0 && state_block_const_map_.count(_sb_key) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+    assert(state_block_map_.count(_sb_key) == 0 and
+           state_block_const_map_.count(_sb_key) == 0 and
+           "Trying to add a state block with an existing type! Use setStateBlock instead.");
+
     state_block_map_.emplace(_sb_key, _sb);
     state_block_const_map_.emplace(_sb_key, _sb);
-    if (!isInStructure(_sb_key))
+    if (not isInStructure(_sb_key))
         appendToStructure(_sb_key);
 
     // conditionally register to problem