collada_urdf: added colladaToFile

collada_urdf/include/collada_urdf/collada_urdf.h

@@ -47,34 +47,41 @@
 
 namespace collada_urdf {
 
-/** Constructs a COLLADA DOM from a file, given the filename
+/** Construct a COLLADA DOM from an URDF file
- * \param file The filename from where to read the XML
+ * \param file The filename from where to read the URDF
  * \param dom The resulting COLLADA DOM
  * \return true on success, false on failure
  */
-bool colladaFromFile(std::string const& file, boost::shared_ptr<DAE>& dom);
+bool colladaFromUrdfFile(std::string const& file, boost::shared_ptr<DAE>& dom);
 
-/** Constructs a COLLADA DOM from a string containing XML
+/** Construct a COLLADA DOM from a string containing URDF
  * \param xml A string containing the XML description of the robot
  * \param dom The resulting COLLADA DOM
  * \return true on success, false on failure
  */
-bool colladaFromString(std::string const& xml, boost::shared_ptr<DAE>& dom);
+bool colladaFromUrdfString(std::string const& xml, boost::shared_ptr<DAE>& dom);
 
-/** Constructs a COLLADA DOM from a TiXmlDocument
+/** Construct a COLLADA DOM from a TiXmlDocument containing URDF
- * \param xml_doc The TiXmlDocument containing the XML description of the robot
+ * \param xml_doc The TiXmlDocument containing URDF
  * \param dom The resulting COLLADA DOM
  * \return true on success, false on failure
  */
-bool colladaFromXml(TiXmlDocument* xml_doc, boost::shared_ptr<DAE>& dom);
+bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr<DAE>& dom);
 
-/** Constructs a COLLADA DOM from a URDF robot model
+/** Construct a COLLADA DOM from a URDF robot model
  * \param robot_model The URDF robot model
  * \param dom The resulting COLLADA DOM
  * \return true on success, false on failure
  */
 bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr<DAE>& dom);
 
+/** Write a COLLADA DOM to a file
+ * \param dom COLLADA DOM to write
+ * \param file The filename to write the document to
+ * \return true on success, false on failure
+ */
+bool colladaToFile(boost::shared_ptr<DAE> dom, std::string const& file);
+
 }
 
 #endif

collada_urdf/include/collada_urdf/collada_writer.h

@@ -115,6 +115,8 @@ private:
     std::string getTimeStampString() const;
 
 private:
+    static int s_doc_count_;
+
     urdf::Model                     robot_;
     boost::shared_ptr<DAE>          collada_;
     domCOLLADA*                     dom_;

collada_urdf/src/collada_urdf.cpp

