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

Jump to bottom

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 all 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
33 changes: 33 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 linkFound = 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)
{
linkFound = 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 (!linkFound) {
sdfwarn << "<link> tag 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 linkFound = 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)
{
linkFound = 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 (!linkFound) {
sdfwarn << "<link> tag reference[" << _linkName << "] does not exist"
<< " 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 linkFound = false;
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
if (sdfIt->first == _linkName)
{
linkFound = true;
sdfdbg << "inserting extension with reference ["
<< _linkName << "] into link.\n";
for (std::vector<SDFExtensionPtr>::iterator ge =
Expand Down Expand Up @@ -2153,19 +2171,27 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
}
}
}
// If we didn't find the link, emit a warning
if (!linkFound) {
sdfwarn << "<link> tag reference[" << _linkName << "] does not exist"
<< " 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 jointFound = false;
auto* doc = _elem->GetDocument();
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
if (sdfIt->first == _jointName)
{
jointFound = true;
for (std::vector<SDFExtensionPtr>::iterator
ge = sdfIt->second.begin();
ge != sdfIt->second.end(); ++ge)
Expand Down Expand Up @@ -2300,6 +2326,13 @@ void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
}
}
}

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

////////////////////////////////////////////////////////////////////////////////
Expand Down
173 changes: 173 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,179 @@ 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.

"<link> tag 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>
<visual name="visual1">
<geometry>
<box>
<size>1 1 1</size>
</box>
Comment on lines +2526 to +2528
Copy link
Collaborator

Choose a reason for hiding this comment

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

Suggested change
<box>
<size>1 1 1</size>
</box>
<box size="1 1 1"/>

Copy link
Author

Choose a reason for hiding this comment

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

Yep, will add to next commit

</geometry>
<material>
Copy link
Collaborator

Choose a reason for hiding this comment

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

Suggested change
<material>
<material name="mat1">

<color rgba="0.8 0.1 0.1 1.0"/>
</material>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
</visual>
</link>
<gazebo reference="vіsual1">
</gazebo>
</robot>)";

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

EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"<link> tag reference[link1] does not exist"
Copy link
Collaborator

Choose a reason for hiding this comment

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

Shouldn't the error message reference visual1?

Suggested change
"<link> tag reference[link1] does not exist"
"<link> tag reference[visual1] does not exist"

Copy link
Author

Choose a reason for hiding this comment

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

Yep looking into this. I'be tried using _elem->Attribute("name") but that doesn't give it either-often giving the link or something different. Similarly the sdfVisualName doesn't seem correct. I'm tracing the code to see if there's an easy way to get the refString from parseURDF but it's taking a bit longer because of my unfamiliarity with the codebase. Please let me know if my approach is correct.

" 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>
<collision name="collision1">
<geometry>
<sphere radius="0.5" />
</geometry>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
</collision>
</link>
<gazebo reference="collіsion1">
<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(),
"<link> tag reference[link1] does not exist"
Copy link
Collaborator

Choose a reason for hiding this comment

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

Suggested change
"<link> tag reference[link1] does not exist"
"<link> tag reference[collision1] 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>
<link name="link2">
<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>
<joint name="joint1" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<axis xyz="0 0 1"/>
<limit lower="-1.57" upper="1.57" effort="10" velocity="1.0"/>
</joint>
<gazebo reference="joіnt1">
<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 comment why the two joint1s are different?

"<joint> tag reference[joint1] does not exist"
" in the URDF model. Please ensure that the reference attribute"
" matches the name of a joint.");
}
Copy link
Collaborator

Choose a reason for hiding this comment

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

I've noticed with this PR that even properly working URDFs print a warning message. Do you mind adding a test with a URDF that has a correct reference and check that there is no warning printed.

Copy link
Author

Choose a reason for hiding this comment

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

Yep ran into this while addressing the previous point. Will do.

}
aagrawal05 marked this conversation as resolved.
Show resolved Hide resolved

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