Add Urdf features: multiple visuals and material linking

src/urdf/UrdfLink.js

@@ -14,11 +14,13 @@ var UrdfVisual = require('./UrdfVisual');
  */
 function UrdfLink(options) {
   this.name = options.xml.getAttribute('name');
+  this.visuals = [];
   var visuals = options.xml.getElementsByTagName('visual');
-  if (visuals.length > 0) {
-    this.visual = new UrdfVisual({
-      xml : visuals[0]
-    });
+
+  for( var i=0; i<visuals.length; i++ ) {
+    this.visuals.push( new UrdfVisual({
+      xml : visuals[i]
+    }) );
   }
 }
 

src/urdf/UrdfMaterial.js

@@ -34,4 +34,14 @@ function UrdfMaterial(options) {
   }
 }
 
-module.exports = UrdfMaterial;
+UrdfMaterial.prototype.isLink = function() {
+  return this.color === null && this.textureFilename === null;
+};
+
+var assign = require('object-assign');
+
+UrdfMaterial.prototype.assign = function(obj) {
+    return assign(this, obj);
+};
+
+module.exports = UrdfMaterial;

src/urdf/UrdfModel.js

@@ -50,7 +50,11 @@ function UrdfModel(options) {
       });
       // Make sure this is unique
       if (this.materials[material.name] !== void 0) {
-        console.warn('Material ' + material.name + 'is not unique.');
+        if( this.materials[material.name].isLink() ) {
+          this.materials[material.name].assign( material );
+        } else {
+          console.warn('Material ' + material.name + 'is not unique.');
+        }
       } else {
         this.materials[material.name] = material;
       }
@@ -63,11 +67,15 @@ function UrdfModel(options) {
         console.warn('Link ' + link.name + ' is not unique.');
       } else {
         // Check for a material
-        if (link.visual && link.visual.material) {
-          if (this.materials[link.visual.material.name] !== void 0) {
-            link.visual.material = this.materials[link.visual.material.name];
-          } else {
-            this.materials[link.visual.material.name] = link.visual.material;
+        for( var j=0; j<link.visuals.length; j++ )
+        {
+          var mat = link.visuals[j].material; 
+          if ( mat !== null ) {
+            if (this.materials[mat.name] !== void 0) {
+              link.visuals[j].material = this.materials[mat.name];
+            } else {
+              this.materials[mat.name] = mat;
+            }
           }
         }
 

test/urdf.test.js

@@ -38,6 +38,20 @@ var sample_urdf = function (){
     '      </material>'+
     '    </visual>'+
     '  </link>'+
+    '  <link name="link5">'+ // link with referenced material and multiple visuals
+    '    <visual>'+
+    '      <geometry>'+
+    '        <box size="1 1 1" />'+
+    '      </geometry>'+
+    '      <material name="blue" />'+
+    '    </visual>'+
+    '    <visual>'+
+    '      <geometry>'+
+    '        <box size="2 2 2" />'+
+    '      </geometry>'+
+    '      <material name="blue" />'+
+    '    </visual>'+
+    '  </link>'+
     '  <joint name="joint1" type="continuous">'+
     '    <parent link="link1"/>'+
     '    <child link="link2"/>'+
@@ -50,6 +64,9 @@ var sample_urdf = function (){
     '    <parent link="link3"/>'+
     '    <child link="link4"/>'+
     '  </joint>'+
+    '  <material name="blue">'+
+    '    <color rgba="0 0 1 1" />'+
+    '  </material>'+
     '</robot>';
 }
 
@@ -71,18 +88,27 @@ describe('URDF', function() {
       });
 
       // Check all the visual elements
-      expect(urdfModel.links['link1'].visual.geometry.radius).to.equal(1.0);
-      expect(urdfModel.links['link2'].visual.geometry.dimension.x).to.equal(0.5);
-      expect(urdfModel.links['link2'].visual.geometry.dimension.y).to.equal(0.5);
-      expect(urdfModel.links['link2'].visual.geometry.dimension.z).to.equal(0.5);
-      expect(urdfModel.links['link3'].visual.geometry.length).to.equal(2.0);
-      expect(urdfModel.links['link3'].visual.geometry.radius).to.equal(0.2);
+      expect(urdfModel.links['link1'].visuals.length).to.equal(1);
+      expect(urdfModel.links['link1'].visuals[0].geometry.radius).to.equal(1.0);
+      expect(urdfModel.links['link2'].visuals[0].geometry.dimension.x).to.equal(0.5);
+      expect(urdfModel.links['link2'].visuals[0].geometry.dimension.y).to.equal(0.5);
+      expect(urdfModel.links['link2'].visuals[0].geometry.dimension.z).to.equal(0.5);
+      expect(urdfModel.links['link3'].visuals[0].geometry.length).to.equal(2.0);
+      expect(urdfModel.links['link3'].visuals[0].geometry.radius).to.equal(0.2);
+
+      expect(urdfModel.links['link4'].visuals.length).to.equal(1);
+      expect(urdfModel.links['link4'].visuals[0].material.name).to.equal('red');
+      expect(urdfModel.links['link4'].visuals[0].material.color.r).to.equal(1.0);
+      expect(urdfModel.links['link4'].visuals[0].material.color.g).to.equal(0);
+      expect(urdfModel.links['link4'].visuals[0].material.color.b).to.equal(0);
+      expect(urdfModel.links['link4'].visuals[0].material.color.a).to.equal(1.0);
 
-      expect(urdfModel.links['link4'].visual.material.name).to.equal('red');
-      expect(urdfModel.links['link4'].visual.material.color.r).to.equal(1.0);
-      expect(urdfModel.links['link4'].visual.material.color.g).to.equal(0);
-      expect(urdfModel.links['link4'].visual.material.color.b).to.equal(0);
-      expect(urdfModel.links['link4'].visual.material.color.a).to.equal(1.0);
+      expect(urdfModel.links['link5'].visuals.length).to.equal(2);
+      expect(urdfModel.links['link5'].visuals[0].material.name).to.equal('blue');
+      expect(urdfModel.links['link5'].visuals[0].material.color.r).to.equal(0.0);
+      expect(urdfModel.links['link5'].visuals[0].material.color.g).to.equal(0.0);
+      expect(urdfModel.links['link5'].visuals[0].material.color.b).to.equal(1.0);
+      expect(urdfModel.links['link5'].visuals[0].material.color.a).to.equal(1.0);
     });
 
     it('is ignorant to the xml node', function(){