Skip to content

Commit

Permalink
changed unnest to unflatten & added expectations back
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 12, 2021
1 parent 33dda02 commit dce6828
Show file tree
Hide file tree
Showing 6 changed files with 120 additions and 77 deletions.
2 changes: 1 addition & 1 deletion sdf/1.8/1_7.convert
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

<convert name="world">
<convert name="model">
<unnest/>
<unflatten/>
</convert>
</convert>

Expand Down
147 changes: 83 additions & 64 deletions src/Converter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -240,11 +240,11 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem,
{
Remove(_elem, childElem);
}
else if (name == "unnest")
else if (name == "unflatten")
{
Unnest(_elem);
Unflatten(_elem);
// TODO(jenn) delete debug statements
// std::cout << "\n\n_elem after unnest:\n"
// std::cout << "\n\n_elem after unflatten:\n"
// << PrintElement(_elem) << std::endl;
}
else if (name != "convert")
Expand All @@ -255,19 +255,19 @@ void Converter::ConvertImpl(tinyxml2::XMLElement *_elem,
}

/////////////////////////////////////////////////
void Converter::Unnest(tinyxml2::XMLElement *_elem)
void Converter::Unflatten(tinyxml2::XMLElement *_elem)
{
SDF_ASSERT(_elem != NULL, "SDF element is NULL");
SDF_ASSERT(_elem != nullptr, "SDF element is nullptr");

tinyxml2::XMLDocument *doc = _elem->GetDocument();

tinyxml2::XMLElement *nextElem = nullptr, *firstUnnestedModel = nullptr;
tinyxml2::XMLElement *nextElem = nullptr, *firstUnflatModel = nullptr;
for (tinyxml2::XMLElement *elem = _elem->FirstChildElement();
elem;
elem = nextElem)
{
// break loop if reached the first unnested model
if (firstUnnestedModel == elem) break;
// break loop if reached the first unflattened model
if (firstUnflatModel == elem) break;

nextElem = elem->NextSiblingElement();
std::string elemName = elem->Name();
Expand All @@ -285,14 +285,15 @@ void Converter::Unnest(tinyxml2::XMLElement *_elem)
size_t found = attrName.find("::");
if (found == std::string::npos)
{
// recursive unnest
// recursive unflatten
if (elemName == "model")
{
// std::cout << "before recursive:\n"
// << PrintElement(elem) << std::endl;
Unnest(elem);
Unflatten(elem);

// std::cout << "unnested model:\n" << PrintElement(elem) << std::endl;
// std::cout << "unflatteneded model:\n"
// << PrintElement(elem) << std::endl;
break;
}

Expand All @@ -305,27 +306,50 @@ void Converter::Unnest(tinyxml2::XMLElement *_elem)

if (FindNewModelElements(_elem, newModel, found + 2))
{
Unnest(newModel);
Unflatten(newModel);
_elem->InsertEndChild(newModel);

// since newModel is inserted at the end, point back to the top element
nextElem = _elem->FirstChildElement();

if (!firstUnnestedModel)
firstUnnestedModel = newModel;
if (!firstUnflatModel)
firstUnflatModel = newModel;
// std::cout << "newModel:\n" << PrintElement(newModel) << std::endl;
}
}
}

/////////////////////////////////////////////////
// used to update //pose/@relative_to in FindNewModelElements
void UpdatePose(tinyxml2::XMLElement *_elem,
const size_t &_newNameIdx,
const std::string &_modelName)
{
tinyxml2::XMLElement *pose = _elem->FirstChildElement("pose");
if (pose && pose->Attribute("relative_to"))
{
std::string poseRelTo = pose->Attribute("relative_to");

SDF_ASSERT(poseRelTo.compare(0, _newNameIdx - 2, _modelName) == 0,
"Error: Pose attribute 'relative_to' does not start with " + _modelName);

poseRelTo = poseRelTo.substr(_newNameIdx);
pose->SetAttribute("relative_to", poseRelTo.c_str());
}

if (_elem->FirstChildElement("camera"))
UpdatePose(_elem->FirstChildElement("camera"), _newNameIdx, _modelName);
}

/////////////////////////////////////////////////
bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,
tinyxml2::XMLElement *_newModel,
const size_t &_newNameIdx)
{
bool unnestedNewModel = false;
bool unflattenedNewModel = false;
std::string newModelName = _newModel->Attribute("name");

// loop through looking for new model elements
tinyxml2::XMLElement *elem = _elem->FirstChildElement(), *nextElem = nullptr;
while (elem)
{
Expand All @@ -342,6 +366,8 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,
(elemName != "frame" && elemName != "joint" &&
elemName != "link" && elemName != "model"))
{
// since //gripper/@name is not flattened but the children are
// & elemAttrName.compare will evaluate to true, don't skip this element
if (elemName != "gripper")
{
elem = nextElem;
Expand Down Expand Up @@ -385,16 +411,16 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,
// strip new model prefix from attached_to
attachedTo = attachedTo.substr(_newNameIdx);
elem->SetAttribute("attached_to", attachedTo.c_str());
}

// remove frame if childAttrName == __model__
if (childAttrName == "__model__" && elem->Attribute("attached_to"))
{
_newModel->SetAttribute("canonical_link", attachedTo.c_str());
_newModel->InsertFirstChild(poseElem);
// remove frame if childAttrName == __model__
if (childAttrName == "__model__")
{
_newModel->SetAttribute("canonical_link", attachedTo.c_str());
_newModel->InsertFirstChild(poseElem);

_elem->DeleteChild(elem);
elem = poseElem;
_elem->DeleteChild(elem);
elem = poseElem;
}
}
} // frame

Expand All @@ -405,40 +431,37 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,
e;
e = e->NextSiblingElement())
{
poseElem = e->FirstChildElement("pose");
if (poseElem != nullptr)
{
std::string poseRelTo = poseElem->Attribute("relative_to");

SDF_ASSERT(poseRelTo.compare(0, _newNameIdx - 2, newModelName) == 0,
"Error: Pose attribute 'relative_to' does not start with " +
newModelName);

poseRelTo = poseRelTo.substr(_newNameIdx);
poseElem->SetAttribute("relative_to", poseRelTo.c_str());
}
UpdatePose(e, _newNameIdx, newModelName);
}
} // link

