From da0af98c438b88ca1733e2610224f044e7b2b66f Mon Sep 17 00:00:00 2001 From: tfield Date: Fri, 23 Apr 2010 21:01:11 +0000 Subject: [PATCH] collada_urdf: asset tag now writes correct create/modified dates and includes source --- .../include/collada_urdf/ColladaWriter.h | 14 +++++- collada_urdf/src/ColladaWriter.cpp | 46 ++++++++++++++----- collada_urdf/src/STLLoader.cpp | 7 +-- collada_urdf/src/urdf_to_collada.cpp | 2 +- 4 files changed, 52 insertions(+), 17 deletions(-) diff --git a/collada_urdf/include/collada_urdf/ColladaWriter.h b/collada_urdf/include/collada_urdf/ColladaWriter.h index 5f0f263..3120c87 100644 --- a/collada_urdf/include/collada_urdf/ColladaWriter.h +++ b/collada_urdf/include/collada_urdf/ColladaWriter.h @@ -53,6 +53,12 @@ namespace collada_urdf { +class ColladaWriterException : public std::runtime_error +{ +public: + ColladaWriterException(std::string const& what) : std::runtime_error(what) { } +}; + class Mesh; class ColladaWriter : public daeErrorHandler @@ -70,9 +76,10 @@ private: public: /** - * \brief Create a ColladaWriter using the specified URDF robot model. + * \brief Create a ColladaWriter using the specified URDF robot model and a source string, + * e.g. the name of the file the URDF was read from. */ - ColladaWriter(urdf::Model* robot); + ColladaWriter(urdf::Model* robot, std::string const& source); virtual ~ColladaWriter(); @@ -114,8 +121,11 @@ private: domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position); domRotateRef addRotate(daeElementRef parent, urdf::Rotation const& r); + std::string getTimeStampString() const; + private: urdf::Model* robot_; + std::string source_; boost::shared_ptr collada_; domCOLLADA* dom_; diff --git a/collada_urdf/src/ColladaWriter.cpp b/collada_urdf/src/ColladaWriter.cpp index f7124d5..8ed8720 100644 --- a/collada_urdf/src/ColladaWriter.cpp +++ b/collada_urdf/src/ColladaWriter.cpp @@ -36,6 +36,8 @@ #include "collada_urdf/STLLoader.h" +#include +#include #include #define foreach BOOST_FOREACH @@ -47,7 +49,10 @@ using boost::shared_ptr; namespace collada_urdf { -ColladaWriter::ColladaWriter(urdf::Model* robot) : robot_(robot) { } +ColladaWriter::ColladaWriter(urdf::Model* robot, string const& source) + : robot_(robot), source_(source) +{ +} bool ColladaWriter::writeDocument(string const& documentName) { initDocument(documentName); @@ -76,10 +81,7 @@ void ColladaWriter::initDocument(string const& documentName) { daeDocument* doc = NULL; daeInt error = collada_->getDatabase()->insertDocument(documentName.c_str(), &doc); // also creates a collada root if (error != DAE_OK || doc == NULL) - { - std::cerr << "Failed to create new document" << std::endl; - throw; - } + throw ColladaWriterException("Failed to create document"); dom_ = daeSafeCast(doc->getDomRoot()); dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML"); @@ -87,15 +89,21 @@ void ColladaWriter::initDocument(string const& documentName) { // Create the required asset tag domAssetRef asset = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_ASSET)); { + string date = getTimeStampString(); + domAsset::domCreatedRef created = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_CREATED)); - created->setValue("2009-04-06T17:01:00.891550"); // @todo: replace with current date + created->setValue(date.c_str()); domAsset::domModifiedRef modified = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_MODIFIED)); - modified->setValue("2009-04-06T17:01:00.891550"); // @todo: replace with current date + modified->setValue(date.c_str()); domAsset::domContributorRef contrib = daeSafeCast(asset->createAndPlace(COLLADA_TYPE_CONTRIBUTOR)); + domAsset::domContributor::domAuthoring_toolRef authoringtool = daeSafeCast(contrib->createAndPlace(COLLADA_ELEMENT_AUTHORING_TOOL)); authoringtool->setValue("URDF Collada Writer"); + domAsset::domContributor::domSource_dataRef sourcedata = daeSafeCast(contrib->createAndPlace(COLLADA_ELEMENT_SOURCE_DATA)); + sourcedata->setValue(source_.c_str()); + domAsset::domUnitRef units = daeSafeCast(asset->createAndPlace(COLLADA_ELEMENT_UNIT)); units->setMeter(1); units->setName("meter"); @@ -217,19 +225,19 @@ void ColladaWriter::addGeometries() { break; } case urdf::Geometry::SPHERE: { - std::cerr << "Warning: geometry type SPHERE of link " << urdf_link->name << " is unsupported" << std::endl; + std::cerr << "Warning: geometry type SPHERE of link " << urdf_link->name << " not exported" << std::endl; break; } case urdf::Geometry::BOX: { - std::cerr << "Warning: geometry type BOX of link " << urdf_link->name << " is unsupported" << std::endl; + std::cerr << "Warning: geometry type BOX of link " << urdf_link->name << " not exported" << std::endl; break; } case urdf::Geometry::CYLINDER: { - std::cerr << "Warning: geometry type CYLINDER of link " << urdf_link->name << " is unsupported" << std::endl; + std::cerr << "Warning: geometry type CYLINDER of link " << urdf_link->name << " not exported" << std::endl; break; } default: { - std::cerr << "Warning: geometry type " << urdf_link->visual->geometry->type << " of link " << urdf_link->name << " is supported" << std::endl; + std::cerr << "Warning: geometry type " << urdf_link->visual->geometry->type << " of link " << urdf_link->name << " not exported" << std::endl; break; } } @@ -833,4 +841,20 @@ domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const return rot; } +string ColladaWriter::getTimeStampString() const { + //"2009-04-06T17:01:00.891550" + + // facet becomes owned by locale, so no need to explicitly delete + boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s"); + + std::stringstream ss(std::stringstream::in | std::stringstream::out); + ss.imbue(std::locale(ss.getloc(), facet)); + ss << boost::posix_time::second_clock::local_time(); + + string date; + ss >> date; + + return date; +} + } diff --git a/collada_urdf/src/STLLoader.cpp b/collada_urdf/src/STLLoader.cpp index d962f79..88dfe6a 100644 --- a/collada_urdf/src/STLLoader.cpp +++ b/collada_urdf/src/STLLoader.cpp @@ -32,12 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include -#include +#include "collada_urdf/STLLoader.h" + #include #include +#include -#include "collada_urdf/STLLoader.h" +#include using std::string; using boost::shared_ptr; diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index cce43ca..e913bbe 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -62,7 +62,7 @@ int main(int argc, char** argv) return -1; } - collada_urdf::ColladaWriter writer(&robot); + collada_urdf::ColladaWriter writer(&robot, input_filename); if (!writer.writeDocument(output_filename)) { std::cerr << "Error writing document" << std::endl; return -1;