Allow to convert URDF color to SDF material tag

src/parser_urdf.cc

@@ -3202,6 +3202,51 @@ void CreateVisual(tinyxml2::XMLElement *_elem, urdf::LinkConstSharedPtr _link,
   // set additional data from extensions
   InsertSDFExtensionVisual(sdfVisual, _link->name);
 
+  if (_visual->material)
+  {
+    // Refer to this comment in github to understand the ambient and diffuse
+    // https://github.com/osrf/sdformat/pull/526#discussion_r623937715
+    auto materialTag = sdfVisual->FirstChildElement("material");
+    // If the material is not included by an extension then create it
+    if (materialTag == nullptr)
+    {
+      AddKeyValue(sdfVisual, "material", "");
+      materialTag = sdfVisual->FirstChildElement("material");
+    }
+    // If the specular and diffuse are defined by an extension then don't
+    // use the color values
+    if (materialTag->FirstChildElement("diffuse") == nullptr &&
+        materialTag->FirstChildElement("ambient") == nullptr)
+    {
+      if (materialTag->FirstChildElement("diffuse") == nullptr)
+      {
+        double color_diffuse[4];
+        color_diffuse[0] =
+          ignition::math::clamp(_visual->material->color.r / 0.8, 0.0, 1.0);
+        color_diffuse[1] =
+          ignition::math::clamp(_visual->material->color.g / 0.8, 0.0, 1.0);
+        color_diffuse[2] =
+          ignition::math::clamp(_visual->material->color.b / 0.8, 0.0, 1.0);
+        color_diffuse[3] = _visual->material->color.a;
+        AddKeyValue(materialTag, "diffuse", Values2str(4, color_diffuse));
+      }
+      if (materialTag->FirstChildElement("ambient") == nullptr)
+      {
+        double color_ambient[4];
+        color_ambient[0] =
+          ignition::math::clamp(
+            0.5 * _visual->material->color.r / 0.4, 0.0, 1.0);
+        color_ambient[1] =
+          ignition::math::clamp(
+            0.5 * _visual->material->color.g / 0.4, 0.0, 1.0);
+        color_ambient[2] =
+          ignition::math::clamp(
+            0.5 * _visual->material->color.b / 0.4, 0.0, 1.0);
+        color_ambient[3] = _visual->material->color.a;
+        AddKeyValue(materialTag, "ambient", Values2str(4, color_ambient));
+      }
+    }
+  }
   // end create _visual node
   _elem->LinkEndChild(sdfVisual);
 }

test/integration/fixed_joint_reduction_collision_visual_extension.urdf

@@ -11,7 +11,7 @@
         <sphere radius="0.02" />
       </geometry>
       <material name="">
-        <color rgba="0.1 0.1 0.1 1" />
+        <color rgba="0.0 0.0 0.0 1" />
       </material>
     </visual>
     <collision>
@@ -33,7 +33,7 @@
         <sphere radius="0.03" />
       </geometry>
       <material name="">
-        <color rgba="0.1 0.1 0.1 1" />
+        <color rgba="0.0 1.0 0.0 1" />
       </material>
     </visual>
     <collision>
@@ -60,7 +60,7 @@
         <sphere radius="0.03" />
       </geometry>
       <material name="">
-        <color rgba="0.1 0.1 0.1 1" />
+        <color rgba="0.0 0.0 0.0 1" />
       </material>
     </visual>
     <collision>
@@ -87,7 +87,7 @@
         <sphere radius="0.05" />
       </geometry>
       <material name="">
-        <color rgba="0.1 0.1 0.1 1" />
+        <color rgba="0.0 0.0 0.0 1" />
       </material>
     </visual>
     <collision>
@@ -167,4 +167,3 @@
     </visual>
   </gazebo>
 </robot>
-

test/integration/material.cc

@@ -167,3 +167,30 @@ TEST(Material, ValidColors)
   EXPECT_EQ(elem->Get<ignition::math::Color>("ambient"),
             ignition::math::Color(0, 0, 0, 1));
 }
+
+//////////////////////////////////////////////////
+TEST(Material, URDFValidColors)
+{
+  std::string testFile =
+      sdf::testing::TestFile("sdf", "material_valid.urdf");
+
+  sdf::Root root;
+  sdf::Errors errors = root.Load(testFile);
+  std::cout << errors << std::endl;
+
+  ASSERT_TRUE(errors.empty());
+
+  sdf::ElementPtr elem = root.Element()->GetElement("model")
+                                       ->GetElement("link")
+                                       ->GetElement("visual")
+                                       ->GetElement("material");
+  ASSERT_NE(elem, nullptr);
+  EXPECT_EQ(elem->Get<ignition::math::Color>("diffuse"),
+            ignition::math::Color(0.0, 1.0f, 1.0f, 1));
+  EXPECT_EQ(elem->Get<ignition::math::Color>("specular"),
+            ignition::math::Color(0.0, 0.0f, 0.0f, 1));
+  EXPECT_EQ(elem->Get<ignition::math::Color>("emissive"),
+            ignition::math::Color(0, 0, 0, 1));
+  EXPECT_EQ(elem->Get<ignition::math::Color>("ambient"),
+            ignition::math::Color(0.0, 1.0f, 1.0f, 1));
+}

test/sdf/material_valid.urdf

@@ -0,0 +1,18 @@
+<?xml version="1.0" ?>
+<robot name="cartopole">
+  <link name="slideBar">
+    <visual>
+      <geometry>
+        <box size="30 0.05 0.05"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="green">
+        <color rgba="0 0.8 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+</robot>