From 786fdbe9a4d6a45d9becd51356e2030044d73931 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 17 Jun 2021 13:25:40 +0200
Subject: [PATCH] Add Sensor::printState()
---
include/core/sensor/sensor_base.h | 2 ++
src/sensor/sensor_base.cpp | 31 +++++++++++++++++++++++++++++++
2 files changed, 33 insertions(+)
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index f4cae62c3..b80ac75c3 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 0d2d54638..e78904988 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
--
GitLab