Merge pull request RobotLocomotion#1246 from patmarion/add-urdf-scale…

…-parsing parse the scale attribute of the mesh element if it exists
V1ns0n · Aug 11, 2015 · 49b88b2 · 49b88b2
2 parents 4e23c98 + 1ec9212
commit 49b88b2
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 3 deletions.
diff --git a/drake/systems/plants/RigidBodyManipulatorURDF.cpp b/drake/systems/plants/RigidBodyManipulatorURDF.cpp
@@ -350,8 +350,15 @@ bool parseGeometry(TiXmlElement* node, const map<string,string>& package_map, co
     }
     string filename(attr);
     string resolved_filename = resolveFilename(filename, package_map, root_dir);
-    element.setGeometry(DrakeShapes::Mesh(filename, resolved_filename));
+    DrakeShapes::Mesh mesh(filename, resolved_filename);
 
+    attr = shape_node->Attribute("scale");
+    if (attr) {
+      stringstream s(attr);
+      s >> mesh.scale;
+    }
+
+    element.setGeometry(mesh);
   } else {
     cerr << "Warning: geometry element has an unknown type and will be ignored." << endl;
   }

diff --git a/drake/systems/plants/shapes/Geometry.cpp b/drake/systems/plants/shapes/Geometry.cpp
@@ -153,11 +153,11 @@ namespace DrakeShapes
   }
 
   Mesh::Mesh(const string& filename)
-    : Geometry(MESH), filename(filename)
+    : Geometry(MESH), scale(1.0), filename(filename)
   {}
 
   Mesh::Mesh(const string& filename, const string& resolved_filename)
-    : Geometry(MESH), filename(filename), resolved_filename(resolved_filename)
+    : Geometry(MESH), scale(1.0), filename(filename), resolved_filename(resolved_filename)
   {}
 
   bool Mesh::extractMeshVertices(Matrix3Xd& vertex_coordinates) const 

diff --git a/drake/systems/plants/shapes/Geometry.h b/drake/systems/plants/shapes/Geometry.h
@@ -103,6 +103,7 @@ namespace DrakeShapes
       virtual void getPoints(Eigen::Matrix3Xd &points) const;
       virtual void getBoundingBoxPoints(Eigen::Matrix3Xd &points) const;
 
+      double scale;
       std::string filename;
       std::string resolved_filename;
       bool extractMeshVertices(Eigen::Matrix3Xd& vertex_coordinates) const;