diff --git a/collada_urdf/include/collada_urdf/ColladaWriter.h b/collada_urdf/include/collada_urdf/collada_writer.h similarity index 80% rename from collada_urdf/include/collada_urdf/ColladaWriter.h rename to collada_urdf/include/collada_urdf/collada_writer.h index f0daa5e..69025fc 100644 --- a/collada_urdf/include/collada_urdf/ColladaWriter.h +++ b/collada_urdf/include/collada_urdf/collada_writer.h @@ -59,6 +59,36 @@ public: ColladaWriterException(std::string const& what) : std::runtime_error(what) { } }; +/** Constructs a COLLADA DOM from a file, given the file name + * \param file The filename from where to read the XML + * \param dom The resulting COLLADA DOM + * \return true on success, false on failure + */ +bool colladaFromFile(std::string const& file, boost::shared_ptr& dom); + +/** Constructs a COLLADA DOM from a string containing XML + * \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); + +/** Constructs a COLLADA DOM from a TiXmlDocument + * \param xml_doc The TiXmlDocument containing the XML description of the robot + * \param dom The resulting COLLADA DOM + * \return true on success, false on failure + */ +bool colladaFromXml(TiXmlDocument* xml_doc, boost::shared_ptr& dom); + +/** Constructs 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); + +// + class Mesh; class ColladaWriter : public daeErrorHandler @@ -75,29 +105,10 @@ private: }; public: - /** \brief Create a ColladaWriter to import URDF from the specified file. - * - * \param filename The name of the URDF file to convert - * \throws ColladaWriterException if file could not be opened, or URDF could not be parsed - */ - 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 robot, std::string const& source); - + ColladaWriter(urdf::Model const& robot); virtual ~ColladaWriter(); - /** \brief Write the model to a COLLADA file. - * - * \param documentName The filename of the document to write to - * \return True if the file was successfully written - * \throws ColladaWriterException if an error occurred writing the COLLADA file - */ - void writeDocument(std::string const& documentName); + boost::shared_ptr convert(); protected: virtual void handleError(daeString msg); @@ -132,10 +143,7 @@ private: std::string getTimeStampString() const; private: - boost::shared_ptr robot_; - std::string source_; - - DAE* dae_; + urdf::Model robot_; boost::shared_ptr collada_; domCOLLADA* dom_; domCOLLADA::domSceneRef scene_; diff --git a/collada_urdf/include/collada_urdf/STLLoader.h b/collada_urdf/include/collada_urdf/stl_loader.h similarity index 100% rename from collada_urdf/include/collada_urdf/STLLoader.h rename to collada_urdf/include/collada_urdf/stl_loader.h diff --git a/collada_urdf/src/ColladaWriter.cpp b/collada_urdf/src/collada_writer.cpp similarity index 94% rename from collada_urdf/src/ColladaWriter.cpp rename to collada_urdf/src/collada_writer.cpp index 507ca31..90364e0 100644 --- a/collada_urdf/src/ColladaWriter.cpp +++ b/collada_urdf/src/collada_writer.cpp @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "collada_urdf/ColladaWriter.h" +#include "collada_urdf/collada_writer.h" -#include "collada_urdf/STLLoader.h" +#include "collada_urdf/stl_loader.h" #include #include @@ -49,40 +49,66 @@ using boost::shared_ptr; namespace collada_urdf { -ColladaWriter::ColladaWriter(std::string const& filename) - : source_(filename), dae_(NULL), dom_(NULL) -{ - TiXmlDocument xml; - if (!xml.LoadFile(filename.c_str())) - throw ColladaWriterException("Error reading XML file"); +bool colladaFromFile(std::string const& file, boost::shared_ptr& dom) { + TiXmlDocument urdf_xml; + if (!urdf_xml.LoadFile(file)) { + ROS_ERROR("Could not load XML file"); + return false; + } - TiXmlElement* robot_xml = xml.FirstChildElement("robot"); - if (!robot_xml) - throw ColladaWriterException("Error parsing URDF model from XML (robot element not found)"); - - robot_ = shared_ptr(new urdf::Model); - if (!robot_->initXml(robot_xml)) - throw ColladaWriterException("Error parsing URDF model from XML"); + return colladaFromXml(&urdf_xml, dom); } -ColladaWriter::ColladaWriter(shared_ptr robot, string const& source) - : robot_(robot), source_(source), dae_(NULL), dom_(NULL) -{ +bool colladaFromString(std::string const& xml, boost::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); } -void ColladaWriter::writeDocument(string const& documentName) { - initDocument(documentName); +bool colladaFromXml(TiXmlDocument* xml_doc, boost::shared_ptr& dom) { + urdf::Model robot_model; + if (!robot_model.initXml(xml_doc)) { + ROS_ERROR("Could not generate robot model"); + return false; + } - SCENE scene = createScene(); + return colladaFromUrdfModel(robot_model, dom); +} - setupPhysics(scene); - addGeometries(); - addKinematics(scene); - addVisuals(scene); - addMaterials(); - addBindings(scene); +bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr& dom) { + ColladaWriter writer(robot_model); + dom = writer.convert(); - collada_->writeAll(); + return dom != shared_ptr(); +} + +// + +ColladaWriter::ColladaWriter(urdf::Model const& robot) : robot_(robot), dom_(NULL) { } + +shared_ptr ColladaWriter::convert() { + try { + initDocument("collada_urdf"); + + SCENE scene = createScene(); + + setupPhysics(scene); + addGeometries(); + addKinematics(scene); + addVisuals(scene); + addMaterials(); + addBindings(scene); + + return collada_; + } + catch (ColladaWriterException ex) { + ROS_ERROR("Error converting: %s", ex.what()); + return shared_ptr(); + } } ColladaWriter::~ColladaWriter() { @@ -98,15 +124,13 @@ void ColladaWriter::handleError(daeString msg) { } void ColladaWriter::handleWarning(daeString msg) { - std::cerr << "COLLADA warning: " << msg << std::endl; + std::cerr << "COLLADA DOM warning: " << msg << std::endl; } void ColladaWriter::initDocument(string const& documentName) { daeErrorHandler::setErrorHandler(this); - dae_ = new DAE(); - - collada_.reset(dae_); + collada_.reset(new DAE); collada_->setIOPlugin(NULL); collada_->setDatabase(NULL); @@ -133,9 +157,6 @@ void ColladaWriter::initDocument(string const& documentName) { 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"); @@ -215,7 +236,7 @@ void ColladaWriter::setupPhysics(SCENE const& scene) { void ColladaWriter::addGeometries() { int link_num = 0; - for (map >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { + for (map >::const_iterator i = robot_.links_.begin(); i != robot_.links_.end(); i++) { shared_ptr urdf_link = i->second; if (urdf_link->visual == NULL || urdf_link->visual->geometry == NULL) @@ -397,7 +418,7 @@ void ColladaWriter::buildMeshFromSTLLoader(shared_ptr stl_mesh, daeElement void ColladaWriter::addJoints(daeElementRef parent) { int joint_num = 0; - for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { + for (map >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) { shared_ptr urdf_joint = i->second; // @@ -553,7 +574,7 @@ void ColladaWriter::addBindings(SCENE const& scene) { kmodel_bind->setNode("v1.node0"); // @todo daeSafeCast(kmodel_bind->createAndPlace(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str()); - for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { + for (map >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) { shared_ptr urdf_joint = i->second; int idof = 0; // @todo assuming 1 dof joints @@ -587,7 +608,7 @@ void ColladaWriter::addKinematics(SCENE const& scene) { // domKinematics_modelRef kmodel = daeSafeCast(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL)); kmodel->setId("k1"); - kmodel->setName(robot_->getName().c_str()); + kmodel->setName(robot_.getName().c_str()); { // domKinematics_model_techniqueRef technique = daeSafeCast(kmodel->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); @@ -596,7 +617,7 @@ void ColladaWriter::addKinematics(SCENE const& scene) { // int link_num = 0; - addKinematicLink(robot_->getRoot(), technique, link_num); + addKinematicLink(robot_.getRoot(), technique, link_num); // } kmodel_ = kmodel; @@ -621,7 +642,7 @@ void ColladaWriter::addKinematics(SCENE const& scene) { } // - for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { + for (map >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) { shared_ptr urdf_joint = i->second; int idof = 0; // @todo assuming 1 dof joints @@ -670,7 +691,7 @@ void ColladaWriter::addKinematicLink(shared_ptr urdf_link, dae { addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation); addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position); - addKinematicLink(robot_->getLink(urdf_joint->child_link_name), attachment_full, link_num); + addKinematicLink(robot_.getLink(urdf_joint->child_link_name), attachment_full, link_num); } // } @@ -681,10 +702,10 @@ void ColladaWriter::addVisuals(SCENE const& scene) { // domNodeRef root_node = daeSafeCast(scene.vscene->createAndPlace(COLLADA_ELEMENT_NODE)); root_node->setId("v1"); - root_node->setName(robot_->getName().c_str()); + root_node->setName(robot_.getName().c_str()); { int link_num = 0; - addVisualLink(robot_->getRoot(), root_node, link_num); + addVisualLink(robot_.getRoot(), root_node, link_num); } } @@ -693,7 +714,7 @@ void ColladaWriter::addMaterials() { ambient.init("1 1 1 0"); diffuse.init("1 1 1 0"); - for (map >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { + for (map >::const_iterator i = robot_.links_.begin(); i != robot_.links_.end(); i++) { shared_ptr urdf_link = i->second; map::const_iterator j = geometry_ids_.find(urdf_link->name); diff --git a/collada_urdf/src/STLLoader.cpp b/collada_urdf/src/stl_loader.cpp similarity index 99% rename from collada_urdf/src/STLLoader.cpp rename to collada_urdf/src/stl_loader.cpp index 138f56c..7278c05 100644 --- a/collada_urdf/src/STLLoader.cpp +++ b/collada_urdf/src/stl_loader.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#include "collada_urdf/STLLoader.h" +#include "collada_urdf/stl_loader.h" #include #include