mesh_loader.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "mesh_loader.h"
00031 #include <resource_retriever/retriever.h>
00032 
00033 #include <boost/filesystem.hpp>
00034 
00035 #include "ogre_helpers/stl_loader.h"
00036 
00037 #include <OGRE/OgreMeshManager.h>
00038 #include <OGRE/OgreTextureManager.h>
00039 #include <OGRE/OgreMaterialManager.h>
00040 #include <OGRE/OgreTexture.h>
00041 #include <OGRE/OgrePass.h>
00042 #include <OGRE/OgreTechnique.h>
00043 #include <OGRE/OgreMaterial.h>
00044 #include <OGRE/OgreTextureUnitState.h>
00045 #include <OGRE/OgreMeshSerializer.h>
00046 #include <OGRE/OgreSubMesh.h>
00047 #include <OGRE/OgreHardwareBufferManager.h>
00048 
00049 #include <tinyxml.h>
00050 
00051 
00052 #include <ros/assert.h>
00053 
00054 #if defined(IS_ASSIMP3)
00055 #include <assimp/Importer.hpp>
00056 #include <assimp/scene.h>
00057 #include <assimp/postprocess.h>
00058 #include <assimp/IOStream.hpp>
00059 #include <assimp/IOSystem.hpp>
00060 #else
00061 #include <assimp/assimp.hpp>
00062 #include <assimp/aiScene.h>
00063 #include <assimp/aiPostProcess.h>
00064 #include <assimp/IOStream.h>
00065 #include <assimp/IOSystem.h>
00066 #endif
00067 
00068 namespace fs = boost::filesystem;
00069 
00070 namespace rviz
00071 {
00072 
00073 class ResourceIOStream : public Assimp::IOStream
00074 {
00075 public:
00076   ResourceIOStream(const resource_retriever::MemoryResource& res)
00077   : res_(res)
00078   , pos_(res.data.get())
00079   {}
00080 
00081   ~ResourceIOStream()
00082   {}
00083 
00084   size_t Read(void* buffer, size_t size, size_t count)
00085   {
00086     size_t to_read = size * count;
00087     if (pos_ + to_read > res_.data.get() + res_.size)
00088     {
00089       to_read = res_.size - (pos_ - res_.data.get());
00090     }
00091 
00092     memcpy(buffer, pos_, to_read);
00093     pos_ += to_read;
00094 
00095     return to_read;
00096   }
00097 
00098   size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; }
00099 
00100   aiReturn Seek( size_t offset, aiOrigin origin)
00101   {
00102     uint8_t* new_pos = 0;
00103     switch (origin)
00104     {
00105     case aiOrigin_SET:
00106       new_pos = res_.data.get() + offset;
00107       break;
00108     case aiOrigin_CUR:
00109       new_pos = pos_ + offset; // TODO is this right?  can offset really not be negative
00110       break;
00111     case aiOrigin_END:
00112       new_pos = res_.data.get() + res_.size - offset; // TODO is this right?
00113       break;
00114     default:
00115       ROS_BREAK();
00116     }
00117 
00118     if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
00119     {
00120       return aiReturn_FAILURE;
00121     }
00122 
00123     pos_ = new_pos;
00124     return aiReturn_SUCCESS;
00125   }
00126 
00127   size_t Tell() const
00128   {
00129     return pos_ - res_.data.get();
00130   }
00131 
00132   size_t FileSize() const
00133   {
00134     return res_.size;
00135   }
00136 
00137   void Flush() {}
00138 
00139 private:
00140   resource_retriever::MemoryResource res_;
00141   uint8_t* pos_;
00142 };
00143 
00144 class ResourceIOSystem : public Assimp::IOSystem
00145 {
00146 public:
00147   ResourceIOSystem()
00148   {
00149   }
00150 
00151   ~ResourceIOSystem()
00152   {
00153   }
00154 
00155   // Check whether a specific file exists
00156   bool Exists(const char* file) const
00157   {
00158     // Ugly -- two retrievals where there should be one (Exists + Open)
00159     // resource_retriever needs a way of checking for existence
00160     // TODO: cache this
00161     resource_retriever::MemoryResource res;
00162     try
00163     {
00164       res = retriever_.get(file);
00165     }
00166     catch (resource_retriever::Exception& e)
00167     {
00168       return false;
00169     }
00170 
00171     return true;
00172   }
00173 
00174   // Get the path delimiter character we'd like to see
00175   char getOsSeparator() const
00176   {
00177     return '/';
00178   }
00179 
00180   // ... and finally a method to open a custom stream
00181   Assimp::IOStream* Open(const char* file, const char* mode = "rb")
00182   {
00183     ROS_ASSERT(mode == std::string("r") || mode == std::string("rb"));
00184 
00185     // Ugly -- two retrievals where there should be one (Exists + Open)
00186     // resource_retriever needs a way of checking for existence
00187     resource_retriever::MemoryResource res;
00188     try
00189     {
00190       res = retriever_.get(file);
00191     }
00192     catch (resource_retriever::Exception& e)
00193     {
00194       return 0;
00195     }
00196 
00197     return new ResourceIOStream(res);
00198   }
00199 
00200   void Close(Assimp::IOStream* stream);
00201 
00202 private:
00203   mutable resource_retriever::Retriever retriever_;
00204 };
00205 
00206 void ResourceIOSystem::Close(Assimp::IOStream* stream)
00207 {
00208   delete stream;
00209 }
00210 
00211 // Mostly stolen from gazebo
00216 void buildMesh( const aiScene* scene, const aiNode* node,
00217                 const Ogre::MeshPtr& mesh,
00218                 Ogre::AxisAlignedBox& aabb, float& radius, const float scale,
00219                 std::vector<Ogre::MaterialPtr>& material_table )
00220 {
00221   if (!node)
00222   {
00223     return;
00224   }
00225 
00226   aiMatrix4x4 transform = node->mTransformation;
00227   aiNode *pnode = node->mParent;
00228   while (pnode)
00229   {
00230     // Don't convert to y-up orientation, which is what the root node in
00231     // Assimp does
00232     if (pnode->mParent != NULL)
00233       transform = pnode->mTransformation * transform;
00234     pnode = pnode->mParent;
00235   }
00236 
00237   aiMatrix3x3 rotation(transform);
00238   aiMatrix3x3 inverse_transpose_rotation(rotation);
00239   inverse_transpose_rotation.Inverse();
00240   inverse_transpose_rotation.Transpose();
00241 
00242   for (uint32_t i = 0; i < node->mNumMeshes; i++)
00243   {
00244     aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
00245 
00246     Ogre::SubMesh* submesh = mesh->createSubMesh();
00247     submesh->useSharedVertices = false;
00248     submesh->vertexData = new Ogre::VertexData();
00249     Ogre::VertexData* vertex_data = submesh->vertexData;
00250     Ogre::VertexDeclaration* vertex_decl = vertex_data->vertexDeclaration;
00251 
00252     size_t offset = 0;
00253     // positions
00254     vertex_decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
00255     offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00256 
00257     // normals
00258     if (input_mesh->HasNormals())
00259     {
00260       vertex_decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL);
00261       offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00262     }
00263 
00264     // texture coordinates (only support 1 for now)
00265     if (input_mesh->HasTextureCoords(0))
00266     {
00267       vertex_decl->addElement(0, offset, Ogre::VET_FLOAT2, Ogre::VES_TEXTURE_COORDINATES, 0);
00268       offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT2);
00269     }
00270 
00271     // todo vertex colors
00272 
00273     // allocate the vertex buffer
00274     vertex_data->vertexCount = input_mesh->mNumVertices;
00275     Ogre::HardwareVertexBufferSharedPtr vbuf = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(vertex_decl->getVertexSize(0),
00276                                                                           vertex_data->vertexCount,
00277                                                                           Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY,
00278                                                                           false);
00279 
00280     vertex_data->vertexBufferBinding->setBinding(0, vbuf);
00281     float* vertices = static_cast<float*>(vbuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
00282 
00283     // Add the vertices
00284     for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
00285     {
00286       aiVector3D p = input_mesh->mVertices[j];
00287       p *= transform;
00288       p *= scale;
00289       *vertices++ = p.x;
00290       *vertices++ = p.y;
00291       *vertices++ = p.z;
00292 
00293       Ogre::Vector3 v(p.x, p.y, p.z);
00294       aabb.merge(v);
00295       float dist = v.length();
00296       if (dist > radius)
00297       {
00298         radius = dist;
00299       }
00300 
00301       if (input_mesh->HasNormals())
00302       {
00303         aiVector3D n = inverse_transpose_rotation * input_mesh->mNormals[j];
00304         n.Normalize();
00305         *vertices++ = n.x;
00306         *vertices++ = n.y;
00307         *vertices++ = n.z;
00308       }
00309 
00310       if (input_mesh->HasTextureCoords(0))
00311       {
00312         *vertices++ = input_mesh->mTextureCoords[0][j].x;
00313         *vertices++ = input_mesh->mTextureCoords[0][j].y;
00314       }
00315     }
00316 
00317     // calculate index count
00318     submesh->indexData->indexCount = 0;
00319     for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
00320     {
00321       aiFace& face = input_mesh->mFaces[j];
00322       submesh->indexData->indexCount += face.mNumIndices;
00323     }
00324 
00325     // If we have less than 65536 (2^16) vertices, we can use a 16-bit index buffer.
00326     if( vertex_data->vertexCount < (1<<16) )
00327     {
00328       // allocate index buffer
00329       submesh->indexData->indexBuffer = Ogre::HardwareBufferManager::getSingleton().createIndexBuffer(
00330         Ogre::HardwareIndexBuffer::IT_16BIT,
00331         submesh->indexData->indexCount,
00332         Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY,
00333         false);
00334 
00335       Ogre::HardwareIndexBufferSharedPtr ibuf = submesh->indexData->indexBuffer;
00336       uint16_t* indices = static_cast<uint16_t*>(ibuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
00337 
00338       // add the indices
00339       for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
00340       {
00341         aiFace& face = input_mesh->mFaces[j];
00342         for (uint32_t k = 0; k < face.mNumIndices; ++k)
00343         {
00344           *indices++ = face.mIndices[k];
00345         }
00346       }
00347 
00348       ibuf->unlock();
00349     }
00350     else 
00351     {
00352       // Else we have more than 65536 (2^16) vertices, so we must
00353       // use a 32-bit index buffer (or subdivide the mesh, which
00354       // I'm too impatient to do right now)
00355 
00356       // allocate index buffer
00357       submesh->indexData->indexBuffer = Ogre::HardwareBufferManager::getSingleton().createIndexBuffer(
00358         Ogre::HardwareIndexBuffer::IT_32BIT,
00359         submesh->indexData->indexCount,
00360         Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY,
00361         false);
00362 
00363       Ogre::HardwareIndexBufferSharedPtr ibuf = submesh->indexData->indexBuffer;
00364       uint32_t* indices = static_cast<uint32_t*>(ibuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
00365 
00366       // add the indices
00367       for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
00368       {
00369         aiFace& face = input_mesh->mFaces[j];
00370         for (uint32_t k = 0; k < face.mNumIndices; ++k)
00371         {
00372           *indices++ = face.mIndices[k];
00373         }
00374       }
00375 
00376       ibuf->unlock();
00377     }
00378     vbuf->unlock();
00379 
00380     submesh->setMaterialName(material_table[input_mesh->mMaterialIndex]->getName());
00381   }
00382 
00383   for (uint32_t i=0; i < node->mNumChildren; ++i)
00384   {
00385     buildMesh(scene, node->mChildren[i], mesh, aabb, radius, scale, material_table);
00386   }
00387 }
00388 
00389 void loadTexture(const std::string& resource_path)
00390 {
00391   if (!Ogre::TextureManager::getSingleton().resourceExists(resource_path))
00392   {
00393     resource_retriever::Retriever retriever;
00394     resource_retriever::MemoryResource res;
00395     try
00396     {
00397       res = retriever.get(resource_path);
00398     }
00399     catch (resource_retriever::Exception& e)
00400     {
00401       ROS_ERROR("%s", e.what());
00402     }
00403 
00404     if (res.size != 0)
00405     {
00406       Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
00407       Ogre::Image image;
00408       std::string extension = fs::extension(fs::path(resource_path));
00409 
00410       if (extension[0] == '.')
00411       {
00412         extension = extension.substr(1, extension.size() - 1);
00413       }
00414 
00415       try
00416       {
00417         image.load(stream, extension);
00418         Ogre::TextureManager::getSingleton().loadImage(resource_path, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
00419       }
00420       catch (Ogre::Exception& e)
00421       {
00422         ROS_ERROR("Could not load texture [%s]: %s", resource_path.c_str(), e.what());
00423       }
00424     }
00425   }
00426 }
00427 
00428 // Mostly cribbed from gazebo
00435 void loadMaterials(const std::string& resource_path,
00436                    const aiScene* scene,
00437                    std::vector<Ogre::MaterialPtr>& material_table_out )
00438 {
00439   for (uint32_t i = 0; i < scene->mNumMaterials; i++)
00440   {
00441     std::stringstream ss;
00442     ss << resource_path << "Material" << i;
00443     Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(ss.str(), ROS_PACKAGE_NAME, true);
00444     material_table_out.push_back(mat);
00445 
00446     Ogre::Technique* tech = mat->getTechnique(0);
00447     Ogre::Pass* pass = tech->getPass(0);
00448 
00449     aiMaterial *amat = scene->mMaterials[i];
00450 
00451     Ogre::ColourValue diffuse(1.0, 1.0, 1.0, 1.0);
00452     Ogre::ColourValue specular(1.0, 1.0, 1.0, 1.0);
00453     Ogre::ColourValue ambient(0.5, 0.5, 0.5, 1.0);
00454 
00455     for (uint32_t j=0; j < amat->mNumProperties; j++)
00456     {
00457       aiMaterialProperty *prop = amat->mProperties[j];
00458       std::string propKey = prop->mKey.data;
00459 
00460       if (propKey == "$tex.file")
00461       {
00462         aiString texName;
00463         aiTextureMapping mapping;
00464         uint32_t uvIndex;
00465         amat->GetTexture(aiTextureType_DIFFUSE,0, &texName, &mapping, &uvIndex);
00466 
00467         // Assume textures are in paths relative to the mesh
00468         std::string texture_path = fs::path(resource_path).parent_path().string() + "/" + texName.data;
00469         loadTexture(texture_path);
00470         Ogre::TextureUnitState* tu = pass->createTextureUnitState();
00471         tu->setTextureName(texture_path);
00472       }
00473       else if (propKey == "$clr.diffuse")
00474       {
00475         aiColor3D clr;
00476         amat->Get(AI_MATKEY_COLOR_DIFFUSE, clr);
00477         diffuse = Ogre::ColourValue(clr.r, clr.g, clr.b);
00478       }
00479       else if (propKey == "$clr.ambient")
00480       {
00481         aiColor3D clr;
00482         amat->Get(AI_MATKEY_COLOR_AMBIENT, clr);
00483 
00484         // Most of our DAE files don't have ambient color defined
00485         if (clr.r > 0 && clr.g > 0 && clr.b > 0)
00486         {
00487           ambient = Ogre::ColourValue(clr.r, clr.g, clr.b);
00488         }
00489       }
00490       else if (propKey == "$clr.specular")
00491       {
00492         aiColor3D clr;
00493         amat->Get(AI_MATKEY_COLOR_SPECULAR, clr);
00494         specular = Ogre::ColourValue(clr.r, clr.g, clr.b);
00495       }
00496       else if (propKey == "$clr.emissive")
00497       {
00498         aiColor3D clr;
00499         amat->Get(AI_MATKEY_COLOR_EMISSIVE, clr);
00500         mat->setSelfIllumination(clr.r, clr.g, clr.b);
00501       }
00502       else if (propKey == "$clr.opacity")
00503       {
00504         float o;
00505         amat->Get(AI_MATKEY_OPACITY, o);
00506         diffuse.a = o;
00507       }
00508       else if (propKey == "$mat.shininess")
00509       {
00510         float s;
00511         amat->Get(AI_MATKEY_SHININESS, s);
00512         mat->setShininess(s);
00513       }
00514       else if (propKey == "$mat.shadingm")
00515       {
00516         int model;
00517         amat->Get(AI_MATKEY_SHADING_MODEL, model);
00518         switch(model)
00519         {
00520           case aiShadingMode_Flat:
00521             mat->setShadingMode(Ogre::SO_FLAT);
00522             break;
00523           case aiShadingMode_Phong:
00524             mat->setShadingMode(Ogre::SO_PHONG);
00525             break;
00526           case aiShadingMode_Gouraud:
00527           default:
00528             mat->setShadingMode(Ogre::SO_GOURAUD);
00529             break;
00530         }
00531       }
00532     }
00533 
00534     int mode = aiBlendMode_Default;
00535     amat->Get(AI_MATKEY_BLEND_FUNC, mode);
00536     switch(mode)
00537     {
00538       case aiBlendMode_Additive:
00539         mat->setSceneBlending(Ogre::SBT_ADD);
00540         break;
00541       case aiBlendMode_Default:
00542       default:
00543         {
00544           if (diffuse.a < 0.99)
00545           {
00546             pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
00547           }
00548           else
00549           {
00550             pass->setSceneBlending(Ogre::SBT_REPLACE);
00551           }
00552         }
00553         break;
00554     }
00555 
00556     mat->setAmbient(ambient);
00557     mat->setDiffuse(diffuse);
00558     specular.a = diffuse.a;
00559     mat->setSpecular(specular);
00560   }
00561 }
00562 
00563 
00564 /*@brief - Get the scaling from units used in this mesh file to meters.
00565   
00566   This function applies only to Collada files. It is necessary because
00567   ASSIMP does not currently expose an api to retrieve the scaling factor.
00568   
00569   @Param[in] resource_path   -   The url of a resource containing a mesh.
00570   
00571   @Returns The scaling factor that converts the mesh to meters. Returns 1.0
00572   for meshes which do not explicitly encode such a scaling. 
00573   
00574 */
00575 
00576 float getMeshUnitRescale(const std::string& resource_path)
00577 {
00578   static std::map<std::string, float> rescale_cache;
00579 
00580    
00581 
00582   // Try to read unit to meter conversion ratio from mesh. Only valid in Collada XML formats. 
00583   TiXmlDocument xmlDoc;
00584   float unit_scale(1.0);
00585   resource_retriever::Retriever retriever;
00586   resource_retriever::MemoryResource res;
00587   try
00588   {
00589     res = retriever.get(resource_path);
00590   }
00591   catch (resource_retriever::Exception& e)
00592   {
00593     ROS_ERROR("%s", e.what());
00594     return unit_scale;
00595   }
00596   
00597   if (res.size == 0)
00598   {
00599     return unit_scale;
00600   }
00601 
00602 
00603   // Use the resource retriever to get the data.
00604   const char * data = reinterpret_cast<const char * > (res.data.get());
00605   xmlDoc.Parse(data);
00606 
00607   // Find the appropriate element if it exists
00608   if(!xmlDoc.Error())
00609   {
00610     TiXmlElement * colladaXml = xmlDoc.FirstChildElement("COLLADA");
00611     if(colladaXml)
00612     {
00613       TiXmlElement *assetXml = colladaXml->FirstChildElement("asset");
00614       if(assetXml)
00615       {
00616         TiXmlElement *unitXml = assetXml->FirstChildElement("unit");
00617         if (unitXml && unitXml->Attribute("meter"))
00618         {
00619           // Failing to convert leaves unit_scale as the default.
00620           if(unitXml->QueryFloatAttribute("meter", &unit_scale) != 0)
00621             ROS_WARN_STREAM("getMeshUnitRescale::Failed to convert unit element meter attribute to determine scaling. unit element: "
00622                             << *unitXml);
00623         }
00624       }
00625     }
00626   }
00627   return unit_scale;
00628 }
00629 
00630 
00631 
00632 Ogre::MeshPtr meshFromAssimpScene(const std::string& name, const aiScene* scene)
00633 {
00634   if (!scene->HasMeshes())
00635   {
00636     ROS_ERROR("No meshes found in file [%s]", name.c_str());
00637     return Ogre::MeshPtr();
00638   }
00639 
00640   std::vector<Ogre::MaterialPtr> material_table;
00641   loadMaterials(name, scene, material_table);
00642 
00643   Ogre::MeshPtr mesh = Ogre::MeshManager::getSingleton().createManual(name, ROS_PACKAGE_NAME);
00644 
00645   Ogre::AxisAlignedBox aabb(Ogre::AxisAlignedBox::EXTENT_NULL);
00646   float radius = 0.0f;
00647   float scale = getMeshUnitRescale(name);
00648   buildMesh(scene, scene->mRootNode, mesh, aabb, radius, scale, material_table);
00649 
00650   mesh->_setBounds(aabb);
00651   mesh->_setBoundingSphereRadius(radius);
00652   mesh->buildEdgeList();
00653 
00654   mesh->load();
00655 
00656   return mesh;
00657 }
00658 
00659 Ogre::MeshPtr loadMeshFromResource(const std::string& resource_path)
00660 {
00661   if (Ogre::MeshManager::getSingleton().resourceExists(resource_path))
00662   {
00663     return Ogre::MeshManager::getSingleton().getByName(resource_path);
00664   }
00665   else
00666   {
00667     fs::path model_path(resource_path);
00668 #if BOOST_FILESYSTEM_VERSION == 3
00669     std::string ext = model_path.extension().string();
00670 #else
00671     std::string ext = model_path.extension();
00672 #endif
00673     if (ext == ".mesh" || ext == ".MESH")
00674     {
00675       resource_retriever::Retriever retriever;
00676       resource_retriever::MemoryResource res;
00677       try
00678       {
00679         res = retriever.get(resource_path);
00680       }
00681       catch (resource_retriever::Exception& e)
00682       {
00683         ROS_ERROR("%s", e.what());
00684         return Ogre::MeshPtr();
00685       }
00686 
00687       if (res.size == 0)
00688       {
00689         return Ogre::MeshPtr();
00690       }
00691 
00692       Ogre::MeshSerializer ser;
00693       Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
00694       Ogre::MeshPtr mesh = Ogre::MeshManager::getSingleton().createManual(resource_path, "rviz");
00695       ser.importMesh(stream, mesh.get());
00696 
00697       return mesh;
00698     }
00699     else if (ext == ".stl" || ext == ".STL" || ext == ".stlb" || ext == ".STLB")
00700     {
00701       resource_retriever::Retriever retriever;
00702       resource_retriever::MemoryResource res;
00703       try
00704       {
00705         res = retriever.get(resource_path);
00706       }
00707       catch (resource_retriever::Exception& e)
00708       {
00709         ROS_ERROR("%s", e.what());
00710         return Ogre::MeshPtr();
00711       }
00712 
00713       if (res.size == 0)
00714       {
00715         return Ogre::MeshPtr();
00716       }
00717 
00718       ogre_tools::STLLoader loader;
00719       if (!loader.load(res.data.get()))
00720       {
00721         ROS_ERROR("Failed to load file [%s]", resource_path.c_str());
00722         return Ogre::MeshPtr();
00723       }
00724 
00725       return loader.toMesh(resource_path);
00726     }
00727     else
00728     {
00729       Assimp::Importer importer;
00730       importer.SetIOHandler(new ResourceIOSystem());
00731       const aiScene* scene = importer.ReadFile(resource_path, aiProcess_SortByPType|aiProcess_GenNormals|aiProcess_Triangulate|aiProcess_GenUVCoords|aiProcess_FlipUVs);
00732       if (!scene)
00733       {
00734         ROS_ERROR("Could not load resource [%s]: %s", resource_path.c_str(), importer.GetErrorString());
00735         return Ogre::MeshPtr();
00736       }
00737 
00738       return meshFromAssimpScene(resource_path, scene);
00739     }
00740   }
00741 
00742   return Ogre::MeshPtr();
00743 }
00744   
00745 }


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:35