Merge branch 'mutex_state_block' into 'master'

Mutex state block

See merge request mobile_robotics/wolf!205

src/state_block.h

@@ -14,6 +14,7 @@ class LocalParametrizationBase;
 
 //std includes
 #include <iostream>
+#include <mutex>
 
 
 namespace wolf {
@@ -48,9 +49,12 @@ public:
 
         NodeBaseWPtr node_ptr_; //< pointer to the wolf Node owning this StateBlock
 
-        bool fixed_; ///< Key to indicate whether the state is fixed or not
+        std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
 
+        std::atomic<int> state_size_; ///< State vector size
         Eigen::VectorXs state_; ///< State vector storing the state values
+        mutable std::mutex mut_state_; ///< State vector mutex
+
         LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
 
     public:
@@ -70,6 +74,11 @@ public:
          **/
         StateBlock(const Eigen::VectorXs& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
 
+        ///< Explicitly not copyable/movable
+        StateBlock(const StateBlock& o) = delete;
+        StateBlock(StateBlock&& o) = delete;
+        StateBlock& operator=(const StateBlock& o) = delete;
+
         /** \brief Destructor
          **/
         virtual ~StateBlock();
@@ -132,6 +141,7 @@ inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalP
 //        notifications_{Notification::ADD},
         node_ptr_(), // nullptr
         fixed_(_fixed),
+        state_size_(_state.size()),
         state_(_state),
         local_param_ptr_(_local_param_ptr)
 {
@@ -142,6 +152,7 @@ inline StateBlock::StateBlock(const unsigned int _size, bool _fixed, LocalParame
 //        notifications_{Notification::ADD},
         node_ptr_(), // nullptr
         fixed_(_fixed),
+        state_size_(_size),
         state_(Eigen::VectorXs::Zero(_size)),
         local_param_ptr_(_local_param_ptr)
 {
@@ -156,31 +167,38 @@ inline StateBlock::~StateBlock()
 
 inline Eigen::VectorXs StateBlock::getState() const
 {
+    std::lock_guard<std::mutex> lock(mut_state_);
     return state_;
 }
 
 inline void StateBlock::setState(const Eigen::VectorXs& _state)
 {
     assert(_state.size() == state_.size());
-    state_ = _state;
+
+    {
+      std::lock_guard<std::mutex> lock(mut_state_);
+      state_ = _state;
+      state_size_ = state_.size();
+    }
+
     addNotification(Notification::STATE_UPDATE);
 }
 
 inline unsigned int StateBlock::getSize() const
 {
-    return state_.size();
+    return state_size_.load();
 }
 
 inline Size StateBlock::getLocalSize() const
 {
     if(local_param_ptr_)
         return local_param_ptr_->getLocalSize();
-    return state_.size();
+    return getSize();
 }
 
 inline bool StateBlock::isFixed() const
 {
-    return fixed_;
+    return fixed_.load();
 }
 
 inline void StateBlock::fix()
@@ -195,7 +213,7 @@ inline void StateBlock::unfix()
 
 inline void StateBlock::setFixed(bool _fixed)
 {
-    fixed_ = _fixed;
+    fixed_.store(_fixed);
     addNotification(Notification::FIX_UPDATE);
 }