collada_urdf: added unit test, new ColladaWriter constructor

collada_urdf/CMakeLists.txt

@@ -4,3 +4,5 @@ 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 src/ColladaWriter.cpp src/STLLoader.cpp)
+
+rosbuild_add_gtest(test_collada_writer test/test_collada_writer.cpp src/ColladaWriter.cpp src/STLLoader.cpp)

collada_urdf/include/collada_urdf/ColladaWriter.h

@@ -76,10 +76,19 @@ private:
 
 public:
     /**
-     * \brief Create a ColladaWriter using the specified URDF robot model and a source string,
+     * \brief Create a ColladaWriter using the specified URDF filename.
-     * e.g. the name of the file the URDF was read from.
+     *
+     * \param filename The name of the URDF file to convert
      */
-    ColladaWriter(urdf::Model* robot, std::string const& source);
+    ColladaWriter(std::string const& filename);
+
+    /**
+     * \brief Create a ColladaWriter using the specified URDF robot model and source.
+     *
+     * \param robot The URDF model to write
+     * \param source The source of the model, e.g. the URL the URDF was read from
+     */
+    ColladaWriter(boost::shared_ptr<urdf::Model> robot, std::string const& source);
 
     virtual ~ColladaWriter();
 
@@ -124,9 +133,10 @@ private:
     std::string getTimeStampString() const;
 
 private:
-    urdf::Model* robot_;
+    boost::shared_ptr<urdf::Model> robot_;
     std::string                    source_;
 
+    DAE*                            dae_;
     boost::shared_ptr<DAE>          collada_;
     domCOLLADA*                     dom_;
     domCOLLADA::domSceneRef         scene_;

collada_urdf/src/ColladaWriter.cpp

@@ -49,8 +49,24 @@ using boost::shared_ptr;
 
 namespace collada_urdf {
 
-ColladaWriter::ColladaWriter(urdf::Model* robot, string const& source)
+ColladaWriter::ColladaWriter(std::string const& filename)
-    : robot_(robot), source_(source)
+    : source_(filename), dae_(NULL), dom_(NULL)
+{
+    TiXmlDocument xml;
+    if (!xml.LoadFile(filename.c_str()))
+        throw ColladaWriterException("Error opening file");
+
+    TiXmlElement* robot_xml = xml.FirstChildElement("robot");
+    if (!robot_xml)
+        throw ColladaWriterException("Error parsing URDF model from XML");
+
+    robot_ = shared_ptr<urdf::Model>(new urdf::Model);
+    if (!robot_->initXml(robot_xml))
+        throw ColladaWriterException("Error parsing URDF model from XML");
+}
+
+ColladaWriter::ColladaWriter(shared_ptr<urdf::Model> robot, string const& source)
+    : robot_(robot), source_(source), dae_(NULL), dom_(NULL)
 {
 }
 
@@ -71,10 +87,28 @@ bool ColladaWriter::writeDocument(string const& documentName) {
     return true;
 }
 
+ColladaWriter::~ColladaWriter() {
+    collada_.reset();
+
+    DAE::cleanup();
+}
+
+// Implementation
+
+void ColladaWriter::handleError(daeString msg) {
+    std::cerr << "COLLADA error: " << msg << std::endl;
+}
+
+void ColladaWriter::handleWarning(daeString msg) {
+    std::cerr << "COLLADA warning: " << msg << std::endl;
+}
+
 void ColladaWriter::initDocument(string const& documentName) {
     daeErrorHandler::setErrorHandler(this);
 
-    collada_.reset(new DAE());
+    dae_ = new DAE();
+
+    collada_.reset(dae_);
     collada_->setIOPlugin(NULL);
     collada_->setDatabase(NULL);
 
@@ -135,13 +169,6 @@ void ColladaWriter::initDocument(string const& documentName) {
     materialsLib_->setId("materials");
 }
 
-ColladaWriter::~ColladaWriter() {
-    collada_.reset();
-    DAE::cleanup();
-}
-
-// Implementation
-
 ColladaWriter::SCENE ColladaWriter::createScene() {
     SCENE s;
 
@@ -175,14 +202,6 @@ ColladaWriter::SCENE ColladaWriter::createScene() {
     return s;
 }
 
-void ColladaWriter::handleError(daeString msg) {
-    std::cerr << "COLLADA error: " << msg << std::endl;
-}
-
-void ColladaWriter::handleWarning(daeString msg) {
-    std::cerr << "COLLADA warning: " << msg << std::endl;
-}
-
 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));

collada_urdf/src/urdf_to_collada.cpp

@@ -56,13 +56,13 @@ int main(int argc, char** argv)
         return -1;
     }
 
-    urdf::Model robot;
+    boost::shared_ptr<urdf::Model> robot(new urdf::Model);
-    if (!robot.initXml(robot_xml)) {
+    if (!robot->initXml(robot_xml)) {
         std::cerr << "Error parsing URDF model from XML" << std::endl;
         return -1;
     }
 
-    collada_urdf::ColladaWriter writer(&robot, input_filename);
+    collada_urdf::ColladaWriter writer(robot, input_filename);
     if (!writer.writeDocument(output_filename)) {
         std::cerr << "Error writing document" << std::endl;
         return -1;

collada_urdf/test/test_collada_writer.cpp

@@ -0,0 +1,40 @@
+// Copyright (c) 2010, 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:
+//
+//     * Redistributions 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 Willow Garage, Inc. 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.
+
+#include "collada_urdf/ColladaWriter.h"
+
+#include <gtest/gtest.h>
+
+TEST(collada_urdf, collada_writer_writes)
+{
+    ASSERT_TRUE(collada_urdf::ColladaWriter("test/pr2.urdf").writeDocument("test/pr2.dae"));
+}
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}