diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index f4cae62c31f2c8389107388bdebedbec72a68b5b..b80ac75c31d27014cb1affc8d9012e60b6476017 100644 --- a/include/core/sensor/sensor_base.h +++ b/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, // diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 0d2d54638d145f333efa0e349990beda54d1b17e..e78904988b99b50695d10a44d2249b9dffc31874 100644 --- a/src/sensor/sensor_base.cpp +++ b/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