diff --git a/urdf_parser/src/link.cpp b/urdf_parser/src/link.cpp index 993f6aa5..d3a7b2f2 100644 --- a/urdf_parser/src/link.cpp +++ b/urdf_parser/src/link.cpp @@ -381,7 +381,7 @@ bool parseVisual(Visual &vis, TiXmlElement *config) vis.material.reset(new Material()); if (!parseMaterial(*vis.material, mat, true)) { - CONSOLE_BRIDGE_logDebug("urdfdom: material has only name, actual material definition may be in the model"); + vis.material.reset(); } } diff --git a/urdf_parser/src/model.cpp b/urdf_parser/src/model.cpp index a905f89e..7e9bc309 100644 --- a/urdf_parser/src/model.cpp +++ b/urdf_parser/src/model.cpp @@ -59,6 +59,33 @@ ModelInterfaceSharedPtr parseURDFFile(const std::string &path) return urdf::parseURDF( xml_str ); } +bool assignMaterial(const VisualSharedPtr& visual, ModelInterfaceSharedPtr& model, const char* link_name) +{ + if (visual->material_name.empty()) + return true; + + const MaterialSharedPtr& material = model->getMaterial(visual->material_name); + if (material) + { + CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material to '%s'", link_name, visual->material_name.c_str()); + visual->material = material; + } + else + { + if (visual->material) + { + CONSOLE_BRIDGE_logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link_name, visual->material_name.c_str()); + model->materials_.insert(make_pair(visual->material->name, visual->material)); + } + else + { + CONSOLE_BRIDGE_logWarn("link '%s' material '%s' undefined.", link_name,visual->material_name.c_str()); + return false; + } + } + return true; +} + ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) { ModelInterfaceSharedPtr model(new ModelInterface); @@ -137,32 +164,15 @@ ModelInterfaceSharedPtr parseURDF(const std::string &xml_string) } else { - // set link visual material + // set link visual(s) material CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str()); if (link->visual) { - if (!link->visual->material_name.empty()) - { - if (model->getMaterial(link->visual->material_name)) - { - CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); - link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); - } - else - { - if (link->visual->material) - { - CONSOLE_BRIDGE_logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); - model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); - } - else - { - CONSOLE_BRIDGE_logError("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); - model.reset(); - return model; - } - } - } + 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));