From 818b8f30df3d43a08b8183f410a8c190169f5656 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Feb 2024 15:23:44 -0800 Subject: [PATCH 1/3] Spec 1.12: add _state suffix to state subelements Signed-off-by: Steve Peters --- sdf/1.12/light_state.sdf | 2 +- sdf/1.12/link_state.sdf | 4 ++-- sdf/1.12/model_state.sdf | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/sdf/1.12/light_state.sdf b/sdf/1.12/light_state.sdf index 673efcdb4..b0e02da86 100644 --- a/sdf/1.12/light_state.sdf +++ b/sdf/1.12/light_state.sdf @@ -1,5 +1,5 @@ - + Light state diff --git a/sdf/1.12/link_state.sdf b/sdf/1.12/link_state.sdf index 28fff465d..a04c8f66f 100644 --- a/sdf/1.12/link_state.sdf +++ b/sdf/1.12/link_state.sdf @@ -1,5 +1,5 @@ - + Link state @@ -27,7 +27,7 @@ - + Collision state diff --git a/sdf/1.12/model_state.sdf b/sdf/1.12/model_state.sdf index 3fd296ff2..9a021040d 100644 --- a/sdf/1.12/model_state.sdf +++ b/sdf/1.12/model_state.sdf @@ -1,12 +1,12 @@ - + Model state Name of the model - + Joint angle @@ -22,7 +22,7 @@ - + A nested model state element Name of the model. From dd2054f4f92bbdd15a5d1202c75ca29886df3b7d Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 27 Feb 2024 15:24:22 -0800 Subject: [PATCH 2/3] Rename state elements when converting to 1.12 Update some existing tests to verify conversion logic. Signed-off-by: Steve Peters --- sdf/1.12/1_11.convert | 44 ++++++++++++++++++++++++++++++++ test/integration/frame.cc | 16 ++++++------ test/integration/nested_model.cc | 26 +++++++++---------- 3 files changed, 65 insertions(+), 21 deletions(-) diff --git a/sdf/1.12/1_11.convert b/sdf/1.12/1_11.convert index 380267d62..a9c624420 100644 --- a/sdf/1.12/1_11.convert +++ b/sdf/1.12/1_11.convert @@ -1,2 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/integration/frame.cc b/test/integration/frame.cc index 1ed67cd3a..85128bc86 100644 --- a/test/integration/frame.cc +++ b/test/integration/frame.cc @@ -206,11 +206,11 @@ TEST(Frame, NoFrame) } //////////////////////////////////////// -// Test parsing nested model states +// Test parsing states with //pose/@relative_to TEST(Frame, StateFrame) { std::ostringstream sdfStr; - sdfStr << "" + sdfStr << "" << "" << "" << "" @@ -239,8 +239,8 @@ TEST(Frame, StateFrame) EXPECT_TRUE(worldElem->HasElement("state")); sdf::ElementPtr stateElem = worldElem->GetElement("state"); - EXPECT_TRUE(stateElem->HasElement("model")); - sdf::ElementPtr modelStateElem = stateElem->GetElement("model"); + EXPECT_TRUE(stateElem->HasElement("model_state")); + sdf::ElementPtr modelStateElem = stateElem->GetElement("model_state"); // model EXPECT_TRUE(modelStateElem->HasAttribute("name")); @@ -271,8 +271,8 @@ TEST(Frame, StateFrame) } // link - EXPECT_TRUE(modelStateElem->HasElement("link")); - sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link"); + EXPECT_TRUE(modelStateElem->HasElement("link_state")); + sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link_state"); EXPECT_TRUE(linkStateElem->HasAttribute("name")); EXPECT_EQ(linkStateElem->Get("name"), "my_link"); @@ -286,8 +286,8 @@ TEST(Frame, StateFrame) gz::math::Pose3d(111, 3, 0, 0, 0, 0)); } - EXPECT_TRUE(stateElem->HasElement("light")); - sdf::ElementPtr lightStateElem = stateElem->GetElement("light"); + EXPECT_TRUE(stateElem->HasElement("light_state")); + sdf::ElementPtr lightStateElem = stateElem->GetElement("light_state"); // light EXPECT_TRUE(lightStateElem->HasAttribute("name")); diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc index 0c63cbc43..a9cc3db8a 100644 --- a/test/integration/nested_model.cc +++ b/test/integration/nested_model.cc @@ -111,7 +111,7 @@ TEST(NestedModel, NestedModel) TEST(NestedModel, State) { std::ostringstream sdfStr; - sdfStr << "" + sdfStr << "" << "" << "" << "" @@ -154,9 +154,9 @@ TEST(NestedModel, State) sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world"); EXPECT_TRUE(worldElem->HasElement("state")); sdf::ElementPtr stateElem = worldElem->GetElement("state"); - EXPECT_TRUE(stateElem->HasElement("model")); + EXPECT_TRUE(stateElem->HasElement("model_state")); - sdf::ElementPtr modelStateElem = stateElem->GetElement("model"); + sdf::ElementPtr modelStateElem = stateElem->GetElement("model_state"); // model sdf EXPECT_TRUE(modelStateElem->HasAttribute("name")); @@ -167,8 +167,8 @@ TEST(NestedModel, State) EXPECT_TRUE(!modelStateElem->HasElement("joint")); // link sdf - EXPECT_TRUE(modelStateElem->HasElement("link")); - sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link"); + EXPECT_TRUE(modelStateElem->HasElement("link_state")); + sdf::ElementPtr linkStateElem = modelStateElem->GetElement("link_state"); EXPECT_TRUE(linkStateElem->HasAttribute("name")); EXPECT_EQ(linkStateElem->Get("name"), "link_00"); EXPECT_TRUE(linkStateElem->HasElement("pose")); @@ -185,9 +185,9 @@ TEST(NestedModel, State) gz::math::Pose3d(0, 0.006121, 0, 0, 0, 0)); // nested model sdf - EXPECT_TRUE(modelStateElem->HasElement("model")); + EXPECT_TRUE(modelStateElem->HasElement("model_state")); sdf::ElementPtr nestedModelStateElem = - modelStateElem->GetElement("model"); + modelStateElem->GetElement("model_state"); EXPECT_TRUE(nestedModelStateElem->HasAttribute("name")); EXPECT_EQ(nestedModelStateElem->Get("name"), "model_01"); EXPECT_TRUE(nestedModelStateElem->HasElement("pose")); @@ -196,9 +196,9 @@ TEST(NestedModel, State) EXPECT_TRUE(!nestedModelStateElem->HasElement("joint")); // nested model's link sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("link")); + EXPECT_TRUE(nestedModelStateElem->HasElement("link_state")); sdf::ElementPtr nestedLinkStateElem = - nestedModelStateElem->GetElement("link"); + nestedModelStateElem->GetElement("link_state"); EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name")); EXPECT_EQ(nestedLinkStateElem->Get("name"), "link_01"); EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); @@ -215,8 +215,8 @@ TEST(NestedModel, State) gz::math::Pose3d(0, 0.000674, 0, 0, 0, 0)); // double nested model sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("model")); - nestedModelStateElem = nestedModelStateElem->GetElement("model"); + EXPECT_TRUE(nestedModelStateElem->HasElement("model_state")); + nestedModelStateElem = nestedModelStateElem->GetElement("model_state"); EXPECT_TRUE(nestedModelStateElem->HasAttribute("name")); EXPECT_EQ(nestedModelStateElem->Get("name"), "model_02"); EXPECT_TRUE(nestedModelStateElem->HasElement("pose")); @@ -225,8 +225,8 @@ TEST(NestedModel, State) EXPECT_TRUE(!nestedModelStateElem->HasElement("joint")); // double nested model's link sdf - EXPECT_TRUE(nestedModelStateElem->HasElement("link")); - nestedLinkStateElem = nestedModelStateElem->GetElement("link"); + EXPECT_TRUE(nestedModelStateElem->HasElement("link_state")); + nestedLinkStateElem = nestedModelStateElem->GetElement("link_state"); EXPECT_TRUE(nestedLinkStateElem->HasAttribute("name")); EXPECT_EQ(nestedLinkStateElem->Get("name"), "link_02"); EXPECT_TRUE(nestedLinkStateElem->HasElement("pose")); From e9de65940850f7449c68ae7a072f109f535e5807 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 29 Feb 2024 17:14:29 -0800 Subject: [PATCH 3/3] Add tests showing problems with conversion The Converter currently only acts on the first child element it finds; it doesn't act on all sibling elements. I've added example SDFormat files and tests to illustrate the issue. Signed-off-by: Steve Peters --- test/integration/nested_model.cc | 96 +++++++++++++++++++ test/sdf/state_nested_model_world.sdf | 79 +++++++++++++++ .../state_nested_model_world_insertion.sdf | 90 +++++++++++++++++ 3 files changed, 265 insertions(+) create mode 100644 test/sdf/state_nested_model_world.sdf create mode 100644 test/sdf/state_nested_model_world_insertion.sdf 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 + + + + +