Skip to content

Commit

Permalink
updated nested model test
Browse files Browse the repository at this point in the history 
Signed-off-by: Jenn Nguyen <jenn@openrobotics.org>
  • Loading branch information
@jennuine
jennuine committed Mar 9, 2021
1 parent e128b71 commit a1006ad
Show file tree
Hide file tree
Showing 4 changed files with 145 additions and 58 deletions.
23 changes: 10 additions & 13 deletions src/Converter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,18 +264,18 @@ void Converter::Unnest(tinyxml2::XMLElement *_elem)

// std::cout << "copy:\n" << PrintElement(copy->ToElement()) << std::endl;

tinyxml2::XMLElement *elem = _elem->FirstChildElement(), *nextElem = nullptr;
while (elem)
tinyxml2::XMLElement *nextElem = nullptr;
for(tinyxml2::XMLElement *elem = _elem->FirstChildElement();
elem;
elem = nextElem)
{
nextElem = elem->NextSiblingElement();

std::string elemName = elem->Name();

// skip element if not one of the following
if (elemName != "frame" && elemName != "joint"
&& elemName != "link" && elemName != "model")
{
elem = nextElem;
continue;
}

Expand All @@ -292,11 +292,9 @@ void Converter::Unnest(tinyxml2::XMLElement *_elem)
Unnest(elem);

// std::cout << "unnested model:\n" << PrintElement(elem) << std::endl;
_elem->InsertEndChild(elem);
break;
}

elem = nextElem;
continue;
}

Expand All @@ -305,8 +303,7 @@ void Converter::Unnest(tinyxml2::XMLElement *_elem)
newModel->SetAttribute("name", newModelName.c_str());

Converter::TupleVector deleteElems =
FindNewModelElements(doc,
copy->ToElement(),
FindNewModelElements(copy->ToElement(),
newModel,
found + 2);

Expand All @@ -331,16 +328,13 @@ void Converter::Unnest(tinyxml2::XMLElement *_elem)
_elem->DeleteChild(e);
}

nextElem = _elem->FirstChildElement();
Unnest(newModel);
}

elem = nextElem;
}
}

/////////////////////////////////////////////////
Converter::TupleVector Converter::FindNewModelElements(
tinyxml2::XMLDocument *_doc,
tinyxml2::XMLElement *_copy,
tinyxml2::XMLElement *_newModel,
const size_t &_newNameIdx)
Expand Down Expand Up @@ -470,7 +464,7 @@ Converter::TupleVector Converter::FindNewModelElements(

// strip new model prefix from //xyz/@expressed_in
std::string axisStr = "axis";
for (int i = 0; i < 2; ++i)
while (true)
{
tinyxml2::XMLElement *axisElem =
elem->FirstChildElement(axisStr.c_str());
Expand All @@ -491,6 +485,9 @@ Converter::TupleVector Converter::FindNewModelElements(
->SetAttribute("expressed_in", expressIn.c_str());
}
}

if (axisStr == "axis2") break;

axisStr += "2";
}
} // joint
Expand Down
4 changes: 1 addition & 3 deletions src/Converter.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,15 +115,13 @@ namespace sdf
using TupleVector = std::vector< std::tuple<std::string, std::string> >;

/// \brief Finds all elements related to the unnested model
/// \param[in] _doc SDF xml doc
/// \param[in] _copy Copy of the entire original element that needs unnested
/// \param[in] _newModel The new unnested model element
/// \param[in] _newNameIdx The index of the new name for child elements
/// \return Vector of tuples containing the element name & attribute name
/// to be deleted from the original element that needs unnested
private:
static TupleVector FindNewModelElements(tinyxml2::XMLDocument *_doc,
tinyxml2::XMLElement *_copy,
static TupleVector FindNewModelElements(tinyxml2::XMLElement *_copy,
tinyxml2::XMLElement *_newModel,
const size_t &_newNameIdx);

Expand Down
84 changes: 84 additions & 0 deletions src/Converter_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2310,6 +2310,90 @@ TEST(Converter, World_17_to_18)
EXPECT_STREQ(convertedElem->Name(), "axis2");
EXPECT_STREQ(convertedElem->FirstChildElement()->Attribute("expressed_in"),
"NewFrame");

// Another flattened world in 1.7 format
xmlString = R"(
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="default">
<model name="ParentModel">
<link name="ChildModel::L1"/>
<model name="A::B"/>
<model name="C">
<link name="D::E"/>
<link name="F::G::H">
<visual name="v1">
<pose relative_to="F::G::__model__">1 0 0 0 0 0</pose>
<geometry>
<sphere>
<radius>0.1</radius>
</sphere>
</geometry>
</visual>
</link>
</model>
</model>
</world>
</sdf>)";

