[parsers] Load material from URDF

.github/workflows/conan.yml

@@ -25,6 +25,10 @@ jobs:
         git config --global core.autocrlf false
         git config --global core.eol lf
       if: startsWith(runner.os, 'Windows')
+    - name: Change Xcode version
+      run: |
+        sudo xcode-select -switch /Applications/Xcode_11.7.app
+      if: startsWith(runner.os, 'macOS')
     - uses: actions/checkout@v2
       with:
         submodules: recursive

src/RBDyn/CMakeLists.txt

@@ -23,4 +23,4 @@ install(
     ARCHIVE DESTINATION "lib"
     RUNTIME DESTINATION "bin"
 )
-install(FILES ${HEADERS} DESTINATION ${INCLUDE_INSTALL_DESTINATION})
+install(FILES ${HEADERS} DESTINATION ${INCLUDE_INSTALL_DESTINATION})

src/parsers/RBDyn/parsers/common.h

@@ -95,11 +95,46 @@ struct RBDYN_PARSERS_DLLAPI Geometry
   Geometry() : type(UNKNOWN) {}
 };
 
+/** A material, as specified in the URDF format, is optionally attached to a visual element
+ *
+ * It can be either:
+ * - NONE if there is not material attached to the visual element
+ * - COLOR if it is a color element (rgba values in the [0, 1] range)
+ * - TEXTURE if it is a texture (filename)
+ */
+struct RBDYN_PARSERS_DLLAPI Material
+{
+  struct Color
+  {
+    double r;
+    double g;
+    double b;
+    double a;
+  };
+  struct Texture
+  {
+    std::string filename;
+  };
+
+  enum class Type
+  {
+    NONE,
+    COLOR,
+    TEXTURE
+  };
+  Type type;
+  using Data = boost::variant<Color, Texture>;
+  Data data;
+
+  Material() : type(Type::NONE) {}
+};
+
 struct RBDYN_PARSERS_DLLAPI Visual
 {
   std::string name;
   sva::PTransformd origin;
   Geometry geometry;
+  Material material;
 };
 
 struct RBDYN_PARSERS_DLLAPI ParserResult

src/parsers/RBDyn/parsers/yaml.h

@@ -54,6 +54,7 @@ class RBDYN_PARSERS_DLLAPI RBDynFromYAML
   std::vector<std::string> filtered_links_;
   bool with_virtual_links_;
   const std::string & spherical_suffix_;
+  std::unordered_map<std::string, Material> materials_;
 
   Eigen::Matrix3d makeInertia(double ixx, double iyy, double izz, double iyz, double ixz, double ixy);
 
@@ -76,6 +77,15 @@ class RBDYN_PARSERS_DLLAPI RBDynFromYAML
                     std::map<std::string, std::vector<Visual>> & data,
                     const std::string & name);
 
