collada_urdf: added binary STLLoader
This commit is contained in:
tfield
parent
ca385b20d4
commit
6fd7f8a489
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
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);
|
||||
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 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);
|
||||
// Import the mesh using STLLoader
|
||||
STLLoader loader;
|
||||
Mesh* mesh = loader.load(string(tmp_filename));
|
||||
delete mesh;
|
||||
|
||||
// Delete the temporary file
|
||||
unlink(tmp_filename);
|
||||
// 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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue