solver manager as friend to access consume functions

include/base/problem/problem.h

@@ -3,6 +3,7 @@
 
 // Fwd refs
 namespace wolf{
+class SolverManager;
 class HardwareBase;
 class TrajectoryBase;
 class MapBase;
@@ -33,6 +34,7 @@ enum Notification
  */
 class Problem : public std::enable_shared_from_this<Problem>
 {
+    friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
 
     protected:
         HardwareBasePtr     hardware_ptr_;
@@ -252,9 +254,13 @@ class Problem : public std::enable_shared_from_this<Problem>
          */
         void removeStateBlock(StateBlockPtr _state_ptr);
 
-        /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
+        /** \brief Returns the size of the map of state block notification
          */
-        std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
+        SizeStd getStateBlockNotificationMapSize() const;
+
+        /** \brief Returns if the state block has been notified, and the notification via parameter
+         */
+        bool getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const;
 
         /** \brief Notifies a new factor to be added to the solver manager
          */
@@ -264,10 +270,24 @@ class Problem : public std::enable_shared_from_this<Problem>
          */
         void removeFactor(FactorBasePtr _factor_ptr);
 
+        /** \brief Returns the size of the map of factor notification
+         */
+        SizeStd getFactorNotificationMapSize() const;
+
+        /** \brief Returns if the factor has been notified, and the notification via parameter
+         */
+        bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
+
+    protected:
+        /** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
+         */
+        std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
+
         /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
          */
         std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
 
+    public:
         // Print and check ---------------------------------------
         /**
          * \brief print wolf tree
@@ -311,12 +331,25 @@ inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificati
     return std::move(state_block_notification_map_);
 }
 
+inline SizeStd Problem::getStateBlockNotificationMapSize() const
+{
+    std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+    return state_block_notification_map_.size();
+}
+
 inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
 {
     std::lock_guard<std::mutex> lock(mut_factor_notifications_);
     return std::move(factor_notification_map_);
 }
 
+inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
+{
+    std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+    return factor_notification_map_.size();
+}
+
+
 } // namespace wolf
 
 #endif // PROBLEM_H