Merge pull request RobotWebTools#60 from Vegekou/patch-1

Urdf shapes fix
k-aguete · Feb 14, 2014 · 68bf2b6 · 68bf2b6
2 parents 3f00bbc + 9ea58b2
commit 68bf2b6
Showing 1 changed file with 38 additions and 0 deletions.
diff --git a/src/urdf/Urdf.js b/src/urdf/Urdf.js
@@ -58,6 +58,44 @@ ROS3D.Urdf = function(options) {
             object : mesh
           });
           this.add(sceneNode);
+        } else {
+          var colorMaterial, shapeMesh;
+          // Save frameID
+          var frameID = '/' + link.name;
+          // Save color material
+          var color = link.visual.material.color;          
+          if (color === null)
+            colorMaterial = ROS3D.makeColorMaterial(0, 0, 0, 1);
+          else
+            colorMaterial = ROS3D.makeColorMaterial(color.r, color.g, color.b, color.a);
+          // Create a shape
+          switch (link.visual.geometry.type) {
+              case ROSLIB.URDF_BOX:
+                  var dimension = link.visual.geometry.dimension;
+                  var shapeGeom = new THREE.CubeGeometry(dimension.x, dimension.y, dimension.z);
+                  shapeMesh = new THREE.Mesh(shapeGeom, colorMaterial);
+                  break;
+              case ROSLIB.URDF_CYLINDER:
+                  var radius = link.visual.geometry.radius;
+                  var length = link.visual.geometry.length;
+                  var shapeGeom = new THREE.CylinderGeometry(radius, radius, length, 16, 1, false);
+                  shapeMesh = new THREE.Mesh(shapeGeom, colorMaterial);
+                  shapeMesh.useQuaternion = true;
+                  shapeMesh.quaternion.setFromAxisAngle(new THREE.Vector3(1, 0, 0), Math.PI * 0.5);
+                  break;
+              case ROSLIB.URDF_SPHERE:
+                  var shapeGeom = new THREE.SphereGeometry(link.visual.geometry.radius, 16);
+                  shapeMesh = new THREE.Mesh(shapeGeom, colorMaterial);
+                  break;
+          }
+          // Create a scene node with the shape
+          var sceneNode = new ROS3D.SceneNode({
+              frameID: frameID,
+              pose: link.visual.origin,
+              tfClient: tfClient,
+              object: shapeMesh
+          });
+          this.add(sceneNode);
         }
       }
     }