bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

collada_urdf: added colladaToFile

This commit is contained in:
tfield 2010-04-24 18:22:25 +00:00
parent 51126cf458
commit 3ab12d3c7f
6 changed files with 50 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
collada_urdf/include/collada_urdf/collada_urdf.h
View File

@ -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<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
* \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<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

2
collada_urdf/include/collada_urdf/collada_writer.h
View File

@ -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_;

15
collada_urdf/src/collada_urdf.cpp
View File

@ -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);
}
}

35
collada_urdf/src/collada_writer.cpp
View File

@ -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);
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<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();
if (!scene_)
// Create top-level elements
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));

4
collada_urdf/src/urdf_to_collada.cpp
View File

@ -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;

6
collada_urdf/test/test_collada_urdf.cpp
View File

@ -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) {