diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp
index e7cbdf18..e20439df 100644
--- a/urdf_parser/src/link.cpp
+++ b/urdf_parser/src/link.cpp
@@ -494,16 +494,14 @@ bool parseLink(Link &link, tinyxml2::XMLElement* config,
VisualSharedPtr vis;
vis.reset(new Visual());
- if (parseVisual(*vis, vis_xml, version))
- {
- link.visual_array.push_back(vis);
- }
- else
+ if (!parseVisual(*vis, vis_xml, version))
{
vis.reset();
CONSOLE_BRIDGE_logError("Could not parse visual element for Link [%s]", link.name.c_str());
return false;
}
+
+ link.visual_array.push_back(vis);
}
// Visual (optional)
@@ -516,16 +514,13 @@ bool parseLink(Link &link, tinyxml2::XMLElement* config,
{
CollisionSharedPtr col;
col.reset(new Collision());
- if (parseCollision(*col, col_xml, version))
- {
- link.collision_array.push_back(col);
- }
- else
+ if (!parseCollision(*col, col_xml, version))
{
col.reset();
CONSOLE_BRIDGE_logError("Could not parse collision element for Link [%s]", link.name.c_str());
return false;
}
+ link.collision_array.push_back(col);
}
// Collision (optional)
diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp
index 2998827f..5fba5d95 100644
--- a/urdf_parser/src/model.cpp
+++ b/urdf_parser/src/model.cpp
@@ -91,6 +91,19 @@ bool assignMaterial(const VisualSharedPtr& visual, ModelInterfaceSharedPtr& mode
return true;
}
+bool operator==(const Material& lhs, const Material& rhs)
+{
+ return lhs.texture_filename == rhs.texture_filename &&
+ lhs.color.r == rhs.color.r &&
+ lhs.color.g == rhs.color.g &&
+ lhs.color.b == rhs.color.b &&
+ lhs.color.a == rhs.color.a;
+}
+inline bool operator!=(const Material& lhs, const Material& rhs)
+{
+ return !(lhs == rhs);
+}
+
ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
{
ModelInterfaceSharedPtr model(new ModelInterface);
@@ -152,27 +165,31 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
MaterialSharedPtr material;
material.reset(new Material);
+ bool success;
try {
- parseMaterial(*material, material_xml, false); // material needs to be fully defined here
- if (model->getMaterial(material->name))
- {
- CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
- material.reset();
- model.reset();
- return model;
- }
- else
- {
- model->materials_.insert(make_pair(material->name,material));
- CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
- }
- }
- catch (ParseError &/*e*/) {
+ success = parseMaterial(*material, material_xml, false); // material needs to be fully defined here
+ } catch(ParseError & /*e*/) {
CONSOLE_BRIDGE_logError("material xml is not initialized correctly");
+ success = false;
+ }
+
+ if (success && material) {
+ if (const MaterialSharedPtr& other = model->getMaterial(material->name);
+ other && *material != *other)
+ {
+ CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
+ success = false;
+ }
+ }
+
+ if (!success) {
material.reset();
model.reset();
return model;
}
+
+ model->materials_.insert(make_pair(material->name,material));
+ CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
}
// Get all Link elements
@@ -181,37 +198,41 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
LinkSharedPtr link;
link.reset(new Link);
+ bool success;
try {
- parseLink(*link, link_xml, version);
- if (model->getLink(link->name))
- {
- CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
- model.reset();
- return model;
- }
- else
- {
- // set link visual(s) material
- CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
- if (link->visual)
- {
- assignMaterial(link->visual, model, link->name.c_str());
- }
- for (const auto& visual : link->visual_array)
- {
- assignMaterial(visual, model, link->name.c_str());
- }
-
- model->links_.insert(make_pair(link->name,link));
- CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
- }
+ success = parseLink(*link, link_xml, version);
+ } catch (ParseError & /*e*/) {
+ success = false;
}
- catch (ParseError &/*e*/) {
+
+ if (!success) {
CONSOLE_BRIDGE_logError("link xml is not initialized correctly");
model.reset();
return model;
}
+
+ if (model->getLink(link->name))
+ {
+ CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
+ model.reset();
+ return model;
+ }
+
+ // set link visual(s) material
+ CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
+ if (link->visual)
+ {
+ assignMaterial(link->visual, model, link->name.c_str());
+ }
+ for (const auto& visual : link->visual_array)
+ {
+ assignMaterial(visual, model, link->name.c_str());
+ }
+
+ model->links_.insert(make_pair(link->name, link));
+ CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
}
+
if (model->links_.empty()){
CONSOLE_BRIDGE_logError("No link elements found in urdf file");
model.reset();
@@ -224,26 +245,28 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string)
JointSharedPtr joint;
joint.reset(new Joint);
- if (parseJoint(*joint, joint_xml, version))
- {
- if (model->getJoint(joint->name))
- {
- CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str());
- model.reset();
- return model;
- }
- else
- {
- model->joints_.insert(make_pair(joint->name,joint));
- CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
- }
+ bool success;
+ try {
+ success = parseJoint(*joint, joint_xml, version);
+ } catch(ParseError & /*e*/) {
+ success = false;
}
- else
- {
+
+ if (!success) {
CONSOLE_BRIDGE_logError("joint xml is not initialized correctly");
model.reset();
return model;
}
+
+ if (model->getJoint(joint->name))
+ {
+ CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str());
+ model.reset();
+ return model;
+ }
+
+ model->joints_.insert(make_pair(joint->name,joint));
+ CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
}
diff --git a/urdf_parser/test/urdf_unit_test.cpp b/urdf_parser/test/urdf_unit_test.cpp
index 6e8d87e1..e83fe64f 100644
--- a/urdf_parser/test/urdf_unit_test.cpp
+++ b/urdf_parser/test/urdf_unit_test.cpp
@@ -27,19 +27,6 @@ bool quat_are_near(urdf::Rotation left, urdf::Rotation right)
std::abs(l[3] + r[3]) < epsilon);
}
-std::ostream &operator<<(std::ostream &os, const urdf::Rotation& rot)
-{
- double roll, pitch, yaw;
- double x, y, z, w;
- rot.getRPY(roll, pitch, yaw);
- rot.getQuaternion(x, y, z, w);
- os << std::setprecision(9)
- << "x: " << x << " y: " << y << " z: " << z << " w: " << w
- << " roll: " << roll << " pitch: " << pitch << " yaw: "<< yaw;
- return os;
-}
-
-
void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
{
urdf::Rotation rot0;
@@ -48,12 +35,6 @@ void check_get_set_rpy_is_idempotent(double x, double y, double z, double w)
rot0.getRPY(roll, pitch, yaw);
urdf::Rotation rot1;
rot1.setFromRPY(roll, pitch, yaw);
- if (true) {
- std::cout << "\n"
- << "before " << rot0 << "\n"
- << "after " << rot1 << "\n"
- << "ok " << quat_are_near(rot0, rot1) << "\n";
- }
EXPECT_TRUE(quat_are_near(rot0, rot1));
}
@@ -66,12 +47,6 @@ void check_get_set_rpy_is_idempotent_from_rpy(double r, double p, double y)
urdf::Rotation rot1;
rot1.setFromRPY(roll, pitch, yaw);
bool ok = quat_are_near(rot0, rot1);
- if (!ok) {
- std::cout << "initial rpy: " << r << " " << p << " " << y << "\n"
- << "before " << rot0 << "\n"
- << "after " << rot1 << "\n"
- << "ok " << ok << "\n";
- }
EXPECT_TRUE(ok);
}
@@ -592,7 +567,6 @@ TEST(URDF_UNIT_TEST, parse_link_doubles)
EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz);
}
-
TEST(URDF_UNIT_TEST, parse_color_doubles)
{
std::string joint_str =
@@ -666,6 +640,67 @@ TEST(URDF_UNIT_TEST, parse_color_doubles)
EXPECT_EQ(0.908, urdf->links_["l1"]->inertial->izz);
}
+TEST(URDF_UNIT_TEST, material_no_name)
+{
+ std::string joint_str =
+ ""
+ " "
+ " "
+ "";
+ urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(joint_str);
+ EXPECT_EQ(nullptr, urdf);
+}
+
+TEST(URDF_UNIT_TEST, materials_no_rgb)
+{
+ std::string urdf_str =
+ ""
+ " "
+ " "
+ "";
+ urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
+ EXPECT_EQ(nullptr, urdf); // material missing RGB
+}
+
+TEST(URDF_UNIT_TEST, duplicate_materials)
+{
+ std::string urdf_str =
+ ""
+ " "
+ " "
+ " "
+ " "
+ " "
+ " "
+ " "
+ "";
+
+ urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
+ EXPECT_TRUE(static_cast(urdf)); // identical materials are fine
+
+ urdf_str =
+ ""
+ " "
+ " "
+ " "
+ " "
+ " "
+ " "
+ " "
+ "";
+ urdf = urdf::parseURDF(urdf_str);
+ EXPECT_EQ(nullptr, urdf); // different materials cause failure
+}
+
+TEST(URDF_UNIT_TEST, link_no_name)
+{
+ std::string urdf_str =
+ ""
+ " "
+ "";
+ urdf::ModelInterfaceSharedPtr urdf = urdf::parseURDF(urdf_str);
+ EXPECT_EQ(nullptr, urdf);
+}
int main(int argc, char **argv)
{