else if (elemName == "joint")
{
std::string eText;

// strip new model prefix from //joint/parent
tinyxml2::XMLElement *e = elem->FirstChildElement("parent");
std::string eText = e->GetText();
if (e != nullptr && e->GetText() != nullptr)
{
eText = e->GetText();

SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
"Error: Joint's <parent> value does not start with " + newModelName);

e->SetText(eText.substr(_newNameIdx).c_str());
e->SetText(eText.substr(_newNameIdx).c_str());
}

// strip new model prefix from //joint/child
e = elem->FirstChildElement("child");
eText = e->GetText();
if (e != nullptr && e->GetText() != nullptr)
{
eText = e->GetText();

SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
"Error: Joint's <child> value does not start with " + newModelName);

e->SetText(std::string(e->GetText()).substr(_newNameIdx).c_str());
e->SetText(eText.substr(_newNameIdx).c_str());
}

// strip new model prefix from //xyz/@expressed_in
std::string axisStr = "axis";
Expand Down Expand Up @@ -474,18 +497,7 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,
e;
e = e->NextSiblingElement("sensor"))
{
poseElem = e->FirstChildElement("pose");
if (poseElem != nullptr)
{
std::string poseRelTo = poseElem->Attribute("relative_to");

SDF_ASSERT(poseRelTo.compare(0, _newNameIdx - 2, newModelName) == 0,
"Error: Pose attribute 'relative_to' does not start with " +
newModelName);

poseRelTo = poseRelTo.substr(_newNameIdx);
poseElem->SetAttribute("relative_to", poseRelTo.c_str());
}
UpdatePose(e, _newNameIdx, newModelName);
}
} // joint

Expand All @@ -498,15 +510,19 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,
std::string eText;
while (e)
{
eText = e->GetText();

if (eText.compare(0, _newNameIdx - 2, newModelName) != 0)
if (e->GetText() != nullptr)
{
hasPrefix = false;
break;
eText = e->GetText();

if (eText.compare(0, _newNameIdx - 2, newModelName) != 0)
{
hasPrefix = false;
break;
}

e->SetText(eText.substr(_newNameIdx).c_str());
}

e->SetText(eText.substr(_newNameIdx).c_str());
e = e->NextSiblingElement("gripper_link");
}

Expand All @@ -520,22 +536,25 @@ bool Converter::FindNewModelElements(tinyxml2::XMLElement *_elem,

// strip prefix from //model/gripper/palm_link
e = elem->FirstChildElement("palm_link");
eText = e->GetText();
if (e != nullptr && e->GetText() != nullptr)
{
eText = e->GetText();

SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
SDF_ASSERT(eText.compare(0, _newNameIdx - 2, newModelName) == 0,
"Error: Gripper's <palm_link> value does not start with "
+ newModelName);

e->SetText(eText.substr(_newNameIdx).c_str());
e->SetText(eText.substr(_newNameIdx).c_str());
}
} // gripper

unnestedNewModel = true;
unflattenedNewModel = true;
_newModel->InsertEndChild(elem);

