Skip to content

Commit

Permalink
fixed error messages, tests, and style
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 17, 2024
1 parent 47f9c1f commit 8046a01
Show file tree
Hide file tree
Showing 2 changed files with 92 additions and 40 deletions.
35 changes: 17 additions & 18 deletions src/parser_urdf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1604,7 +1604,7 @@ void CopyBlob(tinyxml2::XMLElement *_src, tinyxml2::XMLElement *_blob_parent)
void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
bool link_found = false;
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 @@ -1616,7 +1616,7 @@ void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
{
if (sdfIt->first == _linkName)
{
link_found = true;
linkFound = true;
// std::cerr << "============================\n";
// std::cerr << "working on g_extensions for link ["
// << sdfIt->first << "]\n";
Expand Down Expand Up @@ -1911,8 +1911,8 @@ 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"
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.";
}
Expand All @@ -1922,7 +1922,7 @@ void InsertSDFExtensionCollision(tinyxml2::XMLElement *_elem,
void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
bool link_found = false;
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 @@ -1934,7 +1934,7 @@ void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
{
if (sdfIt->first == _linkName)
{
link_found=true;
linkFound=true;
// std::cerr << "============================\n";
// std::cerr << "working on g_extensions for link ["
// << sdfIt->first << "]\n";
Expand Down Expand Up @@ -2113,8 +2113,8 @@ 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"
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.";
}
Expand All @@ -2124,14 +2124,14 @@ void InsertSDFExtensionVisual(tinyxml2::XMLElement *_elem,
void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
const std::string &_linkName)
{
bool link_found = false;
bool linkFound = false;
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
if (sdfIt->first == _linkName)
{
link_found = true;
linkFound = true;
sdfdbg << "inserting extension with reference ["
<< _linkName << "] into link.\n";
for (std::vector<SDFExtensionPtr>::iterator ge =
Expand Down Expand Up @@ -2171,10 +2171,9 @@ 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"
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.";
}
Expand All @@ -2184,15 +2183,15 @@ void InsertSDFExtensionLink(tinyxml2::XMLElement *_elem,
void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem,
const std::string &_jointName)
{
bool joint_found = false;
bool jointFound = false;
auto* doc = _elem->GetDocument();
for (StringSDFExtensionPtrMap::iterator
sdfIt = g_extensions.begin();
sdfIt != g_extensions.end(); ++sdfIt)
{
if (sdfIt->first == _jointName)
{
joint_found = true;
jointFound = true;
for (std::vector<SDFExtensionPtr>::iterator
ge = sdfIt->second.begin();
ge != sdfIt->second.end(); ++ge)
Expand Down Expand Up @@ -2329,9 +2328,9 @@ 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"
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
97 changes: 75 additions & 22 deletions src/parser_urdf_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2505,7 +2505,7 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
parser.InitModelString(str, config, &sdfResult);

EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"<gazebo> tag with reference[link1] does not exist"
"<link> tag reference[link1] does not exist"
" in the URDF model. Please ensure that the reference attribute"
" matches the name of a link.");
}
Expand All @@ -2521,18 +2521,20 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
<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>
</geometry>
<material>
<color rgba="0.8 0.1 0.1 1.0"/>
</material>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
</visual>
</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>
<gazebo reference="vіsual1">
</gazebo>
</robot>)";

sdf::URDF2SDF parser;
Expand All @@ -2541,7 +2543,7 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
parser.InitModelString(str, config, &sdfResult);

EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"<visual> tag with reference[link1] does not exist"
"<link> tag reference[link1] does not exist"
" in the URDF model. Please ensure that the reference attribute"
" matches the name of a link.");
}
Expand All @@ -2557,15 +2559,21 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
<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>
<collision reference="lіnk1">
<geometry>
<sphere>
<radius>0.5</radius>
</sphere>
</geometry>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
</collision>
<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;
Expand All @@ -2574,10 +2582,55 @@ TEST(URDFParser, ParseGazeboRefDoesntExistWarningMessage)
parser.InitModelString(str, config, &sdfResult);

EXPECT_PRED2(sdf::testing::contains, buffer.str(),
"<collision> tag with reference[link1] does not exist"
"<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>
</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(),
"<joint> tag reference[joint1] does not exist"
" in the URDF model. Please ensure that the reference attribute"
" matches the name of a joint.");
}
}

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

0 comments on commit 8046a01

Please sign in to comment.