initial draft of collada to urdf parser (taken from OpenRave implementation) per ticket #4330

colladadom/collada-dom.patch

@@ -52,3 +52,25 @@ Index: dom/make/domTest.mk
  ifeq ($(os),ps3)
  # PS3 doesn't support C++ locales, so tell boost not to use them
  ccFlags += -DBOOST_NO_STD_LOCALE
+
+--- dom/src/dae/daeSIDResolver.cpp
++++ dom/src/dae/daeSIDResolver.cpp
+@@ -152,6 +152,18 @@
+ 	                         list<string>& remainingPart) {
+ 		remainingPart.clear();
+ 
++        // custom change for following instance urls (Rosen Diankov)
++        if ( strncmp( container->getElementName(), "instance_", 9 ) == 0 ) {
++            daeURI *uri = (daeURI*)container->getAttributeValue("url");
++            if ( uri != NULL && uri->getElement() != NULL ) {
++                daeElement *e = findWithDots( uri->getElement(), s, profile, finder, remainingPart );
++                if ( e != NULL ) {
++                    //found it
++                    return e;
++                }
++            }
++        }
++
+ 		// First see if the whole thing resolves correctly
+ 		if (daeElement* result = finder(container, s, profile))
+ 			return result;

urdf/CMakeLists.txt

@@ -22,7 +22,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 rosbuild_gensrv()
 
 #common commands for building c++ executables and libraries
-rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/model.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/link.cpp src/joint.cpp src/model.cpp src/collada_model_reader.cpp)
 #target_link_libraries(${PROJECT_NAME} another_library)
 rosbuild_add_boost_directories()
 #rosbuild_link_boost(${PROJECT_NAME} thread)

urdf/include/urdf/model.h

@@ -46,7 +46,6 @@
 
 namespace urdf{
 
-
 class Model
 {
 public:
@@ -86,7 +85,7 @@ public:
   /// \brief complete list of Materials
   std::map<std::string, boost::shared_ptr<Material> > materials_;
 
-private:
+protected:
   void clear();
 
   std::string name_;
@@ -105,13 +104,14 @@ private:
   /// it's time to find the root Link
   bool initRoot(std::map<std::string, std::string> &parent_link_tree);
 
-
+private:
   /// Model is restricted to a tree for now, which means there exists one root link
   ///  typically, root link is the world(inertial).  Where world is a special link
   /// or is the root_link_ the link attached to the world by PLANAR/FLOATING joint?
   ///  hmm...
   boost::shared_ptr<Link> root_link_;
   
+  friend class ColladaModelReader;
 };
 
 }

urdf/manifest.xml

@@ -12,7 +12,7 @@
 
   <depend package="tinyxml" />
   <depend package="roscpp" />
-
+  <depend package="colladadom" />
   <export>
     <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lurdf"/>
   </export>

urdf/src/model.cpp

@@ -41,6 +41,11 @@
 
 namespace urdf{
 
+bool urdfFromColladaFile(std::string const& daefilename, Model& model);
+bool urdfFromColladaData(std::string const& data, Model& model);
+bool urdfFromTiXML(TiXmlElement *robot_xml, Model& model);
+bool IsColladaFile(const std::string& filename);
+bool IsColladaData(const std::string& data);
 
 Model::Model()
 {
@@ -59,6 +64,10 @@ void Model::clear()
 
 bool Model::initFile(const std::string& filename)
 {
+  // necessary for COLLADA compatibility
+  if( 0&&IsColladaFile(filename) ) {
+    return urdfFromColladaFile(filename,*this);
+  }
   TiXmlDocument xml_doc;
   xml_doc.LoadFile(filename);
 
@@ -89,6 +98,11 @@ bool Model::initParam(const std::string& param)
 
 bool Model::initString(const std::string& xml_string)
 {
+  // necessary for COLLADA compatibility
+  if( IsColladaData(xml_string) ) {
+    return urdfFromColladaData(xml_string,*this);
+  }
+
   TiXmlDocument xml_doc;
   xml_doc.Parse(xml_string.c_str());
 
@@ -104,6 +118,13 @@ bool Model::initXml(TiXmlDocument *xml_doc)
     return false;
   }
 
+  // necessary for COLLADA compatibility
+  if( !!xml_doc->RootElement() ) {
+    if( std::string("COLLADA") == xml_doc->RootElement()->ValueStr() ) {
+      return urdfFromTiXML(xml_doc->RootElement(),*this);
+    }
+  }
+
   TiXmlElement *robot_xml = xml_doc->FirstChildElement("robot");
   if (!robot_xml)
   {
@@ -120,6 +141,11 @@ bool Model::initXml(TiXmlElement *robot_xml)
   ROS_DEBUG("Parsing robot xml");
   if (!robot_xml) return false;
 
+  // necessary for COLLADA compatibility
+  if( std::string("COLLADA") == robot_xml->ValueStr() ) {
+    return urdfFromTiXML(robot_xml,*this);
+  }
+
   // Get robot name
   const char *name = robot_xml->Attribute("name");
   if (!name)