diff --git a/collada_urdf/include/collada_urdf/collada_urdf.h b/collada_urdf/include/collada_urdf/collada_urdf.h index a1d3f66..7030f82 100644 --- a/collada_urdf/include/collada_urdf/collada_urdf.h +++ b/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 - * \param file The filename from where to read the XML +/** Construct a COLLADA DOM from an URDF file + * \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& dom); +bool colladaFromUrdfFile(std::string const& file, boost::shared_ptr& 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& dom); +bool colladaFromUrdfString(std::string const& xml, boost::shared_ptr& dom); -/** Constructs a COLLADA DOM from a TiXmlDocument - * \param xml_doc The TiXmlDocument containing the XML description of the robot +/** Construct a COLLADA DOM from a TiXmlDocument containing URDF + * \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& dom); +bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr& 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& 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 dom, std::string const& file); + } #endif diff --git a/collada_urdf/include/collada_urdf/collada_writer.h b/collada_urdf/include/collada_urdf/collada_writer.h index d579d80..3bf0c30 100644 --- a/collada_urdf/include/collada_urdf/collada_writer.h +++ b/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 collada_; domCOLLADA* dom_; diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp index b407e74..69fbd3f 100644 --- a/collada_urdf/src/collada_urdf.cpp +++ b/collada_urdf/src/collada_urdf.cpp @@ -42,27 +42,27 @@ using boost::shared_ptr; namespace collada_urdf { -bool colladaFromFile(string const& file, shared_ptr& dom) { +bool colladaFromUrdfFile(string const& file, shared_ptr& 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& dom) { +bool colladaFromUrdfString(string const& xml, shared_ptr& 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& dom) { +bool colladaFromUrdfXml(TiXmlDocument* xml_doc, shared_ptr& 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& dom) return dom != shared_ptr(); } +bool colladaToFile(shared_ptr dom, string const& file) { + daeString uri = dom->getDoc(0)->getDocumentURI()->getURI(); + return dom->writeTo(uri, file); +} + } diff --git a/collada_urdf/src/collada_writer.cpp b/collada_urdf/src/collada_writer.cpp index 11b5b0e..7265365 100644 --- a/collada_urdf/src/collada_writer.cpp +++ b/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 ColladaWriter::convert() { try { - initDocument("collada_urdf"); + string doc_count_str = boost::lexical_cast(s_doc_count_++); + initDocument(string("collada_urdf_") + doc_count_str + string(".dae")); SCENE scene = createScene(); @@ -74,12 +85,6 @@ shared_ptr 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); - - collada_.reset(new DAE); - collada_->setIOPlugin(NULL); - collada_->setDatabase(NULL); - + // Create the document 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(doc->getDomRoot()); dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML"); - // Create the required asset tag + // Create asset elements domAssetRef asset = daeSafeCast(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(); - if (!scene_) - scene_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_SCENE)); - + // Create top-level elements + scene_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_SCENE)); visualScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_VISUAL_SCENES)); visualScenesLib_->setId("vscenes"); geometriesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_GEOMETRIES)); @@ -142,7 +140,6 @@ void ColladaWriter::initDocument(string const& documentName) { kinematicsModelsLib_->setId("kmodels"); jointsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS)); jointsLib_->setId("joints"); - physicsScenesLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_PHYSICS_SCENES)); physicsScenesLib_->setId("physics_scenes"); effectsLib_ = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_EFFECTS)); diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index 6081c79..5eb9d16 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/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 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; diff --git a/collada_urdf/test/test_collada_urdf.cpp b/collada_urdf/test/test_collada_urdf.cpp index 0c17a8e..895fa50 100644 --- a/collada_urdf/test/test_collada_urdf.cpp +++ b/collada_urdf/test/test_collada_urdf.cpp @@ -31,11 +31,11 @@ #include -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 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) {