collada_urdf: more tidying up

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
+     * \brief Write the model to a COLLADA file.
-     * @return
+     *
+     * \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<Mesh> 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 const> urdf_link, daeElementRef parent, int& link_num);
-    void addVisuals(SCENE scene);
+
-    void addMaterials();
+    void         addVisuals(SCENE const& scene);
-    domEffectRef    addEffect(std::string const& geometry_id, urdf::Color const& color_ambient, urdf::Color const& color_diffuse);
+    void         addVisualLink(boost::shared_ptr<urdf::Link const> urdf_link, daeElementRef parent, int& link_num);
-    void            addVisualLink(boost::shared_ptr<urdf::Link const> 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);
 

collada_urdf/include/collada_urdf/STLLoader.h

@@ -40,10 +40,7 @@
 #include <string>
 #include <vector>
 
-#define LINE_MAX_LEN 256
+#include <boost/shared_ptr.hpp>
-#define COR3_MAX 200000
-#define ORDER_MAX 10
-#define FACE_MAX 200000
 
 namespace collada_urdf {
 
@@ -79,13 +76,16 @@ public:
 class STLLoader
 {
 public:
-    Mesh* load(std::string const& filename);
+    boost::shared_ptr<Mesh> load(std::string const& filename);
 
 private:
-    void     readBinary  (FILE* filein, Mesh* mesh);
+    FILE*                   file_;
-    uint32_t readLongInt (FILE* filein);
+    boost::shared_ptr<Mesh> mesh_;
-    uint16_t readShortInt(FILE* filein);
+
-    float    readFloat   (FILE* filein);
+    void     readBinary();
+    uint32_t readLongInt();
+    uint16_t readShortInt();
+    float    readFloat();
 };
 
 }

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) {
     // <technique_common>
     domPhysics_scene::domTechnique_commonRef common = daeSafeCast<domPhysics_scene::domTechnique_common>(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<Mesh> 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<Mesh> stl_mesh, daeElementRef parent, string const& geometry_id) {
     // <mesh>
     domMeshRef mesh = daeSafeCast<domMesh>(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) {
     // <kinematics_model id="k1" name="pr2">
     domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL));
     kmodel->setId("k1");
@@ -632,7 +635,7 @@ void ColladaWriter::addKinematicLink(shared_ptr<const urdf::Link> urdf_link, dae
     // </link>
 }
 
-void ColladaWriter::addVisuals(SCENE scene) {
+void ColladaWriter::addVisuals(SCENE const& scene) {
     // <node id="v1" name="pr2">
     domNodeRef root_node = daeSafeCast<domNode>(scene.vscene->createAndPlace(COLLADA_ELEMENT_NODE));
     root_node->setId("v1");

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::addVertex(Vector3 const& v) { vertices.push_back(v); }
-void Mesh::addNormal(const Vector3& n) { normals.push_back(n);  }
+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) {
+// STLLoader
-    Mesh* mesh = new Mesh();
 
-    FILE* file = fopen(filename.c_str(), "r");
+shared_ptr<Mesh> STLLoader::load(std::string const& filename) {
-    readBinary(file, mesh);
+    mesh_ = shared_ptr<Mesh>(new Mesh);
-    fclose(file);
 
-    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);
+            Vector3 vertex(readFloat(), readFloat(), readFloat());
-            double y = readFloat(filein);
+
-            double z = readFloat(filein);
+            int index = mesh_->getVertexIndex(vertex);
-            Vector3 vertex(x,y,z);
-            int index = mesh->getVertexIndex(vertex);
             if (index == -1) {
-                mesh->addVertex(vertex);
+                mesh_->addVertex(vertex);
-                mesh->addNormal(normal);
+                mesh_->addNormal(normal);
-                index = mesh->vertices.size() - 1;
+                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[0] = fgetc(file_);
-    y.ychar[1] = fgetc(filein);
+    y.ychar[1] = fgetc(file_);
-    y.ychar[2] = fgetc(filein);
+    y.ychar[2] = fgetc(file_);
-    y.ychar[3] = fgetc(filein);
+    y.ychar[3] = fgetc(file_);
 
     return y.yint;
 }
 
-uint16_t STLLoader::readShortInt(FILE* filein) {
+uint16_t STLLoader::readShortInt() {
-    uint8_t c1 = fgetc(filein);
+    uint8_t c1 = fgetc(file_);
-    uint8_t c2 = fgetc(filein);
+    uint8_t c2 = fgetc(file_);
 
     uint16_t ival = c1 | (c2 << 8);
 

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(&robot);
-
+    if (!writer.writeDocument(output_filename)) {
-    collada_urdf::ColladaWriter* writer = new collada_urdf::ColladaWriter(output_filename, &robot);
+        std::cerr << "Error writing document" << std::endl;
-    writer->writeScene();
+        return -1;
-    delete writer;
+    }
 
     return 0;
 }