diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index a9cc3db8a..b72dc857c 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -243,6 +243,102 @@ TEST(NestedModel, State) gz::math::Pose3d(0, 0, 0, 0, 0, 0)); } +//////////////////////////////////////// +// Test parsing nested model states +TEST(NestedModel, StateSiblingsConversion1_12) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "state_nested_model_world.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + + // load the state sdf + EXPECT_TRUE(root.Element()->HasElement("world")); + sdf::ElementPtr worldElem = root.Element()->GetElement("world"); + + // Confirm that regular model elements were not migrated + EXPECT_FALSE(worldElem->HasElement("model_state")); + + // Confirm that model and link elements in state were converted to + // model_state and link_state + EXPECT_TRUE(worldElem->HasElement("state")); + sdf::ElementPtr stateElem = worldElem->GetElement("state"); + EXPECT_TRUE(stateElem->HasElement("model_state")) + << stateElem->ToString(""); + EXPECT_FALSE(stateElem->HasElement("model")) + << stateElem->ToString(""); + + // model sdf + sdf::ElementPtr modelStateElem = stateElem->GetElement("model_state"); + EXPECT_TRUE(modelStateElem->HasAttribute("name")); + EXPECT_EQ(modelStateElem->Get("name"), "top_level_model"); + EXPECT_TRUE(modelStateElem->HasElement("link_state")) + << modelStateElem->ToString(""); + EXPECT_TRUE(modelStateElem->HasElement("model_state")) + << modelStateElem->ToString(""); + EXPECT_FALSE(modelStateElem->HasElement("link")) + << modelStateElem->ToString(""); + EXPECT_FALSE(modelStateElem->HasElement("model")) + << modelStateElem->ToString(""); +} + +//////////////////////////////////////// +// Test parsing nested model states +TEST(NestedModel, StateInsertionsConversion1_12) +{ + const std::string testFile = + sdf::testing::TestFile("sdf", "state_nested_model_world_insertion.sdf"); + + // Load the SDF file + sdf::Root root; + auto errors = root.Load(testFile); + EXPECT_TRUE(errors.empty()); + + // load the state sdf + EXPECT_TRUE(root.Element()->HasElement("world")); + sdf::ElementPtr worldElem = root.Element()->GetElement("world"); + + // Confirm that regular model elements were not migrated + EXPECT_FALSE(worldElem->HasElement("model_state")); + + // Confirm that model and link elements in state were converted to + // model_state and link_state + EXPECT_TRUE(worldElem->HasElement("state")); + sdf::ElementPtr stateElem = worldElem->GetElement("state"); + EXPECT_TRUE(stateElem->HasElement("model_state")) + << stateElem->ToString(""); + EXPECT_FALSE(stateElem->HasElement("model")) + << stateElem->ToString(""); + + // model sdf + sdf::ElementPtr modelStateElem = stateElem->GetElement("model_state"); + EXPECT_TRUE(modelStateElem->HasAttribute("name")); + EXPECT_EQ(modelStateElem->Get("name"), "top_level_model"); + EXPECT_TRUE(modelStateElem->HasElement("link_state")) + << modelStateElem->ToString(""); + EXPECT_TRUE(modelStateElem->HasElement("model_state")) + << modelStateElem->ToString(""); + EXPECT_FALSE(modelStateElem->HasElement("link")) + << modelStateElem->ToString(""); + EXPECT_FALSE(modelStateElem->HasElement("model")) + << modelStateElem->ToString(""); + + // insertions + EXPECT_TRUE(stateElem->HasElement("insertions")); + sdf::ElementPtr insertionsElem = stateElem->GetElement("insertions"); + // confirm that //insertions/model and link tags were not converted + EXPECT_FALSE(insertionsElem->HasElement("model_state")); + EXPECT_TRUE(insertionsElem->HasElement("model")); + sdf::ElementPtr insertedModelElem = insertionsElem->GetElement("model"); + EXPECT_TRUE(insertedModelElem->HasAttribute("name")); + EXPECT_EQ(insertedModelElem->Get("name"), "unit_box"); + EXPECT_FALSE(insertedModelElem->HasElement("link_state")); + EXPECT_TRUE(insertedModelElem->HasElement("link")); +} + //////////////////////////////////////// // Test parsing models with joints nested via // Confirm that joint axis rotation is handled differently for 1.4 and 1.5+ diff --git a/test/sdf/state_nested_model_world.sdf b/test/sdf/state_nested_model_world.sdf new file mode 100644 index 000000000..e4a177b3c --- /dev/null +++ b/test/sdf/state_nested_model_world.sdf @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + parent + child + + 1 0 0 + + + + 0 0 -9.8 + + 0 689000000 + 0 732670244 + 1709252095 646182343 + 689 + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + + + diff --git a/test/sdf/state_nested_model_world_insertion.sdf b/test/sdf/state_nested_model_world_insertion.sdf new file mode 100644 index 000000000..d1affbd75 --- /dev/null +++ b/test/sdf/state_nested_model_world_insertion.sdf @@ -0,0 +1,90 @@ + + + + + + + + + + + + + + + + + + + + + + + + parent + child + + 1 0 0 + + + + 0 0 -9.8 + + 0 689000000 + 0 732670244 + 1709252095 646182343 + 689 + + + -0.541687 -2.44761 0.5 0 -0 0 + + + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + + + 0 0 -2.329 0 -0 0 + + + 0 0 -2.329 0 -0 0 + + + + + + -0.5417 -2.448 0.5 0 -0 0 + + + + +