collada_urdf: added binary STLLoader

collada_urdf/CMakeLists.txt

@@ -3,4 +3,4 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 set(ROS_BUILD_TYPE Debug)
 rosbuild_init()
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp)
+rosbuild_add_executable(urdf_to_collada src/urdf_to_collada.cpp src/STLLoader.cpp)

collada_urdf/src/STLLoader.cpp

@@ -0,0 +1,148 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redstributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Tim Field */
+
+// STLLoader.cpp
+
+#include <iostream>
+#include <string.h>
+#include <ctype.h>
+#include <stdio.h>
+
+#include "STLLoader.h"
+
+using namespace collada_urdf;
+
+Vector3::Vector3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { }
+
+bool Vector3::operator==(const Vector3& v) const {
+    return x == v.x && y == v.y && z == v.z;
+}
+
+Mesh::Mesh() {
+}
+
+bool Mesh::hasVertex(const Vector3& v) const {
+    for (std::vector<Vector3>::const_iterator i = vertices.begin(); i != vertices.end(); i++)
+        if (v == *i)
+            return true;
+
+    return false;
+}
+
+unsigned int Mesh::getVertexIndex(const Vector3& v) const {
+    for (unsigned int i = 0; i < vertices.size(); i++)
+        if (vertices[i] == v)
+            return true;
+
+    return false;
+}
+
+void Mesh::addVertex(const Vector3& v) {
+    vertices.push_back(v);
+}
+
+void Mesh::addNormal(const Vector3& n) {
+    normals.push_back(n);
+}
+
+void Mesh::addIndex(unsigned int i) {
+    indices.push_back(i);
+}
+
+//
+
+Mesh* STLLoader::load(const std::string& filename) {
+    Mesh* mesh = new Mesh();
+
+    FILE* file = fopen(filename.c_str(), "r");
+    readBinary(file, mesh);
+    fclose(file);
+
+    return mesh;
+}
+
+void STLLoader::readBinary(FILE* filein, Mesh* mesh) {
+    // 80 byte Header
+    for (int i = 0; i < 80; i++) 
+        fgetc(filein);
+
+    int face_num = readLongInt(filein);
+
+    std::cout << "Reading " << face_num << " faces " << std::endl;
+
+    for (int iface = 0; iface < face_num; iface++) {
+        Vector3 normal(readFloat(filein), readFloat(filein), readFloat(filein));
+        for (int i = 0; i < 3; i++) {
+            Vector3 vertex(readFloat(filein), readFloat(filein), readFloat(filein));            
+            if (!mesh->hasVertex(vertex)) {
+                mesh->addVertex(vertex);
+                mesh->addNormal(normal);
+            }
+            mesh->addIndex(mesh->getVertexIndex(vertex));
+        }
+        readShortInt(filein);  // 2 byte attribute
+    }
+}
+
+float STLLoader::readFloat(FILE* filein) {
+    float rval;
+    if (fread(&rval, sizeof(float), 1, filein) == 0)
+        std::cerr << "Error in STLLoader::readFloat" << std::endl;
+    return rval;
+}
+
+uint32_t STLLoader::readLongInt(FILE* filein) {
+    union
+    {
+        uint32_t yint;
+        char ychar[4];
+    } y;
+    y.ychar[0] = fgetc(filein);
+    y.ychar[1] = fgetc(filein);
+    y.ychar[2] = fgetc(filein);
+    y.ychar[3] = fgetc(filein);
+
+    return y.yint;
+}
+
+uint16_t STLLoader::readShortInt(FILE* filein) {
+    uint8_t c1 = fgetc(filein);
+    uint8_t c2 = fgetc(filein);
+
+    uint16_t ival = c1 | (c2 << 8);
+
+    return ival;
+}

collada_urdf/src/STLLoader.h

@@ -0,0 +1,96 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redstributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Tim Field */
+
+// STLLoader.h
+
+#ifndef COLLADA_URDF_STLLOADER_HH
+#define COLLADA_URDF_STLLOADER_HH
+
+#include <stdint.h>
+#include <string>
+#include <vector>
+
+#define LINE_MAX_LEN 256
+#define COR3_MAX 200000
+#define ORDER_MAX 10
+#define FACE_MAX 200000
+
+namespace collada_urdf
+{
+    class Vector3
+    {
+    public:
+        Vector3(float x, float y, float z);
+
+        bool operator==(const Vector3& v) const;
+
+        float x;
+        float y;
+        float z;
+    };
+
+    class Mesh
+    {
+    public:
+        Mesh();
+
+        bool         hasVertex(const Vector3& v) const;
+        unsigned int getVertexIndex(const Vector3& v) const;
+
+        void addVertex(const Vector3& v);
+        void addNormal(const Vector3& n);
+        void addIndex(unsigned int i);
+
+    private:
+        std::vector<Vector3>      vertices;
+        std::vector<Vector3>      normals;
+        std::vector<unsigned int> indices;
+    };
+
+    class STLLoader
+    {
+    public:
+        Mesh* load(const std::string& filename);
+
+    private:
+        void     readBinary(FILE* filein, Mesh* mesh);
+        uint32_t readLongInt(FILE* filein);
+        uint16_t readShortInt(FILE* filein);
+        float    readFloat(FILE* filein);
+    };
+}
+
+#endif

collada_urdf/src/urdf_to_collada.cpp

@@ -34,6 +34,8 @@
 
 /* Author: Tim Field */
 
+// urdf_to_collada.cpp
+
 #include <dae.h>
 #include <dae/daeErrorHandler.h>
 #include <dom/domCOLLADA.h>
@@ -54,8 +56,12 @@
 #include <urdf/model.h>
 #include <urdf/pose.h>
 
+#include "STLLoader.h"
+
 using namespace std;
 
+namespace collada_urdf {
+
 class ColladaWriter : public daeErrorHandler
 {
 public:
@@ -226,6 +232,9 @@ public:
         for (map<string, boost::shared_ptr<urdf::Link> >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) {
             boost::shared_ptr<urdf::Link> urdf_link = i->second;
 
+            if (urdf_link->visual == NULL || urdf_link->visual->geometry == NULL)
+                continue;
+
             switch (urdf_link->visual->geometry->type) {
                 case urdf::Geometry::MESH: {
                     urdf::Mesh* mesh = (urdf::Mesh*) urdf_link->visual->geometry.get();
@@ -270,24 +279,34 @@ public:
             return;
         }
 
-        // Write the mesh to a temporary file
+        if (true) {
-        char tmp_filename[] = "/tmp/collada_urdf_mesh_XXXXXX";
+            // Write the resource to a temporary file
-        int mesh_fd = mkstemp(tmp_filename);
+            char tmp_filename[] = "/tmp/collada_urdf_XXXXXX";
-        write(mesh_fd, resource.data.get(), resource.size);
+            int fd = mkstemp(tmp_filename);
-        close(mesh_fd);
+            write(fd, resource.data.get(), resource.size);
+            close(fd);
 
-        // Import the mesh using assimp
+            // Import the mesh using STLLoader
-        const aiScene* scene = importer_.ReadFile(tmp_filename, aiProcess_SortByPType /* aiProcess_CalcTangentSpace | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices */);
+            STLLoader loader;
-        if (!scene)
+            Mesh* mesh = loader.load(string(tmp_filename));
-            cerr << "Unable to import mesh " << filename << ": " << importer_.GetErrorString() << endl;
+            delete mesh;
-        else
-            buildMesh(scene->mRootNode, mesh);
             
             // Delete the temporary file
             unlink(tmp_filename);
         }
+        else {        
+            // Import the mesh using assimp
+            const aiScene* scene = importer_.ReadFileFromMemory(resource.data.get(), resource.size, aiProcess_SortByPType /* aiProcess_CalcTangentSpace | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices */);
+            if (!scene)
+                cerr << "Unable to import mesh " << filename << ": " << importer_.GetErrorString() << endl;
+            else {
+                cout << "Successfully loaded mesh " << filename << endl;
+                buildMeshFromAssimp(scene->mRootNode, mesh);
+            }
+        }
+    }
 
-    void buildMesh(aiNode* node, daeElementRef parent) {
+    void buildMeshFromAssimp(aiNode* node, daeElementRef parent) {
         if (node == NULL)
             return;
 
@@ -392,7 +411,7 @@ public:
             }
 
             for (unsigned int i = 0; i < node->mNumChildren; i++)
-                buildMesh(node->mChildren[i], mesh);
+                buildMeshFromAssimp(node->mChildren[i], mesh);
         }
     }
 
@@ -564,6 +583,8 @@ public:
     }
 };
 
+}
+
 int main(int argc, char** argv)
 {
     if (argc != 2) {
@@ -585,7 +606,7 @@ int main(int argc, char** argv)
         return -1;
     }
 
-    ColladaWriter* writer = new ColladaWriter(&robot);
+    collada_urdf::ColladaWriter* writer = new collada_urdf::ColladaWriter(&robot);
     writer->writeScene();
     delete writer;