From 5341a9d91d19703bf3d69c38f0dd0520e51a4967 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Tue, 5 Apr 2022 09:04:02 +0200
Subject: [PATCH] wip
---
include/core/capture/capture_base.h | 59 +++++++++----
include/core/common/node_base.h | 10 ++-
include/core/common/wolf.h | 20 +++--
include/core/common/wolf_list.h | 36 ++++++++
include/core/state_block/has_state_blocks.h | 91 ++++++++++++++++-----
src/state_block/has_state_blocks.cpp | 3 +-
6 files changed, 173 insertions(+), 46 deletions(-)
create mode 100644 include/core/common/wolf_list.h
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index d73f17d8e..3176f13c2 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -80,34 +80,42 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
void setTimeStamp(const TimeStamp& _ts);
void setTimeStampToNow();
- FrameBasePtr getFrame() const;
+ FrameBaseConstPtr getFrame() const;
+ FrameBasePtr getFrame();
private:
void setFrame(const FrameBasePtr _frm_ptr);
public:
- const FeatureBasePtrList& getFeatureList() const;
+ FeatureBaseConstPtrList getFeatureList() const;
+ FactorBasePtrList getFactorList();
- void getFactorList(FactorBasePtrList& _fac_list) const;
- FactorBasePtrList getFactorList() const;
+ void getFactorList(FeatureBaseConstPtrList& _fac_list) const;
+ void getFactorList(FactorBasePtrList& _fac_list);
- SensorBasePtr getSensor() const;
+ SensorBaseConstPtr getSensor() const;
+ SensorBasePtr getSensor();
virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
private:
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
+
public:
unsigned int getHits() const;
- const FactorBasePtrList& getConstrainedByList() const;
+ FactorBaseConstPtrList getConstrainedByList() const;
+ FactorBasePtrList getConstrainedByList();
bool isConstrainedBy(const FactorBasePtr &_factor) const;
-
// State blocks
- StateBlockPtr getStateBlock(const char& _key) const;
- StateBlockPtr getSensorP() const;
- StateBlockPtr getSensorO() const;
- StateBlockPtr getSensorIntrinsic() const;
+ StateBlockConstPtr getStateBlock(const char& _key) const;
+ StateBlockPtr getStateBlock(const char& _key);
+ StateBlockConstPtr getSensorP() const;
+ StateBlockPtr getSensorP();
+ StateBlockConstPtr getSensorO() const;
+ StateBlockPtr getSensorO();
+ StateBlockConstPtr getSensorIntrinsic() const;
+ StateBlockPtr getSensorIntrinsic();
void fix() override;
void unfix() override;
@@ -158,17 +166,32 @@ std::shared_ptr<classType> CaptureBase::emplace(FrameBasePtr _frm_ptr, T&&... al
return cpt;
}
-inline StateBlockPtr CaptureBase::getSensorP() const
+inline StateBlockConstPtr CaptureBase::getSensorP() const
+{
+ return getStateBlock('P');
+}
+
+inline StateBlockPtr CaptureBase::getSensorP()
{
return getStateBlock('P');
}
-inline StateBlockPtr CaptureBase::getSensorO() const
+inline StateBlockConstPtr CaptureBase::getSensorO() const
+{
+ return getStateBlock('O');
+}
+
+inline StateBlockPtr CaptureBase::getSensorO()
{
return getStateBlock('O');
}
-inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
+inline StateBlockConstPtr CaptureBase::getSensorIntrinsic() const
+{
+ return getStateBlock('I');
+}
+
+inline StateBlockPtr CaptureBase::getSensorIntrinsic()
{
return getStateBlock('I');
}
@@ -178,7 +201,13 @@ inline unsigned int CaptureBase::id() const
return capture_id_;
}
-inline FrameBasePtr CaptureBase::getFrame() const
+inline FrameBaseConstPtr CaptureBase::getFrame() const
+{
+ if(frame_ptr_.expired()) return nullptr;
+ else return frame_ptr_.lock();
+}
+
+inline FrameBasePtr CaptureBase::getFrame()
{
if(frame_ptr_.expired()) return nullptr;
else return frame_ptr_.lock();
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index f450b60de..1f7cf9eaf 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -105,7 +105,8 @@ class NodeBase
void setType(const std::string& _type);
void setName(const std::string& _name);
- ProblemPtr getProblem() const;
+ ProblemConstPtr getProblem() const;
+ ProblemPtr getProblem();
};
} // namespace wolf
@@ -160,7 +161,12 @@ inline void NodeBase::setName(const std::string& _name)
node_name_ = _name;
}
-inline ProblemPtr NodeBase::getProblem() const
+inline ProblemConstPtr NodeBase::getProblem() const
+{
+ return problem_ptr_.lock();
+}
+
+inline ProblemPtr NodeBase::getProblem()
{
return problem_ptr_.lock();
}
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 197a6bfb1..0f4346ff5 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -205,14 +205,18 @@ struct MatrixSizeCheck
#define WOLF_LIST_TYPEDEFS(ClassName) \
class ClassName; \
- typedef std::list<ClassName##Ptr> ClassName##PtrList; \
- typedef ClassName##PtrList::iterator ClassName##Iter; \
- typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; \
- typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
- typedef ClassName##WPtrList::iterator ClassName##WIter; \
- typedef ClassName##WPtrList::const_iterator ClassName##WConstIter; \
- typedef ClassName##WPtrList::reverse_iterator ClassName##WRevIter;
+ typedef std::list<ClassName##Ptr> ClassName##PtrList; \
+ typedef ClassName##PtrList::iterator ClassName##Iter; \
+ typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \
+ typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; \
+ typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
+ typedef ClassName##WPtrList::iterator ClassName##WIter; \
+ typedef ClassName##WPtrList::const_iterator ClassName##WConstIter; \
+ typedef ClassName##WPtrList::reverse_iterator ClassName##WRevIter;
+ typedef std::list<ClassName##ConstPtr> ClassName##ConstPtrList; \
+ typedef ClassName##ConstPtrList::iterator ClassName##ConstIter; \
+ typedef ClassName##ConstPtrList::const_iterator ClassName##ConstConstIter; \
+ typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstRevIter;
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
struct StructName; \
diff --git a/include/core/common/wolf_list.h b/include/core/common/wolf_list.h
new file mode 100644
index 000000000..85af60362
--- /dev/null
+++ b/include/core/common/wolf_list.h
@@ -0,0 +1,36 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef WOLF_LIST_H_
+#define WOLF_LIST_H_
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/utils/check_log.h"
+
+namespace wolf {
+
+template <class T>
+class WolfList
+{
+ std::list<std::shared_pointer<T>> list;
+ std::list<std::shared_pointer<const T>> list_const;
+}
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index f1bcd4519..035ee2072 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -47,14 +47,18 @@ class HasStateBlocks
const StateStructure& getStructure() const { return structure_; }
void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
- bool isInStructure(const char& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
+ bool isInStructure(const char& _sb_type) const { return structure_.find(_sb_type) != std::string::npos; }
const std::unordered_map<char, StateBlockPtr>& getStateBlockMap() const;
- std::vector<StateBlockPtr> getStateBlockVec() const;
+ const std::unordered_map<char, StateBlockPtr>& getStateBlockMap();
+ std::vector<StateBlockConstPtr> getStateBlockVec() const;
+ std::vector<StateBlockPtr> getStateBlockVec();
// Some typical shortcuts -- not all should be coded here, see notes below.
- StateBlockPtr getP() const;
- StateBlockPtr getO() const;
+ StateBlockConstPtr getP() const;
+ StateBlockConstPtr getO() const;
+ StateBlockPtr getP();
+ StateBlockPtr getO();
// These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
virtual void fix();
@@ -65,9 +69,10 @@ class HasStateBlocks
virtual unsigned int removeStateBlock(const char& _sb_type);
bool hasStateBlock(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool hasStateBlock(const StateBlockPtr& _sb) const;
- StateBlockPtr getStateBlock(const char& _sb_type) const;
bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb);
bool stateBlockKey(const StateBlockPtr& _sb, char& _key) const;
+ StateBlockConstPtr getStateBlock(const char& _sb_type) const;
+ StateBlockPtr getStateBlock(const char& _sb_type);
std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb) const;
// Emplace derived state blocks (angle, quaternion, etc).
@@ -103,6 +108,7 @@ class HasStateBlocks
private:
StateStructure structure_;
std::unordered_map<char, StateBlockPtr> state_block_map_;
+ std::unordered_map<char, StateBlockConstPtr> state_block_const_map_;
};
@@ -117,7 +123,8 @@ namespace wolf{
inline HasStateBlocks::HasStateBlocks() :
structure_(std::string("")),
- state_block_map_()
+ state_block_map_(),
+ state_block_const_map_()
{
//
}
@@ -127,14 +134,19 @@ inline HasStateBlocks::~HasStateBlocks()
//
}
-inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap() const
+inline const std::unordered_map<char, StateBlockConstPtr>& HasStateBlocks::getStateBlockMap() const
+{
+ return state_block_const_map_;
+}
+
+inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap()
{
return state_block_map_;
}
-inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
+inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const
{
- std::vector<StateBlockPtr> sbv;
+ std::vector<StateBlockConstPtr> sbv;
for (auto& key : structure_)
{
sbv.push_back(getStateBlock(key));
@@ -142,20 +154,26 @@ inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec() const
return sbv;
}
-//inline unsigned int HasStateBlocks::removeStateBlock(const char _sb_type)
-//{
-// return removeStateBlock(std::string(1, _sb_type));
-//}
+inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
+{
+ std::vector<StateBlockPtr> sbv;
+ for (auto& key : structure_)
+ {
+ sbv.push_back(getStateBlock(key));
+ }
+ return sbv;
+}
inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
{
return state_block_map_.erase(_sb_type);
+ return state_block_const_map_.erase(_sb_type);
}
template<typename SB, typename ... Args>
inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
addStateBlock(_sb_type, sb, _problem);
@@ -166,7 +184,7 @@ inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_typ
template<typename ... Args>
inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
auto sb = std::make_shared<StateBlock>(std::forward<Args>(_args_of_base_state_block_constructor)...);
addStateBlock(_sb_type, sb, _problem);
@@ -177,12 +195,22 @@ inline StateBlockPtr HasStateBlocks::emplaceStateBlock(const char& _sb_type, Pro
inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
{
assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
- assert ( state_block_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
+ assert ( state_block_map_.count(_sb_type) > 0 && state_block_const_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
state_block_map_.at(_sb_type) = _sb;
+ state_block_const_map_.at(_sb_type) = _sb;
return true; // success
}
-inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
+inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
+{
+ auto it = state_block_const_map_.find(_sb_type);
+ if (it != state_block_const_map_.end())
+ return it->second;
+ else
+ return nullptr;
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type)
{
auto it = state_block_map_.find(_sb_type);
if (it != state_block_map_.end())
@@ -191,12 +219,22 @@ inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type) c
return nullptr;
}
-inline wolf::StateBlockPtr HasStateBlocks::getP() const
+inline wolf::StateBlockConstPtr HasStateBlocks::getP() const
{
return getStateBlock('P');
}
-inline wolf::StateBlockPtr HasStateBlocks::getO() const
+inline wolf::StateBlockPtr HasStateBlocks::getP()
+{
+ return getStateBlock('P');
+}
+
+inline wolf::StateBlockConstPtr HasStateBlocks::getO() const
+{
+ return getStateBlock('O');
+}
+
+inline wolf::StateBlockPtr HasStateBlocks::getO()
{
return getStateBlock('O');
}
@@ -358,7 +396,20 @@ inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) co
return size;
}
-inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+inline std::unordered_map<char, StateBlockConstPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb) const
+{
+ const auto& it = std::find_if(state_block_const_map_.begin(),
+ state_block_const_map_.end(),
+ [_sb](const std::pair<char, StateBlockPtr>& pair)
+ {
+ return pair.second == _sb;
+ }
+ );
+
+ return it;
+}
+
+inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb)
{
const auto& it = std::find_if(state_block_map_.begin(),
state_block_map_.end(),
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index 36cada937..9cb6546c0 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -27,8 +27,9 @@ namespace wolf
StateBlockPtr HasStateBlocks::addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem)
{
- assert(state_block_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
+ assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
state_block_map_.emplace(_sb_type, _sb);
+ state_block_const_map_.emplace(_sb_type, _sb);
if (!isInStructure(_sb_type))
appendToStructure(_sb_type);
--
GitLab