From 82495f3c84983bd2b2d4f1aa1bcd670d7c95b287 Mon Sep 17 00:00:00 2001 From: Oleg Klimov Date: Fri, 10 Mar 2017 18:17:38 +0300 Subject: [PATCH] URDF loader: reuse the same resources finder routine for TinyRendererVisualShapeConverter --- .../ImportURDFDemo/BulletUrdfImporter.cpp | 54 ++- .../Importers/ImportURDFDemo/UrdfParser.cpp | 27 +- .../Importers/ImportURDFDemo/UrdfParser.h | 14 +- .../TinyRendererVisualShapeConverter.cpp | 322 +++++++----------- 4 files changed, 170 insertions(+), 247 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 3137af5ae..844b0adef 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -457,9 +457,8 @@ static btCollisionShape* createConvexHullFromShapes(std::vector shorter; - int cnt = path_or_shorter.size(); + shorter.push_back("../.."); + shorter.push_back(".."); + shorter.push_back("."); + int cnt = urdf_path.size(); for (int i=0; im_geometry.m_meshFileName, collision->m_sourceFileLocation, &existing_file, &fileType); - if (!success) break; // error message already printed - GLInstanceGraphicsShape* glmesh = 0; - switch (fileType) { + switch (collision->m_geometry.m_meshFileType) { case FILE_OBJ: if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { - glmesh = LoadMeshFromObj(existing_file.c_str(), 0); + glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0); } else { std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, existing_file.c_str()); + std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str()); //create a convex hull for each shape, and store it in a btCompoundShape shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); @@ -613,7 +608,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co break; case FILE_STL: - glmesh = LoadMeshFromSTL(existing_file.c_str()); + glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str()); break; case FILE_COLLADA: @@ -622,7 +617,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co btAlignedObjectArray visualShapeInstances; btTransform upAxisTrans;upAxisTrans.setIdentity(); float unitMeterScaling = 1; - LoadMeshFromCollada(existing_file.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2); + LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2); glmesh = new GLInstanceGraphicsShape; glmesh->m_indices = new b3AlignedObjectArray(); @@ -691,7 +686,7 @@ upAxisMat.setIdentity(); if (!glmesh || glmesh->m_numvertices<=0) { - b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, existing_file.c_str()); + b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, collision->m_geometry.m_meshFileName.c_str()); delete glmesh; break; } @@ -802,17 +797,12 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha case URDF_GEOM_MESH: { - std::string existing_file; - int fileType; - bool success = findExistingMeshFile(urdfPathPrefix, visual->m_geometry.m_meshFileName, visual->m_sourceFileLocation, &existing_file, &fileType); - if (!success) break; // error message already printed - - switch (fileType) + switch (visual->m_geometry.m_meshFileType) { case FILE_OBJ: { b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(existing_file, meshData)) + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) { if (meshData.m_textureImage) @@ -830,7 +820,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha case FILE_STL: { - glmesh = LoadMeshFromSTL(existing_file.c_str()); + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); break; } @@ -842,7 +832,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha float unitMeterScaling = 1; int upAxis = 2; - LoadMeshFromCollada(existing_file.c_str(), + LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, @@ -910,7 +900,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha } glmesh->m_numIndices = glmesh->m_indices->size(); glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(existing_file); + //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName); break; } @@ -918,7 +908,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices<=0) { - b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, existing_file.c_str()); + b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, visual->m_geometry.m_meshFileName.c_str()); break; } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index e27794dde..4ccb9b14a 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -444,21 +444,24 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger* return false; } - geom.m_meshFileName = shape->Attribute("filename"); - geom.m_meshScale.setValue(1,1,1); + bool success = findExistingMeshFile( + m_urdf2Model.m_sourceFile, shape->Attribute("filename"), sourceFileLocation(shape), + &geom.m_meshFileName, &geom.m_meshFileType); + if (!success) return false; // warning printed + geom.m_meshScale.setValue(1,1,1); - if (shape->Attribute("scale")) + if (shape->Attribute("scale")) { if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger)) - { - logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n"); - std::string scalar_str = shape->Attribute("scale"); - double scaleFactor = urdfLexicalCast(scalar_str.c_str()); - if (scaleFactor) - { - geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor); - } - } + { + logger->reportWarning("%s: scale should be a vector3, not single scalar. Workaround activated.\n"); + std::string scalar_str = shape->Attribute("scale"); + double scaleFactor = urdfLexicalCast(scalar_str.c_str()); + if (scaleFactor) + { + geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor); + } + } } else { } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 4e7484d23..505e2658f 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -69,12 +69,22 @@ struct UrdfGeometry double m_cylinderRadius; double m_cylinderLength; - btVector3 m_planeNormal; + btVector3 m_planeNormal; + enum { + FILE_STL =1, + FILE_COLLADA =2, + FILE_OBJ =3, + }; + int m_meshFileType; std::string m_meshFileName; - btVector3 m_meshScale; + btVector3 m_meshScale; }; +bool findExistingMeshFile(const std::string& urdf_path, std::string fn, + const std::string& error_message_prefix, + std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere + struct UrdfVisual { std::string m_sourceFileLocation; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index cfe25c938..1876e9623 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -36,13 +36,6 @@ subject to the following restrictions: #include "../TinyRenderer/model.h" #include "../ThirdPartyLibs/stb_image/stb_image.h" -enum MyFileType -{ - MY_FILE_STL=1, - MY_FILE_COLLADA=2, - MY_FILE_OBJ=3, -}; - struct MyTexture2 { unsigned char* textureData; @@ -241,223 +234,150 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref convexColShape = sphereShape; convexColShape->setMargin(0.001); break; - - break; } case URDF_GEOM_MESH: { - if (visual->m_name.length()) + strncpy(visualShapeOut.m_meshAssetFileName, visual->m_geometry.m_meshFileName.c_str(), VISUAL_SHAPE_MAX_PATH_LEN); + visualShapeOut.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN-1] = 0; + + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0]; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; + visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; + + visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0]; + visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1]; + visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2]; + visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0]; + visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1]; + visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2]; + visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3]; + + switch (visual->m_geometry.m_meshFileType) { - //b3Printf("visual->name=%s\n", visual->m_name.c_str()); - } - if (1)//visual->m_geometry) - { - if (visual->m_geometry.m_meshFileName.length()) + case UrdfGeometry::FILE_OBJ: { - const char* filename = visual->m_geometry.m_meshFileName.c_str(); - //b3Printf("mesh->filename=%s\n", filename); - char fullPath[1024]; - int fileType = 0; - - char tmpPathPrefix[1024]; - std::string xml_string; - int maxPathLen = 1024; - b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen); - - char visualPathPrefix[1024]; - sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix); - - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - b3FileUtils::toLower(fullPath); - if (strstr(fullPath, ".dae")) + //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); + b3ImportMeshData meshData; + if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData)) { - fileType = MY_FILE_COLLADA; - } - if (strstr(fullPath, ".stl")) - { - fileType = MY_FILE_STL; - } - if (strstr(fullPath,".obj")) - { - fileType = MY_FILE_OBJ; - } - - sprintf(fullPath, "%s%s", urdfPathPrefix, filename); - - visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0]; - visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; - visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; - visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0]; - visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1]; - visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2]; - visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0]; - visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1]; - visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2]; - visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3]; - - int sl = strlen(fullPath); - if (sl < (VISUAL_SHAPE_MAX_PATH_LEN-1)) - { - memcpy(visualShapeOut.m_meshAssetFileName, fullPath, sl); - visualShapeOut.m_meshAssetFileName[sl] = 0; - } - - FILE* f = fopen(fullPath, "rb"); - if (f) - { - fclose(f); - - - - switch (fileType) + if (meshData.m_textureImage) { - case MY_FILE_OBJ: - { - //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix); - b3ImportMeshData meshData; - if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData)) - { - - if (meshData.m_textureImage) - { - MyTexture2 texData; - texData.m_width = meshData.m_textureWidth; - texData.m_height = meshData.m_textureHeight; - texData.textureData = meshData.m_textureImage; - texturesOut.push_back(texData); - } - glmesh = meshData.m_gfxShape; - } - - - break; - } - - case MY_FILE_STL: - { - glmesh = LoadMeshFromSTL(fullPath); - break; + MyTexture2 texData; + texData.m_width = meshData.m_textureWidth; + texData.m_height = meshData.m_textureHeight; + texData.textureData = meshData.m_textureImage; + texturesOut.push_back(texData); } - case MY_FILE_COLLADA: + glmesh = meshData.m_gfxShape; + } + break; + } + case UrdfGeometry::FILE_STL: + glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str()); + break; + case UrdfGeometry::FILE_COLLADA: + { + btAlignedObjectArray visualShapes; + btAlignedObjectArray visualShapeInstances; + btTransform upAxisTrans; upAxisTrans.setIdentity(); + float unitMeterScaling = 1; + int upAxis = 2; + + LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str(), + visualShapes, + visualShapeInstances, + upAxisTrans, + unitMeterScaling, + upAxis); + + glmesh = new GLInstanceGraphicsShape; + // int index = 0; + glmesh->m_indices = new b3AlignedObjectArray(); + glmesh->m_vertices = new b3AlignedObjectArray(); + + for (int i = 0; im_shapeIndex]; + + b3AlignedObjectArray verts; + verts.resize(gfxShape->m_vertices->size()); + + int baseIndex = glmesh->m_vertices->size(); + + for (int i = 0; im_vertices->size(); i++) { + verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; + verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; + verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; + verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; + verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; + verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; + verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; + verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; + verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - btAlignedObjectArray visualShapes; - btAlignedObjectArray visualShapeInstances; - btTransform upAxisTrans; upAxisTrans.setIdentity(); - float unitMeterScaling = 1; - int upAxis = 2; + } - LoadMeshFromCollada(fullPath, - visualShapes, - visualShapeInstances, - upAxisTrans, - unitMeterScaling, - upAxis); + int curNumIndices = glmesh->m_indices->size(); + int additionalIndices = gfxShape->m_indices->size(); + glmesh->m_indices->resize(curNumIndices + additionalIndices); + for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; + } - glmesh = new GLInstanceGraphicsShape; - // int index = 0; - glmesh->m_indices = new b3AlignedObjectArray(); - glmesh->m_vertices = new b3AlignedObjectArray(); - - for (int i = 0; im_shapeIndex]; - - b3AlignedObjectArray verts; - verts.resize(gfxShape->m_vertices->size()); - - int baseIndex = glmesh->m_vertices->size(); - - for (int i = 0; im_vertices->size(); i++) - { - verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0]; - verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1]; - verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2]; - verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0]; - verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1]; - verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0]; - verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1]; - verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2]; - verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3]; - - } - - int curNumIndices = glmesh->m_indices->size(); - int additionalIndices = gfxShape->m_indices->size(); - glmesh->m_indices->resize(curNumIndices + additionalIndices); - for (int k = 0; km_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex; - } - - //compensate upAxisTrans and unitMeterScaling here - btMatrix4x4 upAxisMat; - upAxisMat.setIdentity(); + //compensate upAxisTrans and unitMeterScaling here + btMatrix4x4 upAxisMat; + upAxisMat.setIdentity(); // upAxisMat.setPureRotation(upAxisTrans.getRotation()); - btMatrix4x4 unitMeterScalingMat; - unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); - btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; - //btMatrix4x4 worldMat = instance->m_worldTransform; - int curNumVertices = glmesh->m_vertices->size(); - int additionalVertices = verts.size(); - glmesh->m_vertices->reserve(curNumVertices + additionalVertices); + btMatrix4x4 unitMeterScalingMat; + unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling)); + btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform; + //btMatrix4x4 worldMat = instance->m_worldTransform; + int curNumVertices = glmesh->m_vertices->size(); + int additionalVertices = verts.size(); + glmesh->m_vertices->reserve(curNumVertices + additionalVertices); - for (int v = 0; vm_vertices->push_back(verts[v]); - } - } - glmesh->m_numIndices = glmesh->m_indices->size(); - glmesh->m_numvertices = glmesh->m_vertices->size(); - //glmesh = LoadMeshFromCollada(fullPath); - - break; - } - default: + for (int v = 0; vm_vertices->push_back(verts[v]); } - } - - - if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) - { - //apply the geometry scaling - for (int i=0;im_vertices->size();i++) - { - glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; - glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; - glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; - } - - } - else - { - b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath); - } - - } - else - { - b3Warning("mesh geometry not found %s\n", fullPath); } + glmesh->m_numIndices = glmesh->m_indices->size(); + glmesh->m_numvertices = glmesh->m_vertices->size(); + //glmesh = LoadMeshFromCollada(visual->m_geometry.m_meshFileName.c_str()); + break; + } + default: + // should never get here (findExistingMeshFile returns false if it doesn't recognize extension) + btAssert(0); + } + if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0)) + { + //apply the geometry scaling + for (int i=0;im_vertices->size();i++) + { + glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0]; + glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1]; + glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2]; } } - - + else + { + b3Warning("issue extracting mesh from COLLADA/STL file %s\n", visual->m_geometry.m_meshFileName.c_str()); + } break; - } + } // case mesh + default: { b3Warning("Error: unknown visual geometry type\n");