Add test for re-notification after solve

src/solver/solver_manager.cpp

@@ -50,7 +50,7 @@ void SolverManager::update()
             }
             else
             {
-                WOLF_DEBUG("Tried to add an already added !");
+                WOLF_DEBUG("Tried to add a StateBlock that was already added !");
             }
         }
         else

test/gtest_has_state_blocks.cpp

@@ -12,6 +12,7 @@
 #include "core/sensor/sensor_base.h"
 #include "core/landmark/landmark_base.h"
 #include "core/state_block/state_quaternion.h"
+#include "core/ceres_wrapper/ceres_manager.h"
 
 using namespace wolf;
 
@@ -108,6 +109,50 @@ TEST_F(HasStateBlocksTest, Notifications_addStateBlock)
 
 }
 
+TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add)
+{
+
+    CeresManagerPtr solver = std::make_shared<CeresManager>(problem);
+
+
+    Notification n;
+
+    // Add SB, make KF
+
+    ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
+
+    F0->link(problem->getTrajectory());
+    F0->addStateBlock("V", sbv0);
+    F0->setKey();
+
+    ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+    ASSERT_EQ(n, ADD);
+
+    // solve. This will clear all notifications
+
+    std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+    ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+    // Notify again the same SB
+
+    problem->notifyStateBlock(sbv0, ADD);
+
+    ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
+    ASSERT_EQ(n, ADD);
+
+    // solve again
+
+    report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+    ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n));
+
+    // Add again the same SB. This should crash
+
+    ASSERT_DEATH( F0->addStateBlock("V", sbv0) , "" );
+
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);