diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index b574f7ac1a452b13262a751a43870897720a7198..d0a311aec438e3cc81d526aca264fcadc0f902b8 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -347,6 +347,10 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // state.emplace(key, _frame_state.segment(index, size)); + // append new key to frame structure + if (frame_structure_.find(key) == std::string::npos) // not found + frame_structure_ += std::string(1,key); + index += size; } @@ -356,6 +360,7 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // _time_stamp, _frame_structure, state); + return frm; } @@ -364,36 +369,44 @@ FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // const SizeEigen _dim) { return emplaceFrame(_time_stamp, - _frame_structure, - getState(_time_stamp)); + _frame_structure, + getState(_time_stamp)); } FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // - const StateStructure& _frame_structure, // - const VectorComposite& _frame_state) + const StateStructure& _frame_structure, // + const VectorComposite& _frame_state) { return FrameBase::emplaceFrame<FrameBase>(getTrajectory(), - _time_stamp, - _frame_structure, - _frame_state); + _time_stamp, + _frame_structure, + _frame_state); } FrameBasePtr Problem::emplaceFrame (const TimeStamp& _time_stamp, // const VectorComposite& _frame_state) { + // append new keys to frame structure + for (const auto& pair_key_vec : _frame_state) + { + const auto& key = pair_key_vec.first; + if (frame_structure_.find(key) == std::string::npos) // not found + frame_structure_ += std::string(1,key); + } + return FrameBase::emplaceFrame<FrameBase>(getTrajectory(), - _time_stamp, - getFrameStructure(), - _frame_state); + _time_stamp, + getFrameStructure(), + _frame_state); } FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp, // const Eigen::VectorXd& _frame_state) { return emplaceFrame(_time_stamp, - this->getFrameStructure(), - this->getDim(), - _frame_state); + this->getFrameStructure(), + this->getDim(), + _frame_state); } FrameBasePtr Problem::emplaceFrame(const TimeStamp& _time_stamp) diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index f40f2f44f24521c88bb7d8b8732fb0bcc3c321cb..181fb1ce43bc80a8a04034191973d5ac60635b1e 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -228,6 +228,7 @@ TEST(Problem, emplaceFrame_factory) FrameBasePtr f1 = P->emplaceFrame(1, "PO" , 3, VectorXd(7) ); FrameBasePtr f2 = P->emplaceFrame(2, "POV", 3, VectorXd(10) ); + // check that all frames are effectively in the trajectory ASSERT_EQ(P->getTrajectory()->getFrameMap().size(), (SizeStd) 3); @@ -239,6 +240,8 @@ TEST(Problem, emplaceFrame_factory) ASSERT_EQ(f0->getProblem(), P); ASSERT_EQ(f1->getProblem(), P); ASSERT_EQ(f2->getProblem(), P); + + ASSERT_EQ(P->getFrameStructure(), "POV"); } TEST(Problem, StateBlocks) @@ -565,6 +568,6 @@ TEST(Problem, getState) int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - //::testing::GTEST_FLAG(filter) = "Problem.getState"; +// ::testing::GTEST_FLAG(filter) = "Problem.emplaceFrame_factory"; return RUN_ALL_TESTS(); }