xmlDoc.Clear();
xmlDoc.Parse(xmlString.c_str());

sdf::Converter::Convert(&xmlDoc, &convertXmlDoc);

// Check some basic elements
convertedElem = xmlDoc.FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "sdf");
convertedElem = convertedElem->FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "world");
convertedElem = convertedElem->FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "model");
EXPECT_STREQ(convertedElem->Attribute("name"), "ParentModel");
EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);

// Check unnested elements
convertedElem = convertedElem->FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "model");
EXPECT_STREQ(convertedElem->Attribute("name"), "C");
convertedElem = convertedElem->NextSiblingElement();
EXPECT_STREQ(convertedElem->Name(), "model");
EXPECT_STREQ(convertedElem->Attribute("name"), "ChildModel");
EXPECT_STREQ(convertedElem->FirstChildElement()->Name(), "link");
EXPECT_STREQ(convertedElem->FirstChildElement()->Attribute("name"), "L1");
convertedElem = convertedElem->NextSiblingElement();
EXPECT_STREQ(convertedElem->Name(), "model");
EXPECT_STREQ(convertedElem->Attribute("name"), "A");
EXPECT_STREQ(convertedElem->FirstChildElement()->Name(), "model");
EXPECT_STREQ(convertedElem->FirstChildElement()->Attribute("name"), "B");
EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
convertedElem = convertedElem->PreviousSiblingElement();
convertedElem = convertedElem->PreviousSiblingElement();
EXPECT_STREQ(convertedElem->Name(), "model");
EXPECT_STREQ(convertedElem->Attribute("name"), "C");
convertedElem = convertedElem->FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "model");
EXPECT_STREQ(convertedElem->Attribute("name"), "D");
EXPECT_STREQ(convertedElem->FirstChildElement()->Name(), "link");
EXPECT_STREQ(convertedElem->FirstChildElement()->Attribute("name"), "E");
convertedElem = convertedElem->NextSiblingElement();
EXPECT_STREQ(convertedElem->Name(), "model");
EXPECT_STREQ(convertedElem->Attribute("name"), "F");
convertedElem = convertedElem->FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "model");
EXPECT_STREQ(convertedElem->Attribute("name"), "G");
EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
convertedElem = convertedElem->FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "link");
EXPECT_STREQ(convertedElem->Attribute("name"), "H");
EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
convertedElem = convertedElem->FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "visual");
EXPECT_STREQ(convertedElem->Attribute("name"), "v1");
EXPECT_EQ(convertedElem->NextSiblingElement(), nullptr);
convertedElem = convertedElem->FirstChildElement();
EXPECT_STREQ(convertedElem->Name(), "pose");
EXPECT_STREQ(convertedElem->Attribute("relative_to"), "__model__");
EXPECT_STREQ(convertedElem->NextSiblingElement()->Name(), "geometry");
}

/////////////////////////////////////////////////
Expand Down
92 changes: 50 additions & 42 deletions test/integration/nested_model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -641,59 +641,67 @@ TEST(NestedModel, PartiallyFlattened)
const sdf::Model *outerModel = world->ModelByIndex(0);
ASSERT_NE(nullptr, outerModel);
EXPECT_EQ("ParentModel", outerModel->Name());
EXPECT_EQ(4u, outerModel->LinkCount());
EXPECT_NE(nullptr, outerModel->LinkByIndex(0));
EXPECT_NE(nullptr, outerModel->LinkByIndex(1));
EXPECT_NE(nullptr, outerModel->LinkByIndex(2));
EXPECT_NE(nullptr, outerModel->LinkByIndex(3));
EXPECT_EQ(nullptr, outerModel->LinkByIndex(4));

EXPECT_TRUE(outerModel->LinkNameExists("M1::L1"));
EXPECT_TRUE(outerModel->LinkNameExists("M1::L2"));
EXPECT_TRUE(outerModel->LinkNameExists("M1::L3"));
EXPECT_TRUE(outerModel->LinkNameExists("M1::L4"));

EXPECT_EQ(3u, outerModel->JointCount());
EXPECT_TRUE(outerModel->JointNameExists("M1::J1"));
EXPECT_TRUE(outerModel->JointNameExists("M1::J2"));
EXPECT_TRUE(outerModel->JointNameExists("M1::J3"));

