Add Sensor::printState()

include/core/sensor/sensor_base.h

@@ -244,6 +244,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
                                  bool state_blocks,
                                  std::ostream& stream ,
                                  std::string _tabs = "") const;
+        void printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
+
         void print(int depth, //
                    bool constr_by, //
                    bool metric, //

src/sensor/sensor_base.cpp

@@ -429,6 +429,36 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
     printState(_metric, _state_blocks, _stream, _tabs);
 }
 
+void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+    if (_metric && _state_blocks)
+    {
+        for (const auto &key : getStructure())
+        {
+            auto sb = getStateBlockDynamic(key);
+            if (sb)
+                _stream << _tabs << "  " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
+                        << std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl;
+        }
+    }
+    else if (_metric)
+    {
+        _stream << _tabs << "  " << (isFixed() ? "Fix" : "Est") << ",\t x = ( " << std::setprecision(3)
+                << getStateVector().transpose() << " )" << std::endl;
+    }
+    else if (_state_blocks)
+    {
+        _stream << _tabs << "  " << "sb:";
+        for (const auto &key : getStructure())
+        {
+            const auto &sb = getStateBlockDynamic(key);
+            if (sb)
+                _stream << "    " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
+        }
+        _stream << std::endl;
+    }
+}
+
 void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
 {
     printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
@@ -509,4 +539,5 @@ bool SensorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool
     return _log.is_consistent_;
 }
 
+
 } // namespace wolf