From 991a0a5148df439106c83a8bbd35805d3a860f28 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 19 Dec 2020 01:29:17 +0100
Subject: [PATCH] Fix frame structure upon emplaceFrame
---
src/problem/problem.cpp | 39 ++++++++++++++++++++++++++-------------
test/gtest_problem.cpp | 5 ++++-
2 files changed, 30 insertions(+), 14 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b574f7ac1..d0a311aec 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 f40f2f44f..181fb1ce4 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();
}
--
GitLab