Merge branch 'state_blocks' into 'master'

State blocks

See merge request mobile_robotics/wolf!212

src/problem.cpp

@@ -379,8 +379,7 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
     _state_ptr->addNotification(StateBlock::Notification::ADD);
 
     // Push all notifications from SB to problem
-    for (auto notif : _state_ptr->getNotifications())
-        notifyStateBlock(_state_ptr, notif);
+    notifyStateBlock(_state_ptr);
 
     return _state_ptr;
 }
@@ -394,10 +393,10 @@ void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
 
     // Add remove notification
     _state_ptr->addNotification(StateBlock::Notification::REMOVE);
-    notifyStateBlock(_state_ptr, StateBlock::Notification::REMOVE);
+    notifyStateBlock(_state_ptr);
 }
 
-void Problem::notifyStateBlock(StateBlockPtr _state_ptr, const StateBlock::Notification _type)
+void Problem::notifyStateBlock(StateBlockPtr _state_ptr)
 {
     notified_state_block_list_.push_back(_state_ptr);
     notified_state_block_list_.sort();

src/problem.h

@@ -273,7 +273,7 @@ class Problem : public std::enable_shared_from_this<Problem>
 
         /** \brief Notify State Block change
          */
-        void notifyStateBlock(StateBlockPtr _state_ptr, const StateBlock::Notification _type);
+        void notifyStateBlock(StateBlockPtr _state_ptr);
 
         /** \brief Gets a list of state blocks which state has been changed to be handled by the solver
          */

src/processor_tracker_landmark_polyline.cpp

@@ -956,8 +956,6 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
                 // Unfix origin
                 polyline_ptr->getPPtr()->unfix();
                 polyline_ptr->getOPtr()->unfix();
-                getProblem()->updateFixStateBlockPtr(polyline_ptr->getPPtr());
-                getProblem()->updateFixStateBlockPtr(polyline_ptr->getOPtr());
 
                 // Move origin to B
                 polyline_ptr->getPPtr()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
@@ -989,7 +987,6 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
                 for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++)
                 {
                     polyline_ptr->getPointStateBlockPtr(id)->fix();
-                    getProblem()->updateFixStateBlockPtr(polyline_ptr->getPointStateBlockPtr(id));
                 }
             }
         }

src/state_block.cpp

@@ -16,7 +16,7 @@ void StateBlock::setState(const Eigen::VectorXs& _state, const bool _notify)
     {
         addNotification(StateBlock::Notification::UPDATE_STATE);
         if (getProblem() != nullptr)
-            getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_STATE);
+            getProblem()->notifyStateBlock(shared_from_this());
     }
 }
 
@@ -26,7 +26,7 @@ void StateBlock::setFixed(bool _fixed)
     // Notify
     addNotification(StateBlock::Notification::UPDATE_FIX);
     if (getProblem() != nullptr)
-        getProblem()->notifyStateBlock(shared_from_this(), StateBlock::Notification::UPDATE_FIX);
+        getProblem()->notifyStateBlock(shared_from_this());
 }
 
 void StateBlock::addNotification(const StateBlock::Notification _new_notification)
@@ -67,31 +67,9 @@ StateBlock::Notifications StateBlock::consumeNotifications() const
 
 StateBlock::Notifications StateBlock::getNotifications() const
 {
+    std::lock_guard<std::mutex> lock(notifictions_mut_);
     return notifications_;
 }
 
-void StateBlock::printNotifications() const
-{
-    WOLF_TRACE("SB Notifications for: ", shared_from_this())
-    for (auto notif : notifications_)
-    {
-        switch (notif)
-        {
-            case Notification::ADD:
-                WOLF_TRACE("   ADD")
-                break;
-            case Notification::REMOVE:
-                WOLF_TRACE("   REMOVE")
-                break;
-            case Notification::UPDATE_FIX:
-                WOLF_TRACE("   UPDATE_FIX")
-                break;
-            case Notification::UPDATE_STATE:
-                WOLF_TRACE("   UPDATE_STATE")
-                break;
-        }
-    }
-
-}
 
 }

src/state_block.h

@@ -143,10 +143,6 @@ public:
          **/
         StateBlock::Notifications getNotifications() const;
 
-        /** \brief Print list of notifications
-         **/
-        void printNotifications() const;
-
 };
 
 } // namespace wolf