+  /** Parse a material tag and cache it */
+  void parseMaterial(const YAML::Node & material);
+
+  /** Parse a material tag into the output parameter (if the tag is valid)
+   *
+   * \param cache If true, lookup existing materials in the cache
+   */
+  void parseMaterial(const YAML::Node & material, Material & out, bool cache = true);
+
   void parseLink(const YAML::Node & link);
 
   bool parseJointType(const YAML::Node & type,

src/parsers/to_urdf.cpp

@@ -101,73 +101,99 @@ std::string to_urdf(const ParserResult & res)
 
     auto generate_visual = [&](const char * type, const std::map<std::string, std::vector<Visual>> & visuals) {
       auto visuals_it = visuals.find(link.name());
-      if(visuals_it != visuals.end())
+      if(visuals_it == visuals.end())
       {
-        for(const auto & visual : visuals_it->second)
+        return;
+      }
+      for(size_t i = 0; i < visuals_it->second.size(); ++i)
+      {
+        const auto & visual = visuals_it->second[i];
+        if(visual.geometry.type == Geometry::Type::UNKNOWN)
         {
-          if(visual.geometry.type == Geometry::Type::UNKNOWN)
-          {
-            continue;
-          }
+          continue;
+        }
 
-          auto visual_node = doc.NewElement(type);
+        auto visual_node = doc.NewElement(type);
 
-          set_origin_from_ptransform(visual_node, visual.origin);
+        set_origin_from_ptransform(visual_node, visual.origin);
 
-          auto geometry_node = doc.NewElement("geometry");
-          switch(visual.geometry.type)
+        const auto & material = visual.material;
+        if(material.type != Material::Type::NONE)
+        {
+          auto material_node = doc.NewElement("material");
+          material_node->SetAttribute("name", ("material_" + link.name() + "_" + std::to_string(i)).c_str());
+          if(material.type == Material::Type::COLOR)
           {
-            case Geometry::Type::BOX:
-            {
-              auto node = doc.NewElement("box");
-              const auto box = boost::get<Geometry::Box>(visual.geometry.data);
-              set_vec3d(node, "size", box.size);
-              geometry_node->InsertEndChild(node);
-            }
-            break;
-            case Geometry::Type::CYLINDER:
-            {
-              auto node = doc.NewElement("cylinder");
-              const auto cylinder = boost::get<Geometry::Cylinder>(visual.geometry.data);
-              node->SetAttribute("radius", std::to_string(cylinder.radius).c_str());
-              node->SetAttribute("length", std::to_string(cylinder.length).c_str());
-              geometry_node->InsertEndChild(node);
-            }
-            break;
-            case Geometry::Type::MESH:
-            {
-              auto node = doc.NewElement("mesh");
-              const auto mesh = boost::get<Geometry::Mesh>(visual.geometry.data);
-              node->SetAttribute("filename", mesh.filename.c_str());
-              node->SetAttribute("scale", std::to_string(mesh.scale).c_str());
-              geometry_node->InsertEndChild(node);
-            }
-            break;
-            case Geometry::Type::SPHERE:
-            {
-              auto node = doc.NewElement("sphere");
-              const auto sphere = boost::get<Geometry::Sphere>(visual.geometry.data);
-              node->SetAttribute("radius", std::to_string(sphere.radius).c_str());
-              geometry_node->InsertEndChild(node);
-            }
-            break;
-            case Geometry::Type::SUPERELLIPSOID:
-            {
-              auto node = doc.NewElement("superellipsoid");
-              const auto superellipsoid = boost::get<Geometry::Superellipsoid>(visual.geometry.data);
-              set_vec3d(node, "size", superellipsoid.size);
-              node->SetAttribute("epsilon1", std::to_string(superellipsoid.epsilon1).c_str());
-              node->SetAttribute("epsilon2", std::to_string(superellipsoid.epsilon2).c_str());
-              geometry_node->InsertEndChild(node);
-            }
-            break;
-            case Geometry::Type::UNKNOWN:
-              break;
+            const auto & c = boost::get<Material::Color>(material.data);
+            auto color_node = doc.NewElement("color");
+            color_node->SetAttribute("rgba", (std::to_string(c.r) + " " + std::to_string(c.g) + " "
+                                              + std::to_string(c.b) + " " + std::to_string(c.a))
+                                                 .c_str());
+            material_node->InsertEndChild(color_node);
           }
-          visual_node->InsertEndChild(geometry_node);
+          else if(material.type == Material::Type::TEXTURE)
+          {
+            const auto & texture = boost::get<Material::Texture>(material.data);
+            auto texture_node = doc.NewElement("texture");
+            texture_node->SetAttribute("filename", texture.filename.c_str());
+            material_node->InsertEndChild(texture_node);
+          }
+          visual_node->InsertEndChild(material_node);
+        }
 
-          node->InsertEndChild(visual_node);
+        auto geometry_node = doc.NewElement("geometry");
+        switch(visual.geometry.type)
+        {
+          case Geometry::Type::BOX:
+          {
+            auto node = doc.NewElement("box");
+            const auto box = boost::get<Geometry::Box>(visual.geometry.data);
+            set_vec3d(node, "size", box.size);
+            geometry_node->InsertEndChild(node);
+          }
+          break;
+          case Geometry::Type::CYLINDER:
+          {
+            auto node = doc.NewElement("cylinder");
+            const auto cylinder = boost::get<Geometry::Cylinder>(visual.geometry.data);
+            node->SetAttribute("radius", std::to_string(cylinder.radius).c_str());
+            node->SetAttribute("length", std::to_string(cylinder.length).c_str());
+            geometry_node->InsertEndChild(node);
+          }
+          break;
+          case Geometry::Type::MESH:
+          {
+            auto node = doc.NewElement("mesh");
+            const auto mesh = boost::get<Geometry::Mesh>(visual.geometry.data);
+            node->SetAttribute("filename", mesh.filename.c_str());
+            node->SetAttribute("scale", std::to_string(mesh.scale).c_str());
+            geometry_node->InsertEndChild(node);
+          }
+          break;
+          case Geometry::Type::SPHERE:
+          {
+            auto node = doc.NewElement("sphere");
+            const auto sphere = boost::get<Geometry::Sphere>(visual.geometry.data);
+            node->SetAttribute("radius", std::to_string(sphere.radius).c_str());
+            geometry_node->InsertEndChild(node);
+          }
+          break;
+          case Geometry::Type::SUPERELLIPSOID:
+          {
+            auto node = doc.NewElement("superellipsoid");
+            const auto superellipsoid = boost::get<Geometry::Superellipsoid>(visual.geometry.data);
+            set_vec3d(node, "size", superellipsoid.size);
+            node->SetAttribute("epsilon1", std::to_string(superellipsoid.epsilon1).c_str());
+            node->SetAttribute("epsilon2", std::to_string(superellipsoid.epsilon2).c_str());
+            geometry_node->InsertEndChild(node);
+          }
+          break;
+          case Geometry::Type::UNKNOWN:
+            break;
         }
+        visual_node->InsertEndChild(geometry_node);
+
+        node->InsertEndChild(visual_node);
       }
     };