From 5b33005a0b01dbd413542b0e5a7a471b015aa421 Mon Sep 17 00:00:00 2001 From: tfield Date: Fri, 23 Apr 2010 19:49:15 +0000 Subject: [PATCH] collada_urdf: more tidying up --- .../include/collada_urdf/ColladaWriter.h | 38 ++++++--- collada_urdf/include/collada_urdf/STLLoader.h | 18 ++--- collada_urdf/src/ColladaWriter.cpp | 49 ++++++------ collada_urdf/src/STLLoader.cpp | 77 +++++++++++-------- collada_urdf/src/urdf_to_collada.cpp | 23 ++++-- 5 files changed, 120 insertions(+), 85 deletions(-) diff --git a/collada_urdf/include/collada_urdf/ColladaWriter.h b/collada_urdf/include/collada_urdf/ColladaWriter.h index 728438c..5f0f263 100644 --- a/collada_urdf/include/collada_urdf/ColladaWriter.h +++ b/collada_urdf/include/collada_urdf/ColladaWriter.h @@ -69,34 +69,48 @@ private: }; public: - ColladaWriter(std::string const& documentName, urdf::Model* robot); + /** + * \brief Create a ColladaWriter using the specified URDF robot model. + */ + ColladaWriter(urdf::Model* robot); + virtual ~ColladaWriter(); /** - * todo - * @return + * \brief Write the model to a COLLADA file. + * + * \param documentName The filename of the document to write to + * \return True if the file was successfully written */ - bool writeScene(); + bool writeDocument(std::string const& documentName); protected: virtual void handleError(daeString msg); virtual void handleWarning(daeString msg); private: + void initDocument(std::string const& documentName); SCENE createScene(); - void setupPhysics(SCENE scene); + + void setupPhysics(SCENE const& scene); + void addGeometries(); void loadMesh(std::string const& filename, domGeometryRef geometry, std::string const& geometry_id); bool loadMeshWithSTLLoader(resource_retriever::MemoryResource const& resource, domGeometryRef geometry, std::string const& geometry_id); - void buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, std::string const& geometry_id); + void buildMeshFromSTLLoader(boost::shared_ptr stl_mesh, daeElementRef parent, std::string const& geometry_id); + + void addKinematics(SCENE const& scene); void addJoints(daeElementRef parent); - void addBindings(SCENE scene); - void addKinematics(SCENE scene); void addKinematicLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num); - void addVisuals(SCENE scene); - void addMaterials(); - domEffectRef addEffect(std::string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse); - void addVisualLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num); + + void addVisuals(SCENE const& scene); + void addVisualLink(boost::shared_ptr urdf_link, daeElementRef parent, int& link_num); + + void addMaterials(); + domEffectRef addEffect(std::string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse); + + void addBindings(SCENE const& scene); + domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position); domRotateRef addRotate(daeElementRef parent, urdf::Rotation const& r); diff --git a/collada_urdf/include/collada_urdf/STLLoader.h b/collada_urdf/include/collada_urdf/STLLoader.h index 6bec856..c96395d 100644 --- a/collada_urdf/include/collada_urdf/STLLoader.h +++ b/collada_urdf/include/collada_urdf/STLLoader.h @@ -40,10 +40,7 @@ #include #include -#define LINE_MAX_LEN 256 -#define COR3_MAX 200000 -#define ORDER_MAX 10 -#define FACE_MAX 200000 +#include namespace collada_urdf { @@ -79,13 +76,16 @@ public: class STLLoader { public: - Mesh* load(std::string const& filename); + boost::shared_ptr load(std::string const& filename); private: - void readBinary (FILE* filein, Mesh* mesh); - uint32_t readLongInt (FILE* filein); - uint16_t readShortInt(FILE* filein); - float readFloat (FILE* filein); + FILE* file_; + boost::shared_ptr mesh_; + + void readBinary(); + uint32_t readLongInt(); + uint16_t readShortInt(); + float readFloat(); }; } diff --git a/collada_urdf/src/ColladaWriter.cpp b/collada_urdf/src/ColladaWriter.cpp index b6e626a..f7124d5 100644 --- a/collada_urdf/src/ColladaWriter.cpp +++ b/collada_urdf/src/ColladaWriter.cpp @@ -47,7 +47,26 @@ using boost::shared_ptr; namespace collada_urdf { -ColladaWriter::ColladaWriter(string const& documentName, urdf::Model* robot) : robot_(robot) { +ColladaWriter::ColladaWriter(urdf::Model* robot) : robot_(robot) { } + +bool ColladaWriter::writeDocument(string const& documentName) { + initDocument(documentName); + + SCENE scene = createScene(); + + setupPhysics(scene); + addGeometries(); + addKinematics(scene); + addVisuals(scene); + addMaterials(); + addBindings(scene); + + collada_->writeAll(); + + return true; +} + +void ColladaWriter::initDocument(string const& documentName) { daeErrorHandler::setErrorHandler(this); collada_.reset(new DAE()); @@ -113,21 +132,6 @@ ColladaWriter::~ColladaWriter() { DAE::cleanup(); } -bool ColladaWriter::writeScene() { - SCENE scene = createScene(); - - setupPhysics(scene); - addGeometries(); - addKinematics(scene); - addVisuals(scene); - addMaterials(); - addBindings(scene); - - collada_->writeAll(); - - return true; -} - // Implementation ColladaWriter::SCENE ColladaWriter::createScene() { @@ -171,7 +175,7 @@ void ColladaWriter::handleWarning(daeString msg) { std::cerr << "COLLADA warning: " << msg << std::endl; } -void ColladaWriter::setupPhysics(SCENE scene) { +void ColladaWriter::setupPhysics(SCENE const& scene) { // domPhysics_scene::domTechnique_commonRef common = daeSafeCast(scene.pscene->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); { @@ -258,9 +262,8 @@ bool ColladaWriter::loadMeshWithSTLLoader(resource_retriever::MemoryResource con // Import the mesh using STLLoader STLLoader loader; - Mesh* stl_mesh = loader.load(string(tmp_filename)); + shared_ptr stl_mesh = loader.load(string(tmp_filename)); buildMeshFromSTLLoader(stl_mesh, geometry, geometry_id); - delete stl_mesh; // Delete the temporary file unlink(tmp_filename); @@ -268,7 +271,7 @@ bool ColladaWriter::loadMeshWithSTLLoader(resource_retriever::MemoryResource con return true; } -void ColladaWriter::buildMeshFromSTLLoader(Mesh* stl_mesh, daeElementRef parent, string const& geometry_id) { +void ColladaWriter::buildMeshFromSTLLoader(shared_ptr stl_mesh, daeElementRef parent, string const& geometry_id) { // domMeshRef mesh = daeSafeCast(parent->createAndPlace(COLLADA_ELEMENT_MESH)); { @@ -498,7 +501,7 @@ void ColladaWriter::addJoints(daeElementRef parent) { } } -void ColladaWriter::addBindings(SCENE scene) { +void ColladaWriter::addBindings(SCENE const& scene) { string model_id = kmodel_->getID(); string inst_model_sid = string("inst_") + model_id; @@ -538,7 +541,7 @@ void ColladaWriter::addBindings(SCENE scene) { } } -void ColladaWriter::addKinematics(SCENE scene) { +void ColladaWriter::addKinematics(SCENE const& scene) { // domKinematics_modelRef kmodel = daeSafeCast(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL)); kmodel->setId("k1"); @@ -632,7 +635,7 @@ void ColladaWriter::addKinematicLink(shared_ptr urdf_link, dae // } -void ColladaWriter::addVisuals(SCENE scene) { +void ColladaWriter::addVisuals(SCENE const& scene) { // domNodeRef root_node = daeSafeCast(scene.vscene->createAndPlace(COLLADA_ELEMENT_NODE)); root_node->setId("v1"); diff --git a/collada_urdf/src/STLLoader.cpp b/collada_urdf/src/STLLoader.cpp index ada5ef4..d962f79 100644 --- a/collada_urdf/src/STLLoader.cpp +++ b/collada_urdf/src/STLLoader.cpp @@ -39,14 +39,21 @@ #include "collada_urdf/STLLoader.h" +using std::string; +using boost::shared_ptr; + namespace collada_urdf { +// Vector3 + Vector3::Vector3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { } -bool Vector3::operator==(const Vector3& v) const { +bool Vector3::operator==(Vector3 const& v) const { return x == v.x && y == v.y && z == v.z; } +// Mesh + Mesh::Mesh() { } @@ -58,70 +65,74 @@ int Mesh::getVertexIndex(const Vector3& v) const { return -1; } -void Mesh::addVertex(const Vector3& v) { vertices.push_back(v); } -void Mesh::addNormal(const Vector3& n) { normals.push_back(n); } +void Mesh::addVertex(Vector3 const& v) { vertices.push_back(v); } +void Mesh::addNormal(Vector3 const& 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(); +// STLLoader - FILE* file = fopen(filename.c_str(), "r"); - readBinary(file, mesh); - fclose(file); +shared_ptr STLLoader::load(std::string const& filename) { + mesh_ = shared_ptr(new Mesh); - return mesh; + file_ = fopen(filename.c_str(), "r"); + readBinary(); + fclose(file_); + file_ = NULL; + + return mesh_; } -void STLLoader::readBinary(FILE* filein, Mesh* mesh) { +void STLLoader::readBinary() { // 80 byte Header for (int i = 0; i < 80; i++) - fgetc(filein); + fgetc(file_); - int face_num = readLongInt(filein); + int face_num = readLongInt(); for (int iface = 0; iface < face_num; iface++) { - Vector3 normal(readFloat(filein), readFloat(filein), readFloat(filein)); + Vector3 normal(readFloat(), readFloat(), readFloat()); + for (int i = 0; i < 3; i++) { - double x = readFloat(filein); - double y = readFloat(filein); - double z = readFloat(filein); - Vector3 vertex(x,y,z); - int index = mesh->getVertexIndex(vertex); + Vector3 vertex(readFloat(), readFloat(), readFloat()); + + int index = mesh_->getVertexIndex(vertex); if (index == -1) { - mesh->addVertex(vertex); - mesh->addNormal(normal); - index = mesh->vertices.size() - 1; + mesh_->addVertex(vertex); + mesh_->addNormal(normal); + index = mesh_->vertices.size() - 1; } - mesh->addIndex(index); + mesh_->addIndex(index); } - readShortInt(filein); // 2 byte attribute + + readShortInt(); // 2 byte attribute } } -float STLLoader::readFloat(FILE* filein) { +float STLLoader::readFloat() { float rval; - if (fread(&rval, sizeof(float), 1, filein) == 0) + if (fread(&rval, sizeof(float), 1, file_) == 0) std::cerr << "Error in STLLoader::readFloat" << std::endl; + return rval; } -uint32_t STLLoader::readLongInt(FILE* filein) { +uint32_t STLLoader::readLongInt() { 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); + y.ychar[0] = fgetc(file_); + y.ychar[1] = fgetc(file_); + y.ychar[2] = fgetc(file_); + y.ychar[3] = fgetc(file_); return y.yint; } -uint16_t STLLoader::readShortInt(FILE* filein) { - uint8_t c1 = fgetc(filein); - uint8_t c2 = fgetc(filein); +uint16_t STLLoader::readShortInt() { + uint8_t c1 = fgetc(file_); + uint8_t c2 = fgetc(file_); uint16_t ival = c1 | (c2 << 8); diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index 1693f75..cce43ca 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -41,25 +41,32 @@ int main(int argc, char** argv) return -1; } + std::string input_filename(argv[1]); + std::string output_filename(argv[2]); + TiXmlDocument robot_model_xml; - robot_model_xml.LoadFile(argv[1]); + if (!robot_model_xml.LoadFile(input_filename.c_str())) { + std::cerr << "Error opening file " << argv[1] << std::endl; + return -1; + } + TiXmlElement* robot_xml = robot_model_xml.FirstChildElement("robot"); if (!robot_xml) { - std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl; + std::cerr << "Error parsing URDF model from XML" << std::endl; return -1; } urdf::Model robot; if (!robot.initXml(robot_xml)) { - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; + std::cerr << "Error parsing URDF model from XML" << std::endl; return -1; } - std::string output_filename(argv[2]); - - collada_urdf::ColladaWriter* writer = new collada_urdf::ColladaWriter(output_filename, &robot); - writer->writeScene(); - delete writer; + collada_urdf::ColladaWriter writer(&robot); + if (!writer.writeDocument(output_filename)) { + std::cerr << "Error writing document" << std::endl; + return -1; + } return 0; }