Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft: Parser warning message when <gazebo> reference does not exist in URDF #1392

Open
wants to merge 9 commits into
base: sdf14
Choose a base branch
from
Open

Draft: Parser warning message when <gazebo> reference does not exist in URDF #1392

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
640766f
initial draft of warning message functionality
aagrawal05 Apr 4, 2024
ff06de3
fixed formatting
aagrawal05 Apr 5, 2024
bff4f75
Formatting changes parser_urdf_TEST.cc
aagrawal05 Apr 5, 2024
d9f56f5
Formatting changes in parser_urdf.cc
aagrawal05 Apr 5, 2024
2d221fe
Update parser_urdf_TEST.cc and fixed mistake in test
aagrawal05 Apr 5, 2024
d610aa9
added other SDFExtensions
aagrawal05 Aug 16, 2024
47f9c1f
Merge branch 'sdf14' into parser_ref_warning_message
azeey Aug 16, 2024
8046a01
fixed error messages, tests, and style
aagrawal05 Aug 17, 2024
7f7b183
style
aagrawal05 Aug 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 34 additions & 0 deletions src/parser_urdf.cc
View file
Open in desktop
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;
aagrawal05 marked this conversation as resolved.
Show resolved Hide resolved
// 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"
aagrawal05 marked this conversation as resolved.
Show resolved Hide resolved
<< " 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,18 +2112,26 @@ 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"
aagrawal05 marked this conversation as resolved.
Show resolved Hide resolved
<< " in the URDF model. Please ensure that the reference attribute"
<< " matches the name of a link.";
}
}

////////////////////////////////////////////////////////////////////////////////
void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
bool link_found = false;
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
if (sdfIt->first == _linkName)
{
link_found = true;
sdfdbg << "inserting extension with reference ["
<< _linkName << "] into link.\n";
for (std::vector<SDFExtensionPtr>::iterator ge =
Expand Down Expand Up @@ -2153,19 +2171,28 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
}
}
}

// If we didn't find the link, emit a warning
if (!link_found) {
sdfwarn << "<gazebo> tag with reference[" << _linkName << "] does not exist"
aagrawal05 marked this conversation as resolved.
Show resolved Hide resolved
<< " in the URDF model. Please ensure that the reference attribute"
<< " matches the name of a link.";
}
}

////////////////////////////////////////////////////////////////////////////////
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 @@ -2300,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
120 changes: 120 additions & 0 deletions src/parser_urdf_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2460,6 +2460,126 @@ TEST(URDFParser, ZeroMassLeafLink)
}
}

/////////////////////////////////////////////////
TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
{
// Redirect sdfwarn output
std::stringstream buffer;
sdf::testing::RedirectConsoleStream redir(
sdf::Console::Instance()->GetMsgStream(), &buffer);
#ifdef _WIN32
sdf::Console::Instance()->SetQuiet(false);
sdf::testing::ScopeExit revertSetQuiet(
[]
{
sdf::Console::Instance()->SetQuiet(true);
});
#endif

// test if reference to link exists
{
// 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>
<gazebo reference="lіnk1">
<sensor name="link1_imu" type="imu">
<always_on>1</always_on>
<update_rate>100</update_rate>
<pose>0.13525 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
<plugin name="sensor_plugin" filename="example_plugin.so" />
</sensor>
</gazebo>
</robot>)";

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

EXPECT_PRED2(sdf::testing::contains, buffer.str(),
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you add a comment here why the two link1s are not the same? I believe the і (https://www.compart.com/en/unicode/U+0456) in <gazebo reference="lіnk1"> is the issue.

"<gazebo> 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>
<visual reference="lіnk1">
aagrawal05 marked this conversation as resolved.
Show resolved Hide resolved
<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.");
}
}
aagrawal05 marked this conversation as resolved.
Show resolved Hide resolved

/////////////////////////////////////////////////
/// Main
int main(int argc, char **argv)
Expand Down