diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index 42b8b03a48b64271319a98a624a7a75684d76218..76df765d20b1f3d83cb5b3646beebe5463c72ef3 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -15,6 +15,10 @@ namespace wolf { +/// State of nodes containing several state blocks +typedef std::unordered_map<std::string, Eigen::VectorXd> State; + + class HasStateBlocks { public: @@ -70,6 +74,8 @@ class HasStateBlocks unsigned int getSize(std::string _sub_structure="") const; unsigned int getLocalSize(std::string _sub_structure="") const; + State getStateComposite(); + private: std::string structure_; std::map<std::string, StateBlockPtr> state_block_map_; @@ -266,6 +272,15 @@ inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) cons return state; } +inline State HasStateBlocks::getStateComposite() +{ + State state; + for (auto& pair_key_kf : state_block_map_) + { + state.emplace(pair_key_kf.first, pair_key_kf.second->getState()); + } + return state; +} inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const { @@ -303,6 +318,5 @@ inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) con return size; } - } // namespace wolf #endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */