From 6fd7f8a489ef6f984c7a8db787f36206c176d03a Mon Sep 17 00:00:00 2001 From: tfield Date: Fri, 5 Mar 2010 02:40:48 +0000 Subject: [PATCH] collada_urdf: added binary STLLoader --- collada_urdf/CMakeLists.txt | 2 +- collada_urdf/src/STLLoader.cpp | 148 +++++++++++++++++++++++++++ collada_urdf/src/STLLoader.h | 96 +++++++++++++++++ collada_urdf/src/urdf_to_collada.cpp | 57 +++++++---- 4 files changed, 284 insertions(+), 19 deletions(-) create mode 100644 collada_urdf/src/STLLoader.cpp create mode 100644 collada_urdf/src/STLLoader.h diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt index 037d9e2..0763be9 100644 --- a/collada_urdf/CMakeLists.txt +++ b/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) diff --git a/collada_urdf/src/STLLoader.cpp b/collada_urdf/src/STLLoader.cpp new file mode 100644 index 0000000..446310e --- /dev/null +++ b/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 +#include +#include +#include + +#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::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; +} diff --git a/collada_urdf/src/STLLoader.h b/collada_urdf/src/STLLoader.h new file mode 100644 index 0000000..d01e560 --- /dev/null +++ b/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 +#include +#include + +#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 vertices; + std::vector normals; + std::vector 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 diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index fabb82e..30efe46 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -34,6 +34,8 @@ /* Author: Tim Field */ +// urdf_to_collada.cpp + #include #include #include @@ -54,8 +56,12 @@ #include #include +#include "STLLoader.h" + using namespace std; +namespace collada_urdf { + class ColladaWriter : public daeErrorHandler { public: @@ -226,6 +232,9 @@ public: for (map >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { boost::shared_ptr 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(); @@ -269,25 +278,35 @@ public: cerr << "Unable to load mesh file " << filename << ": " << e.what() << endl; return; } - - // Write the mesh to a temporary file - char tmp_filename[] = "/tmp/collada_urdf_mesh_XXXXXX"; - int mesh_fd = mkstemp(tmp_filename); - write(mesh_fd, resource.data.get(), resource.size); - close(mesh_fd); - - // Import the mesh using assimp - const aiScene* scene = importer_.ReadFile(tmp_filename, aiProcess_SortByPType /* aiProcess_CalcTangentSpace | aiProcess_Triangulate | aiProcess_JoinIdenticalVertices */); - if (!scene) - cerr << "Unable to import mesh " << filename << ": " << importer_.GetErrorString() << endl; - else - buildMesh(scene->mRootNode, mesh); - // Delete the temporary file - unlink(tmp_filename); + if (true) { + // Write the resource to a temporary file + char tmp_filename[] = "/tmp/collada_urdf_XXXXXX"; + int fd = mkstemp(tmp_filename); + write(fd, resource.data.get(), resource.size); + close(fd); + + // Import the mesh using STLLoader + STLLoader loader; + Mesh* mesh = loader.load(string(tmp_filename)); + delete 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;