fix segfault on groovy problem ros/robot_model#4

jsk-ros-pkg · Feb 14, 2013 · db0ae3b · db0ae3b
1 parent 34f2089
commit db0ae3b
Show file tree

Hide file tree

Showing 2 changed files with 200 additions and 1 deletion.
diff --git a/jsk_ros_patch/collada_urdf_jsk_patch/Makefile b/jsk_ros_patch/collada_urdf_jsk_patch/Makefile
@@ -8,7 +8,7 @@ HG_URL = http://kforge.ros.org/robotmodel/robot_model
 HOGE=$(shell rosversion -d)
 ifeq ($(shell rosversion -d),groovy)
 HG_REVISION = `rosversion collada_urdf`
-HG_PATCH =  cyliner_box_sphere.patch set_url_name.patch cmake_catkin.patch
+HG_PATCH = collada.patch cyliner_box_sphere.patch set_url_name.patch cmake_catkin.patch
 HG_BIN_DIR  = `pwd`/$(HG_DIR)/collada_urdf/build/devel/lib/collada_urdf
 else
 HG_REVISION = robot_model-`rosversion robot_model`

diff --git a/jsk_ros_patch/collada_urdf_jsk_patch/collada.patch b/jsk_ros_patch/collada_urdf_jsk_patch/collada.patch
@@ -0,0 +1,199 @@
+diff -r 57cb2365cf54 collada_urdf/include/collada_urdf/collada_urdf.h
+--- a/collada_urdf/include/collada_urdf/collada_urdf.h	Sat Dec 22 15:41:12 2012 +0200
++++ b/collada_urdf/include/collada_urdf/collada_urdf.h	Fri Feb 15 02:32:02 2013 +0900
+@@ -50,40 +50,12 @@
+     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
++/** Write a COLLADA DOM to a file
+  * \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);
++bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, std::string const& file);
+
+ }
+
+diff -r 57cb2365cf54 collada_urdf/src/collada_urdf.cpp
+--- a/collada_urdf/src/collada_urdf.cpp	Sat Dec 22 15:41:12 2012 +0200
++++ b/collada_urdf/src/collada_urdf.cpp	Fri Feb 15 02:32:02 2013 +0900
+@@ -83,6 +83,8 @@
+
+ using namespace std;
+
++namespace ColladaDOM150 { }
++
+ namespace collada_urdf {
+
+ using namespace ColladaDOM150;
+@@ -593,24 +595,24 @@
+ 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
+@@ -672,14 +674,24 @@
+             _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);
+@@ -1762,8 +1774,9 @@
+     int _writeoptions;
+
+     const urdf::Model& _robot;
+-    boost::shared_ptr<DAE> _collada;
++    DAE _collada;
+     domCOLLADA* _dom;
++    daeDocument *_doc;
+     domCOLLADA::domSceneRef _globalscene;
+
+     domLibrary_visual_scenesRef _visualScenesLib;
+@@ -1792,45 +1805,13 @@
+ {
+ }
+
+-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;
++bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, string const& file) {
++    ColladaWriter writer(robot_model,0);
++    if ( ! writer.convert() ) {
++        std::cerr << std::endl << "Error converting document" << std::endl;
++        return -1;
+     }
+-
+-    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) {
+-    ColladaWriter writer(robot_model,0);
+-    dom = writer.convert();
+-    return dom != boost::shared_ptr<DAE>();
+-}
+-
+-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);
+ }
+
+ }
+diff -r 57cb2365cf54 collada_urdf/src/urdf_to_collada.cpp
+--- a/collada_urdf/src/urdf_to_collada.cpp	Sat Dec 22 15:41:12 2012 +0200
++++ b/collada_urdf/src/urdf_to_collada.cpp	Fri Feb 15 02:32:02 2013 +0900
+@@ -54,13 +54,7 @@
+         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;