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

fix segfault on groovy, not tested on fuerte see #4

This commit is contained in:
Kei Okada 2013-02-15 11:03:45 +09:00
parent 3ee9e5ab25
commit ff7c4f4ace
3 changed files with 31 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
collada_urdf/include/collada_urdf/collada_urdf.h
View File

@ -50,40 +50,12 @@ public:
ColladaUrdfException(std::string const& what);
};
/** 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 colladaFromUrdfFile(std::string const& file, boost::shared_ptr<DAE>& dom);
/** 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 colladaFromUrdfString(std::string const& xml, boost::shared_ptr<DAE>& dom);
/** 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 colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr<DAE>& dom);
/** 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 robot_model The URDF robot model
* \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);
bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, std::string const& file);
}

75
collada_urdf/src/collada_urdf.cpp
View File

@ -595,24 +595,24 @@ private:
public:
ColladaWriter(const urdf::Model& robot, int writeoptions) : _writeoptions(writeoptions), _robot(robot), _dom(NULL) {
daeErrorHandler::setErrorHandler(this);
_collada.reset(new DAE);
_collada->setIOPlugin(NULL);
_collada->setDatabase(NULL);
_importer.SetIOHandler(new ResourceIOSystem());
}
virtual ~ColladaWriter() {
}
boost::shared_ptr<DAE> convert()
daeDocument* doc() {
return _doc;
}
bool convert()
{
try {
const char* documentName = "urdf_snapshot";
daeDocument *doc = NULL;
daeInt error = _collada->getDatabase()->insertDocument(documentName, &doc ); // also creates a collada root
if (error != DAE_OK || doc == NULL) {
daeInt error = _collada.getDatabase()->insertDocument(documentName, &_doc ); // also creates a collada root
if (error != DAE_OK || _doc == NULL) {
throw ColladaUrdfException("Failed to create document");
}
_dom = daeSafeCast<domCOLLADA>(doc->getDomRoot());
_dom = daeSafeCast<domCOLLADA>(_doc->getDomRoot());
_dom->setAttribute("xmlns:math","http://www.w3.org/1998/Math/MathML");
//create the required asset tag
@ -674,14 +674,24 @@ public:
_WritePhysics();
_WriteRobot();
_WriteBindingsInstance_kinematics_scene();
return _collada;
return true;
}
catch (ColladaUrdfException ex) {
ROS_ERROR("Error converting: %s", ex.what());
return boost::shared_ptr<DAE>();
return false;
}
}
bool writeTo(string const& file) {
try {
daeString uri = _doc->getDocumentURI()->getURI();
_collada.writeTo(uri, file);
} catch (ColladaUrdfException ex) {
return false;
}
return true;
}
protected:
virtual void handleError(daeString msg) {
throw ColladaUrdfException(msg);
@ -1764,8 +1774,9 @@ private:
int _writeoptions;
const urdf::Model& _robot;
boost::shared_ptr<DAE> _collada;
DAE _collada;
domCOLLADA* _dom;
daeDocument *_doc;
domCOLLADA::domSceneRef _globalscene;
domLibrary_visual_scenesRef _visualScenesLib;
@ -1794,45 +1805,13 @@ ColladaUrdfException::ColladaUrdfException(std::string const& what)
{
}
bool colladaFromUrdfFile(string const& file, boost::shared_ptr<DAE>& dom) {
TiXmlDocument urdf_xml;
if (!urdf_xml.LoadFile(file)) {
ROS_ERROR("Could not load XML file");
return false;
}
return colladaFromUrdfXml(&urdf_xml, dom);
}
bool colladaFromUrdfString(string const& xml, boost::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 colladaFromUrdfXml(&urdf_xml, dom);
}
bool colladaFromUrdfXml(TiXmlDocument* xml_doc, boost::shared_ptr<DAE>& dom) {
urdf::Model robot_model;
if (!robot_model.initXml(xml_doc)) {
ROS_ERROR("Could not generate robot model");
return false;
}
return colladaFromUrdfModel(robot_model, dom);
}
bool colladaFromUrdfModel(urdf::Model const& robot_model, boost::shared_ptr<DAE>& dom) {
bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, string const& file) {
ColladaWriter writer(robot_model,0);
dom = writer.convert();
return dom != boost::shared_ptr<DAE>();
if ( ! writer.convert() ) {
std::cerr << std::endl << "Error converting document" << std::endl;
return -1;
}
bool colladaToFile(boost::shared_ptr<DAE> dom, string const& file) {
daeString uri = dom->getDoc(0)->getDocumentURI()->getURI();
return dom->writeTo(uri, file);
return writer.writeTo(file);
}
}

8
collada_urdf/src/urdf_to_collada.cpp
View File

@ -54,13 +54,7 @@ int main(int argc, char** argv)
ROS_ERROR("failed to open urdf file %s",input_filename.c_str());
}
boost::shared_ptr<DAE> dom;
if (!collada_urdf::colladaFromUrdfModel(robot_model, dom)) {
std::cerr << std::endl << "Error converting document" << std::endl;
return -1;
}
collada_urdf::colladaToFile(dom, output_filename);
collada_urdf::WriteUrdfModelToColladaFile(robot_model, output_filename);
std::cout << std::endl << "Document successfully written to " << output_filename << std::endl;
return 0;