Skip to content

Commit

Permalink
added other SDFExtensions
Browse files Browse the repository at this point in the history 
Signed-off-by: Aditya Agrawal <[email protected]>
  • Loading branch information
@aagrawal05
aagrawal05 committed Aug 16, 2024
1 parent 2d221fe commit d610aa9
Show file tree
Hide file tree
Showing 2 changed files with 96 additions and 8 deletions.
25 changes: 25 additions & 0 deletions src/parser_urdf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1604,6 +1604,7 @@ void CopyBlob(tinyxml2::XMLElement *_src, tinyxml2::XMLElement *_blob_parent)
void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
bool link_found = false;
// loop through extensions for the whole model
// and see which ones belong to _linkName
// This might be complicated since there's:
Expand All @@ -1615,6 +1616,7 @@ void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
{
if (sdfIt->first == _linkName)
{
link_found = true;
// std::cerr << "============================\n";
// std::cerr << "working on g_extensions for link ["
// << sdfIt->first << "]\n";
Expand Down Expand Up @@ -1908,12 +1910,19 @@ void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
}
}
}
// If we didn't find the link, emit a warning
if (!link_found) {
sdfwarn << "<collision> tag with reference[" << _linkName << "] does not exist"
<< " in the URDF model. Please ensure that the reference attribute"
<< " matches the name of a link.";
}
}

////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
bool link_found = false;
// loop through extensions for the whole model
// and see which ones belong to _linkName
// This might be complicated since there's:
Expand All @@ -1925,6 +1934,7 @@ void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
{
if (sdfIt->first == _linkName)
{
link_found=true;
// std::cerr << "============================\n";
// std::cerr << "working on g_extensions for link ["
// << sdfIt->first << "]\n";
Expand Down Expand Up @@ -2102,6 +2112,12 @@ void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
}
}
}
// If we didn't find the link, emit a warning
if (!link_found) {
sdfwarn << "<visual> tag with reference[" << _linkName << "] does not exist"
<< " in the URDF model. Please ensure that the reference attribute"
<< " matches the name of a link.";
}
}

////////////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -2168,13 +2184,15 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
const std::string &_jointName)
{
bool joint_found = false;
auto* doc = _elem->GetDocument();
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
if (sdfIt->first == _jointName)
{
joint_found = true;
for (std::vector<SDFExtensionPtr>::iterator
ge = sdfIt->second.begin();
ge != sdfIt->second.end(); ++ge)
Expand Down Expand Up @@ -2309,6 +2327,13 @@ void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
}
}
}

// If we didn't find the link, emit a warning
if (!joint_found) {
sdfwarn << "<joint> tag with name[" << _jointName << "] does not exist"
<< " in the URDF model. Please ensure that the name attribute"
<< " matches the name of a joint.";
}
}

////////////////////////////////////////////////////////////////////////////////
Expand Down
79 changes: 71 additions & 8 deletions src/parser_urdf_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2476,8 +2476,8 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
});
#endif

// test if reference to link exists
{
// test if reference to link exists
{
// clear the contents of the buffer
buffer.str("");

Expand Down Expand Up @@ -2509,12 +2509,75 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
" in the URDF model. Please ensure that the reference attribute"
" matches the name of a link.");
}

/* TODO(aagrawal05): Similar tests for -
InsertSDFExtensionCollision,
InsertSDFExtensionRobot,
InsertSDFExtensionVisual,
InsertSDFExtensionJoint */

{
// clear the contents of the buffer
buffer.str("");

std::string str = R"(
<robot name="test_robot">
<link name="link1">
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<visual reference="lіnk1">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<color rgba="0.8 0.1 0.1 1.0"/>
</material>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
</visual>
</robot>)";

sdf::URDF2SDF parser;
tinyxml2::XMLDocument sdfResult;
sdf::ParserConfig config;
parser.InitModelString(str, config, &sdfResult);

EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"<visual> tag with reference[link1] does not exist"
" in the URDF model. Please ensure that the reference attribute"
" matches the name of a link.");
}

{
// clear the contents of the buffer
buffer.str("");

std::string str = R"(
<robot name="test_robot">
<link name="link1">
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
</inertial>
</link>
<collision reference="lіnk1">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
</collision>
</robot>)";

sdf::URDF2SDF parser;
tinyxml2::XMLDocument sdfResult;
sdf::ParserConfig config;
parser.InitModelString(str, config, &sdfResult);

EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"<collision> tag with reference[link1] does not exist"
" in the URDF model. Please ensure that the reference attribute"
" matches the name of a link.");
}
}

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

0 comments on commit d610aa9

Please sign in to comment.