Add Key frame assert to FrameBase::link

src/frame/frame_base.cpp

@@ -287,6 +287,7 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
 {
     assert(!is_removing_ && "linking a removed frame");
     assert(this->getTrajectory() == nullptr && "linking an already linked frame");
+    assert(isKey() && "Trying to link a non keyframe");
 
     if(_trj_ptr)
     {

test/gtest_has_state_blocks.cpp

@@ -55,7 +55,7 @@ TEST_F(HasStateBlocksTest, Notifications_setKey_add)
     Notification n;
     ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));
 
-    // F0->link(problem->getTrajectory());
+    ASSERT_DEATH(F0->link(problem->getTrajectory()), "Assertion `isKey()");
 
     // ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n));