EXPECT_EQ(3u, outerModel->FrameCount());
EXPECT_TRUE(outerModel->FrameNameExists("M1::__model__"));
EXPECT_TRUE(outerModel->FrameNameExists("M1::F1"));
EXPECT_TRUE(outerModel->FrameNameExists("M1::F2"));

EXPECT_EQ(1u, outerModel->ModelCount());
EXPECT_NE(nullptr, outerModel->ModelByIndex(0));
EXPECT_EQ(nullptr, outerModel->ModelByIndex(1));

// Get the middle model
const sdf::Model *midModel = outerModel->ModelByIndex(0);
ASSERT_NE(nullptr, midModel);
EXPECT_EQ("M1::M2", midModel->Name());
// TODO: the following expectations should be true, but ModelNameExists
// and ModelByName do not support names containing "::".
// EXPECT_TRUE(outerModel->ModelNameExists("M1::M2"));
// EXPECT_EQ(midModel, outerModel->ModelByName("M1::M2"));

EXPECT_EQ(1u, midModel->LinkCount());
EXPECT_EQ(1u, midModel->ModelCount());
EXPECT_EQ("M1", midModel->Name());
EXPECT_EQ("L1", midModel->CanonicalLinkName());
EXPECT_EQ(ignition::math::Pose3d(10, 0, 0, 0, -0, 1.5708),
midModel->SemanticPose().RawPose());
EXPECT_EQ("__model__", midModel->SemanticPose().RelativeTo());

EXPECT_EQ(4u, midModel->LinkCount());
EXPECT_NE(nullptr, midModel->LinkByIndex(0));
EXPECT_EQ(nullptr, midModel->LinkByIndex(1));
EXPECT_NE(nullptr, midModel->LinkByIndex(1));
EXPECT_NE(nullptr, midModel->LinkByIndex(2));
EXPECT_NE(nullptr, midModel->LinkByIndex(3));
EXPECT_EQ(nullptr, midModel->LinkByIndex(4));

EXPECT_TRUE(midModel->LinkNameExists("L1"));
EXPECT_TRUE(midModel->LinkNameExists("L2"));
EXPECT_TRUE(midModel->LinkNameExists("L3"));
EXPECT_TRUE(midModel->LinkNameExists("L4"));

EXPECT_EQ(3u, midModel->JointCount());
EXPECT_TRUE(midModel->JointNameExists("J1"));
EXPECT_TRUE(midModel->JointNameExists("J2"));
EXPECT_TRUE(midModel->JointNameExists("J3"));

EXPECT_EQ(2u, midModel->FrameCount());
EXPECT_TRUE(midModel->FrameNameExists("F1"));
EXPECT_TRUE(midModel->FrameNameExists("F2"));

EXPECT_EQ(1u, midModel->ModelCount());
EXPECT_NE(nullptr, midModel->ModelByIndex(0));
EXPECT_EQ(nullptr, midModel->ModelByIndex(1));

EXPECT_TRUE(midModel->ModelNameExists("M2"));

// Get the inner model of midModel
const sdf::Model *innerModel = midModel->ModelByIndex(0);
ASSERT_NE(nullptr, innerModel);
EXPECT_EQ("M2", innerModel->Name());
EXPECT_EQ(innerModel, midModel->ModelByName("M2"));

EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, -0, 0),
innerModel->SemanticPose().RawPose());
EXPECT_EQ("F1", innerModel->SemanticPose().RelativeTo());

EXPECT_TRUE(midModel->LinkNameExists("L5"));
EXPECT_EQ(1u, innerModel->LinkCount());
EXPECT_NE(nullptr, innerModel->LinkByIndex(0));
EXPECT_EQ(nullptr, innerModel->LinkByIndex(1));

EXPECT_EQ(0u, midModel->JointCount());
EXPECT_TRUE(innerModel->LinkNameExists("L5"));
EXPECT_NE(nullptr, innerModel->LinkByName("L5"));

EXPECT_EQ(0u, midModel->FrameCount());
EXPECT_EQ(0u, innerModel->JointCount());

EXPECT_EQ(0u, midModel->ModelCount());
EXPECT_EQ(0u, innerModel->FrameCount());

// test nested names from outer model
// TODO: the following expectations also fail due to the limitations of
// ModelNameExists and ModelByName not supporting names containing "::".
// const std::string midLinkNestedName = "M1::M2::L5";
// EXPECT_TRUE(outerModel->LinkNameExists(midLinkNestedName));
// EXPECT_NE(nullptr, outerModel->LinkByName(midLinkNestedName));
EXPECT_EQ(0u, innerModel->ModelCount());
}

//////////////////////////////////////////////////
Expand Down

0 comments on commit a1006ad

Please sign in to comment.