elem = nextElem;
}

return unnestedNewModel;
return unflattenedNewModel;
}

/////////////////////////////////////////////////
Expand Down
21 changes: 10 additions & 11 deletions src/Converter.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,19 +105,18 @@ namespace sdf
private: static void Remove(tinyxml2::XMLElement *_elem,
tinyxml2::XMLElement *_removeElem);

/// \brief Unnest an element (conversion from SDFormat <= 1.7 to 1.8)
/// \param[in] _elem The element to unnest
private: static void Unnest(tinyxml2::XMLElement *_elem);
/// \brief Unflatten an element (conversion from SDFormat <= 1.7 to 1.8)
/// \param[in] _elem The element to unflatten
private: static void Unflatten(tinyxml2::XMLElement *_elem);

/// \brief Finds all elements related to the unnested model
/// \param[in] _elem The element to unnest
/// \param[in] _newModel The new unnested model element
/// \brief Finds all elements related to the unflattened model
/// \param[in] _elem The element to unflatten
/// \param[in] _newModel The new unflattened model element
/// \param[in] _newNameIdx The index of the new name for child elements
/// \return True if unnested new model elements
private:
static bool FindNewModelElements(tinyxml2::XMLElement *_elem,
tinyxml2::XMLElement *_newModel,
const size_t &_newNameIdx);
/// \return True if unflattened new model elements
private: static bool FindNewModelElements(tinyxml2::XMLElement *_elem,
tinyxml2::XMLElement *_newModel,
const size_t &_newNameIdx);

private: static const char *GetValue(const char *_valueElem,
const char *_valueAttr,
Expand Down
8 changes: 8 additions & 0 deletions src/Converter_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2231,6 +2231,11 @@ TEST(Converter, World_17_to_18)
</sphere>
</geometry>
</visual>
<sensor name="s1">
<camera name="c1" type="camera">
<pose relative_to="ChildModel::__model__">0 0 1 0 0 0</pose>
</camera>
</sensor>
</link>
<link name="ChildModel::L2">
<pose relative_to="ChildModel::__model__">0 0 0 0 0 0</pose>
Expand All @@ -2251,6 +2256,9 @@ TEST(Converter, World_17_to_18)
</axis2>
<sensor name="camera" type="camera">
<pose relative_to="ChildModel::NewFrame">1 0 0 0 0 0</pose>
<camera name="c2">
<pose relative_to="ChildModel::NewFrame">0 0 1 0 0 0</pose>
</camera>
</sensor>
</joint>
<gripper name="gripper">
Expand Down
2 changes: 1 addition & 1 deletion test/integration/converter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,7 +153,7 @@ void ParserStringConverter(const std::string &_version)

/////////////////////////////////////////////////
/// Test conversion unflattened world in 1.7 to 1.8
TEST(ConverterIntegration, UnnestConversion)
TEST(ConverterIntegration, UnflattenConversion)
{
const std::string filename =
sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
Expand Down
17 changes: 17 additions & 0 deletions test/integration/nested_model.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -641,6 +641,21 @@ TEST(NestedModel, PartiallyFlattened)
const sdf::Model *outerModel = world->ModelByIndex(0);
ASSERT_NE(nullptr, outerModel);
EXPECT_EQ("ParentModel", outerModel->Name());

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_TRUE(outerModel->JointNameExists("M1::J1"));
EXPECT_TRUE(outerModel->JointNameExists("M1::J2"));
EXPECT_TRUE(outerModel->JointNameExists("M1::J3"));

EXPECT_TRUE(outerModel->FrameNameExists("M1::F1"));
EXPECT_TRUE(outerModel->FrameNameExists("M1::F2"));

EXPECT_TRUE(outerModel->ModelNameExists("M1::M2"));

EXPECT_EQ(1u, outerModel->ModelCount());

// Get the middle model
Expand Down Expand Up @@ -685,6 +700,7 @@ TEST(NestedModel, PartiallyFlattened)
ASSERT_NE(nullptr, innerModel);
EXPECT_EQ("M2", innerModel->Name());
EXPECT_EQ(innerModel, midModel->ModelByName("M2"));
EXPECT_EQ(innerModel, outerModel->ModelByName("M1::M2"));

EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, -0, 0),
innerModel->SemanticPose().RawPose());
Expand All @@ -696,6 +712,7 @@ TEST(NestedModel, PartiallyFlattened)

EXPECT_TRUE(innerModel->LinkNameExists("L5"));
EXPECT_NE(nullptr, innerModel->LinkByName("L5"));
EXPECT_TRUE(outerModel->LinkNameExists("M1::M2::L5"));

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

Expand Down

0 comments on commit dce6828

Please sign in to comment.