Resolve "HasStateBlocks::addStateBlock() needs to register SB's"

Closes #292 (closed)

test/gtest_has_state_blocks.cpp

@@ -67,6 +67,9 @@ TEST_F(HasStateBlocksTest, Notifications_setKey)
 TEST_F(HasStateBlocksTest, Notifications_addStateBlock)
 {
     Notification n;
+
+    // First add SB, than make KF
+
     ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
 
     F0->link(problem->getTrajectory());
@@ -86,6 +89,23 @@ TEST_F(HasStateBlocksTest, Notifications_addStateBlock)
 
     ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n));
     ASSERT_EQ(n, ADD);
+
+    // first make KF, then add SB
+
+    F1->link(problem->getTrajectory());
+
+    F1->addStateBlock("P", sbp1);
+
+    F1->setKey();
+
+    ASSERT_TRUE(problem->getStateBlockNotification(sbp1, n));
+    ASSERT_FALSE(problem->getStateBlockNotification(sbv1, n));
+
+    F1->addStateBlock("V", sbv1);
+
+    ASSERT_TRUE(problem->getStateBlockNotification(sbv1, n));
+    ASSERT_EQ(n, ADD);
+
 }
 
 int main(int argc, char **argv)