@@ -42,27 +42,27 @@ using boost::shared_ptr;
 
 namespace collada_urdf {
 
-bool colladaFromFile(string const& file, shared_ptr<DAE>& dom) {
+bool colladaFromUrdfFile(string const& file, shared_ptr<DAE>& dom) {
     TiXmlDocument urdf_xml;
     if (!urdf_xml.LoadFile(file)) {
         ROS_ERROR("Could not load XML file");
         return false;
     }
 
-    return colladaFromXml(&urdf_xml, dom);
+    return colladaFromUrdfXml(&urdf_xml, dom);
 }
 
-bool colladaFromString(string const& xml, shared_ptr<DAE>& dom) {
+bool colladaFromUrdfString(string const& xml, shared_ptr<DAE>& dom) {
     TiXmlDocument urdf_xml;
     if (urdf_xml.Parse(xml.c_str()) == 0) {
         ROS_ERROR("Could not parse XML document");
         return false;
     }
 
-    return colladaFromXml(&urdf_xml, dom);
+    return colladaFromUrdfXml(&urdf_xml, dom);
 }
 
-bool colladaFromXml(TiXmlDocument* xml_doc, shared_ptr<DAE>& dom) {
+bool colladaFromUrdfXml(TiXmlDocument* xml_doc, shared_ptr<DAE>& dom) {
     urdf::Model robot_model;
     if (!robot_model.initXml(xml_doc)) {
         ROS_ERROR("Could not generate robot model");
@@ -79,4 +79,9 @@ bool colladaFromUrdfModel(urdf::Model const& robot_model, shared_ptr<DAE>& dom)
     return dom != shared_ptr<DAE>();
 }
 
+bool colladaToFile(shared_ptr<DAE> dom, string const& file) {
+	daeString uri = dom->getDoc(0)->getDocumentURI()->getURI();
+	return dom->writeTo(uri, file);
+}
+
 }

collada_urdf/src/collada_writer.cpp

@@ -51,11 +51,22 @@ using boost::shared_ptr;
 
 namespace collada_urdf {
 
-ColladaWriter::ColladaWriter(urdf::Model const& robot) : robot_(robot), dom_(NULL) { }
+int ColladaWriter::s_doc_count_ = 0;
+
+ColladaWriter::ColladaWriter(urdf::Model const& robot) : robot_(robot), dom_(NULL) {
+    daeErrorHandler::setErrorHandler(this);
+
+    collada_.reset(new DAE);
+    collada_->setIOPlugin(NULL);
+    collada_->setDatabase(NULL);
+}
+
+ColladaWriter::~ColladaWriter() { }
 
 shared_ptr<DAE> ColladaWriter::convert() {
     try {
-        initDocument("collada_urdf");
+    	string doc_count_str = boost::lexical_cast<string>(s_doc_count_++);
+        initDocument(string("collada_urdf_") + doc_count_str + string(".dae"));
 
         SCENE scene = createScene();
 
@@ -74,12 +85,6 @@ shared_ptr<DAE> ColladaWriter::convert() {
     }
 }
 
-ColladaWriter::~ColladaWriter() {
-    collada_.reset();
-
-    DAE::cleanup();
-}
-
 // Implementation
 
 void ColladaWriter::handleError(daeString msg) {
@@ -91,12 +96,7 @@ void ColladaWriter::handleWarning(daeString msg) {
 }
 
 void ColladaWriter::initDocument(string const& documentName) {
-    daeErrorHandler::setErrorHandler(this);
+	// Create the document
-
-    collada_.reset(new DAE);
-    collada_->setIOPlugin(NULL);
-    collada_->setDatabase(NULL);
-
     daeDocument* doc = NULL;
     daeInt error = collada_->getDatabase()->insertDocument(documentName.c_str(), &doc); // also creates a collada root
     if (error != DAE_OK || doc == NULL)
@@ -105,7 +105,7 @@ void ColladaWriter::initDocument(string const& documentName) {
     dom_ = daeSafeCast<domCOLLADA>(doc->getDomRoot());
     dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML");
 
-    // Create the required asset tag
+    // Create asset elements
     domAssetRef asset = daeSafeCast<domAsset>(dom_->createAndPlace(COLLADA_ELEMENT_ASSET));
     {
         string date = getTimeStampString();
@@ -128,10 +128,8 @@ void ColladaWriter::initDocument(string const& documentName) {
         zup->setValue(UP_AXIS_Z_UP);
     }
 
-    scene_ = dom_->getScene();
+    // Create top-level elements
-    if (!scene_)
+    scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->createAndPlace(COLLADA_ELEMENT_SCENE));
-        scene_ = daeSafeCast<domCOLLADA::domScene>(dom_->createAndPlace(COLLADA_ELEMENT_SCENE));
-
     visualScenesLib_ = daeSafeCast<domLibrary_visual_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES));
     visualScenesLib_->setId("vscenes");
     geometriesLib_ = daeSafeCast<domLibrary_geometries>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES));
@@ -142,7 +140,6 @@ void ColladaWriter::initDocument(string const& documentName) {
     kinematicsModelsLib_->setId("kmodels");
     jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS));
     jointsLib_->setId("joints");
-
     physicsScenesLib_ = daeSafeCast<domLibrary_physics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES));
     physicsScenesLib_->setId("physics_scenes");
     effectsLib_ = daeSafeCast<domLibrary_effects>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS));

collada_urdf/src/urdf_to_collada.cpp

@@ -47,12 +47,12 @@ int main(int argc, char** argv)
     std::string output_filename(argv[2]);
 
     boost::shared_ptr<DAE> dom;
-    if (!collada_urdf::colladaFromFile(input_filename, dom)) {
+    if (!collada_urdf::colladaFromUrdfFile(input_filename, dom)) {
         std::cerr << std::endl << "Error converting document" << std::endl;
         return -1;
     }
 
-    dom->write("/u/tfield/test.dae");
+    collada_urdf::colladaToFile(dom, output_filename);
     std::cout << std::endl << "Document successfully written to " << output_filename << std::endl;
 
     return 0;

collada_urdf/test/test_collada_urdf.cpp

@@ -31,11 +31,11 @@
 
 #include <gtest/gtest.h>
 
-TEST(collada_urdf, collada_from_file_works)
+TEST(collada_urdf, collada_from_urdf_file_works)
 {
-    // An exception will be thrown on any error opening the URDF or writing the COLLADA file
     boost::shared_ptr<DAE> dom;
-    ASSERT_TRUE(collada_urdf::colladaFromFile("test/pr2.urdf", dom));
+    ASSERT_TRUE(collada_urdf::colladaFromUrdfFile("test/pr2.urdf", dom));
+    ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
 }
 
 int main(int argc, char **argv) {