First pass of load files through an interface (to allow loading from memory, zip file etc). So instead of posix fopen/fread, using CommonFileIOInterface.

A fileIO plugin can override custom file IO operations. As a small test, load files from a zipfile in memory.
Default fileIO implementation is in examples/Utils/b3BulletDefaultFileIO.h
Affects URDF, SDF, MJCF, Wavefront OBJ, STL, DAE, images.
This commit is contained in:
erwincoumans
2018-10-08 21:27:08 -07:00
parent a1543714f1
commit dba239fe8d
49 changed files with 1211 additions and 322 deletions

View File

@@ -24,7 +24,7 @@ subject to the following restrictions:
#include "LoadMeshFromCollada.h"
#include "Bullet3Common/b3FileUtils.h"
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
class ImportColladaSetup : public CommonRigidBodyBase
@@ -79,7 +79,7 @@ void ImportColladaSetup::initPhysics()
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024))
if (!b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024,0))
return;
btVector3 shift(0, 0, 0);
@@ -94,7 +94,7 @@ void ImportColladaSetup::initPhysics()
btTransform upAxisTrans;
upAxisTrans.setIdentity();
btVector3 color(0, 0, 1);
btVector4 color(0, 0, 1,1);
#ifdef COMPARE_WITH_ASSIMP
static int useAssimp = 0;
@@ -119,7 +119,8 @@ void ImportColladaSetup::initPhysics()
{
fileIndex = 0;
}
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis);
b3BulletDefaultFileIO fileIO;
LoadMeshFromCollada(relativeFileName, visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, upAxis,&fileIO);
#endif // COMPARE_WITH_ASSIMP
//at the moment our graphics engine requires instances that share the same visual shape to be added right after registering the shape

View File

@@ -22,10 +22,11 @@ subject to the following restrictions:
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
using namespace tinyxml2;
#include "Bullet3Common/b3FileUtils.h"
#include "LinearMath/btHashMap.h"
#include <assert.h>
#include "btMatrix4x4.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#define MAX_VISUAL_SHAPES 512
@@ -561,7 +562,7 @@ void getUnitMeterScalingAndUpAxisTransform(XMLDocument& doc, btTransform& tr, fl
}
}
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis)
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling, int clientUpAxis, struct CommonFileIOInterface* fileIO)
{
// GLInstanceGraphicsShape* instance = 0;
@@ -570,16 +571,32 @@ void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLIn
float extraScaling = 1; //0.01;
btHashMap<btHashString, int> name2ShapeIndex;
b3FileUtils f;
char filename[1024];
if (!f.findFile(relativeFileName, filename, 1024))
if (!fileIO->findResourcePath(relativeFileName, filename, 1024))
{
b3Warning("File not found: %s\n", filename);
return;
}
XMLDocument doc;
if (doc.LoadFile(filename) != XML_SUCCESS)
//doc.Parse((const char*)filedata, 0, TIXML_ENCODING_UTF8);
b3AlignedObjectArray<char> xmlString;
int fileHandle = fileIO->fileOpen(filename,"r");
if (fileHandle>=0)
{
int size = fileIO->getFileSize(fileHandle);
xmlString.resize(size);
int actual = fileIO->fileRead(fileHandle, &xmlString[0],size);
if (actual==size)
{
}
}
if (xmlString.size()==0)
return;
if (doc.Parse(&xmlString[0], xmlString.size()) != XML_SUCCESS)
//if (doc.LoadFile(filename) != XML_SUCCESS)
return;
//We need units to be in meter, so apply a scaling using the asset/units meter

View File

@@ -28,7 +28,8 @@ void LoadMeshFromCollada(const char* relativeFileName,
btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,
btTransform& upAxisTrans,
float& unitMeterScaling,
int clientUpAxis);
int clientUpAxis,
struct CommonFileIOInterface* fileIO);
//#define COMPARE_WITH_ASSIMP
#ifdef COMPARE_WITH_ASSIMP

View File

@@ -6,6 +6,8 @@
#include "BulletCollision/CollisionShapes/btShapeHull.h"
#include "../../CommonInterfaces/CommonRenderInterface.h"
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include "../ImportURDFDemo/UrdfFindMeshFile.h"
#include <string>
#include "../../Utils/b3ResourcePath.h"
#include <iostream>
@@ -32,6 +34,7 @@
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleMesh.h"
using namespace tinyxml2;
#define mjcf_sphere_indiced textured_detailed_sphere_indices
@@ -203,13 +206,16 @@ struct BulletMJCFImporterInternalData
int m_flags;
int m_textureId;
CommonFileIOInterface* m_fileIO;
BulletMJCFImporterInternalData()
: m_inertiaFromGeom(true),
m_activeModel(-1),
m_activeBodyUniqueId(-1),
m_flags(0),
m_textureId(-1)
m_textureId(-1),
m_fileIO(0)
{
m_pathPrefix[0] = 0;
}
@@ -887,7 +893,7 @@ struct BulletMJCFImporterInternalData
{
geom.m_type = URDF_GEOM_MESH;
geom.m_meshFileName = assetPtr->m_fileName;
bool exists = findExistingMeshFile(
bool exists = UrdfFindMeshFile(m_fileIO,
m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
&geom.m_meshFileName,
&geom.m_meshFileType);
@@ -1408,13 +1414,14 @@ struct BulletMJCFImporterInternalData
}
};
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, int flags)
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, CommonFileIOInterface* fileIO, int flags)
{
m_data = new BulletMJCFImporterInternalData();
m_data->m_guiHelper = helper;
m_data->m_customVisualShapesConverter = customConverter;
m_data->m_flags = flags;
m_data->m_textureId = -1;
m_data->m_fileIO = fileIO;
}
BulletMJCFImporter::~BulletMJCFImporter()
@@ -1433,7 +1440,7 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024) > 0);
bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0);
m_data->m_sourceFileName = relativeFileName;
std::string xml_string;
@@ -1962,7 +1969,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
case UrdfGeometry::FILE_OBJ:
{
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO))
{
if (meshData.m_textureImage1)
{
@@ -1980,7 +1987,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
case UrdfGeometry::FILE_STL:
{
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO);
break;
}
@@ -1998,7 +2005,8 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
upAxis,
m_data->m_fileIO);
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
@@ -2243,7 +2251,7 @@ void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
if (m_data->m_customVisualShapesConverter)
{
const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, inertialFrame, link, 0, colObj->getBroadphaseHandle()->getUid(), objectIndex, m_data->m_fileIO);
}
}
@@ -2379,12 +2387,12 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
{
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0);
glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0,m_data->m_fileIO);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
//create a convex hull for each shape, and store it in a btCompoundShape
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin);
@@ -2393,7 +2401,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
}
case UrdfGeometry::FILE_STL:
{
glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO);
break;
}
default:

View File

@@ -27,7 +27,7 @@ class BulletMJCFImporter : public URDFImporterInterface
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MJCFURDFTexture>& texturesOut) const;
public:
BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, int flags);
BulletMJCFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO, int flags);
virtual ~BulletMJCFImporter();
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);

View File

@@ -10,7 +10,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../ImportURDFDemo/MyMultiBodyCreator.h"
@@ -200,7 +200,8 @@ void ImportMJCFSetup::initPhysics()
}
int flags = 0;
BulletMJCFImporter importer(m_guiHelper, 0, flags);
b3BulletDefaultFileIO fileIO;
BulletMJCFImporter importer(m_guiHelper, 0, &fileIO, flags);
MyMJCFLogger logger;
bool result = importer.loadMJCF(m_fileName, &logger);
if (result)

View File

@@ -9,6 +9,7 @@
#include "stb_image/stb_image.h"
#include "../ImportObjDemo/LoadMeshFromObj.h"
#include "Bullet3Common/b3HashMap.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
struct CachedTextureResult
{
@@ -45,7 +46,7 @@ struct CachedTextureManager
};
static CachedTextureManager sTexCacheMgr;
bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData, struct CommonFileIOInterface* fileIO)
{
B3_PROFILE("loadAndRegisterMeshFromFileInternal");
meshData.m_gfxShape = 0;
@@ -55,7 +56,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
meshData.m_isCached = false;
char relativeFileName[1024];
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
if (fileIO->findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
char pathPrefix[1024];
@@ -65,7 +66,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
std::vector<tinyobj::shape_t> shapes;
{
B3_PROFILE("tinyobj::LoadObj");
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix);
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix,fileIO);
//std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix);
}
@@ -91,7 +92,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
char relativeFileName[1024];
sprintf(relativeFileName, "%s%s", prefix[i], filename);
char relativeFileName2[1024];
if (b3ResourcePath::findResourcePath(relativeFileName, relativeFileName2, 1024))
if (fileIO->findResourcePath(relativeFileName, relativeFileName2, 1024))
{
if (b3IsFileCachingEnabled())
{
@@ -110,7 +111,30 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
if (image == 0)
{
image = stbi_load(relativeFileName, &width, &height, &n, 3);
b3AlignedObjectArray<char> buffer;
buffer.reserve(1024);
int fileId = fileIO->fileOpen(relativeFileName,"rb");
if (fileId>=0)
{
int size = fileIO->getFileSize(fileId);
if (size>0)
{
buffer.resize(size);
int actual = fileIO->fileRead(fileId,&buffer[0],size);
if (actual != size)
{
b3Warning("STL filesize mismatch!\n");
buffer.resize(0);
}
}
}
if (buffer.size())
{
image = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
}
//image = stbi_load(relativeFileName, &width, &height, &n, 3);
meshData.m_textureImage1 = image;

View File

@@ -16,7 +16,7 @@ struct b3ImportMeshData
class b3ImportMeshUtility
{
public:
static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData);
static bool loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData, struct CommonFileIOInterface* fileIO);
};
#endif //B3_IMPORT_MESH_UTILITY_H

View File

@@ -7,6 +7,7 @@
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "Wavefront2GLInstanceGraphicsShape.h"
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
#include "Bullet3Common/b3FileUtils.h"
#include "stb_image/stb_image.h"
@@ -56,7 +57,8 @@ int loadAndRegisterMeshFromFile2(const std::string& fileName, CommonRenderInterf
int shapeId = -1;
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
b3BulletDefaultFileIO fileIO;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData,&fileIO))
{
int textureIndex = -1;
@@ -94,7 +96,7 @@ void ImportObjSetup::initPhysics()
btQuaternion orn = trans.getRotation();
btVector3 scaling(1, 1, 1);
btVector3 color(1, 1, 1);
btVector4 color(1, 1, 1,1);
int shapeId = loadAndRegisterMeshFromFile2(m_fileName, m_guiHelper->getRenderInterface());
if (shapeId >= 0)

View File

@@ -33,7 +33,9 @@ void b3EnableFileCaching(int enable)
std::string LoadFromCachedOrFromObj(
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath)
const char* mtl_basepath,
struct CommonFileIOInterface* fileIO
)
{
CachedObjResult* resultPtr = gCachedObjResults[filename];
if (resultPtr)
@@ -43,7 +45,7 @@ std::string LoadFromCachedOrFromObj(
return result.m_msg;
}
std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath);
std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath,fileIO);
CachedObjResult result;
result.m_msg = err;
result.m_shapes = shapes;
@@ -54,13 +56,13 @@ std::string LoadFromCachedOrFromObj(
return err;
}
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath)
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath, struct CommonFileIOInterface* fileIO)
{
B3_PROFILE("LoadMeshFromObj");
std::vector<tinyobj::shape_t> shapes;
{
B3_PROFILE("tinyobj::LoadObj2");
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath);
std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath,fileIO);
}
{

View File

@@ -8,11 +8,13 @@ struct GLInstanceGraphicsShape;
int b3IsFileCachingEnabled();
void b3EnableFileCaching(int enable);
std::string LoadFromCachedOrFromObj(
std::vector<tinyobj::shape_t>& shapes, // [output]
const char* filename,
const char* mtl_basepath);
const char* mtl_basepath,
struct CommonFileIOInterface* fileIO);
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath);
GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const char* materialPrefixPath,struct CommonFileIOInterface* fileIO);
#endif //LOAD_MESH_FROM_OBJ_H

View File

@@ -162,7 +162,7 @@ void ImportSDFSetup::initPhysics()
m_dynamicsWorld->setGravity(gravity);
BulletURDFImporter u2b(m_guiHelper, 0, 1, 0);
BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
bool loadOk = u2b.loadSDF(m_fileName);

View File

@@ -7,6 +7,7 @@
#include "LoadMeshFromSTL.h"
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
class ImportSTLSetup : public CommonRigidBodyBase
{
@@ -55,7 +56,7 @@ void ImportSTLSetup::initPhysics()
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
char relativeFileName[1024];
if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024))
if (!b3ResourcePath::findResourcePath(m_fileName, relativeFileName, 1024,0))
{
b3Warning("Cannot find file %s\n", m_fileName);
return;
@@ -65,7 +66,8 @@ void ImportSTLSetup::initPhysics()
// int index=10;
{
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName);
b3BulletDefaultFileIO fileIO;
GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName,&fileIO);
btTransform trans;
trans.setIdentity();
@@ -74,7 +76,7 @@ void ImportSTLSetup::initPhysics()
btVector3 position = trans.getOrigin();
btQuaternion orn = trans.getRotation();
btVector3 color(0, 0, 1);
btVector4 color(0, 0, 1,1);
int shapeId = m_guiHelper->getRenderInterface()->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);

View File

@@ -5,6 +5,7 @@
#include "../../OpenGLWindow/GLInstanceGraphicsShape.h"
#include <stdio.h> //fopen
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
struct MySTLTriangle
{
@@ -14,25 +15,21 @@ struct MySTLTriangle
float vertex2[3];
};
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName, struct CommonFileIOInterface* fileIO)
{
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName, "rb");
if (file)
int fileHandle = fileIO->fileOpen(relativeFileName, "rb");
if (fileHandle>=0)
{
int size = 0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
size = fileIO->getFileSize(fileHandle);
{
b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
}
else
{
if (size)
if (size>=0)
{
//b3Warning("Open STL file of %d bytes\n",size);
char* memoryBuffer = new char[size + 1];
int actualBytesRead = fread(memoryBuffer, 1, size, file);
int actualBytesRead = fileIO->fileRead(fileHandle, memoryBuffer, size);
if (actualBytesRead != size)
{
b3Warning("Error reading from file %s", relativeFileName);
@@ -97,7 +94,7 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
delete[] memoryBuffer;
}
}
fclose(file);
fileIO->fileClose(fileHandle);
}
if (shape)
{

View File

@@ -22,9 +22,14 @@ subject to the following restrictions:
#include "BulletCollision/CollisionShapes/btShapeHull.h" //to create a tesselation of a generic btConvexShape
#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
#include "../../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include "Bullet3Common/b3FileUtils.h"
#include <string>
#include "../../Utils/b3ResourcePath.h"
#include "../../Utils/b3BulletDefaultFileIO.h"
#include "URDF2Bullet.h" //for flags
#include "../ImportMeshUtility/b3ImportMeshUtility.h"
@@ -35,13 +40,16 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001;
#include <list>
#include "UrdfParser.h"
ATTRIBUTE_ALIGNED16(struct)
BulletURDFInternalData
{
BT_DECLARE_ALIGNED_ALLOCATOR();
b3BulletDefaultFileIO m_defaultFileIO;
UrdfParser m_urdfParser;
struct GUIHelperInterface* m_guiHelper;
struct CommonFileIOInterface* m_fileIO;
std::string m_sourceFile;
char m_pathPrefix[1024];
int m_bodyId;
@@ -63,7 +71,9 @@ BulletURDFInternalData
m_pathPrefix[sizeof(m_pathPrefix) - 1] = 0; // required, strncpy doesn't write zero on overflow
}
BulletURDFInternalData()
BulletURDFInternalData(CommonFileIOInterface* fileIO)
:m_urdfParser(fileIO? fileIO : &m_defaultFileIO),
m_fileIO(fileIO? fileIO : &m_defaultFileIO)
{
m_enableTinyRenderer = true;
m_pathPrefix[0] = 0;
@@ -74,6 +84,8 @@ BulletURDFInternalData
{
m_urdfParser.setGlobalScaling(scaling);
}
};
void BulletURDFImporter::printTree()
@@ -81,9 +93,12 @@ void BulletURDFImporter::printTree()
// btAssert(0);
}
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling, int flags)
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO,double globalScaling, int flags)
{
m_data = new BulletURDFInternalData;
m_data = new BulletURDFInternalData(fileIO);
m_data->setGlobalScaling(globalScaling);
m_data->m_flags = flags;
m_data->m_guiHelper = helper;
@@ -128,7 +143,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) > 0;
bool fileFound = m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024) > 0;
std::string xml_string;
@@ -143,6 +158,23 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
fu.extractPath(relativeFileName, path, sizeof(path));
m_data->setSourceFile(relativeFileName, path);
//read file
int fileId = m_data->m_fileIO->fileOpen(relativeFileName,"r");
char destBuffer[8192];
char* line = 0;
do
{
line = m_data->m_fileIO->readLine(fileId, destBuffer, 8192);
if (line)
{
xml_string += (std::string(destBuffer) + "\n");
}
}
while (line);
#if 0
std::fstream xml_file(relativeFileName, std::fstream::in);
while (xml_file.good())
{
@@ -151,6 +183,8 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
xml_string += (line + "\n");
}
xml_file.close();
#endif
}
BulletErrorLogger loggie;
@@ -178,7 +212,7 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
b3FileUtils fu;
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024)) > 0;
bool fileFound = (m_data->m_fileIO->findResourcePath(fileName, relativeFileName, 1024)) > 0;
std::string xml_string;
@@ -507,107 +541,7 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
return compound;
}
bool findExistingMeshFile(
const std::string& urdf_path, std::string fn,
const std::string& error_message_prefix,
std::string* out_found_filename, int* out_type)
{
if (fn.size() <= 4)
{
b3Warning("%s: invalid mesh filename '%s'\n", error_message_prefix.c_str(), fn.c_str());
return false;
}
std::string ext;
std::string ext_ = fn.substr(fn.size() - 4);
for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i)
{
ext += char(tolower(*i));
}
if (ext == ".dae")
{
*out_type = UrdfGeometry::FILE_COLLADA;
}
else if (ext == ".stl")
{
*out_type = UrdfGeometry::FILE_STL;
}
else if (ext == ".obj")
{
*out_type = UrdfGeometry::FILE_OBJ;
}
else if (ext == ".cdf")
{
*out_type = UrdfGeometry::FILE_CDF;
}
else
{
b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
return false;
}
std::string drop_it_pack = "package://";
std::string drop_it_model = "model://";
if (fn.substr(0, drop_it_pack.length()) == drop_it_pack)
fn = fn.substr(drop_it_pack.length());
else if (fn.substr(0, drop_it_model.length()) == drop_it_model)
fn = fn.substr(drop_it_model.length());
std::list<std::string> shorter;
shorter.push_back("../..");
shorter.push_back("..");
shorter.push_back(".");
int cnt = urdf_path.size();
for (int i = 0; i < cnt; ++i)
{
if (urdf_path[i] == '/' || urdf_path[i] == '\\')
{
shorter.push_back(urdf_path.substr(0, i));
}
}
shorter.reverse();
std::string existing_file;
{
std::string attempt = fn;
FILE* f = fopen(attempt.c_str(), "rb");
if (f)
{
existing_file = attempt;
fclose(f);
}
}
if (existing_file.empty())
{
for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x)
{
std::string attempt = *x + "/" + fn;
FILE* f = fopen(attempt.c_str(), "rb");
if (!f)
{
//b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str());
continue;
}
fclose(f);
existing_file = attempt;
//b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str());
break;
}
}
if (existing_file.empty())
{
b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str());
return false;
}
else
{
*out_found_filename = existing_file;
return true;
}
}
int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const
{
@@ -708,7 +642,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
@@ -758,16 +692,16 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
if (m_data->m_fileIO->findResourcePath(collision->m_geometry.m_meshFileName.c_str(), relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix);
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), pathPrefix,m_data->m_fileIO);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str());
std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO);
//create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags);
@@ -777,7 +711,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
break;
case UrdfGeometry::FILE_STL:
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str(), m_data->m_fileIO);
break;
case UrdfGeometry::FILE_COLLADA:
@@ -787,7 +721,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
btTransform upAxisTrans;
upAxisTrans.setIdentity();
float unitMeterScaling = 1;
LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2);
LoadMeshFromCollada(collision->m_geometry.m_meshFileName.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2, m_data->m_fileIO);
glmesh = new GLInstanceGraphicsShape;
glmesh->m_indices = new b3AlignedObjectArray<int>();
@@ -982,7 +916,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
case UrdfGeometry::FILE_OBJ:
{
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData, m_data->m_fileIO))
{
if (meshData.m_textureImage1)
{
@@ -1000,7 +934,7 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
case UrdfGeometry::FILE_STL:
{
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str(),m_data->m_fileIO);
break;
}
@@ -1018,7 +952,8 @@ void BulletURDFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
upAxis,
m_data->m_fileIO);
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
@@ -1326,7 +1261,7 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
UrdfLink* const* linkPtr = model.m_links.getAtIndex(urdfIndex);
if (linkPtr)
{
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO);
}
}
}
@@ -1403,18 +1338,18 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn
{
const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col, pathPrefix);
m_data->m_allocatedCollisionShapes.push_back(childShape);
if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* compound = (btCompoundShape*)childShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
{
m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i));
}
}
if (childShape)
{
m_data->m_allocatedCollisionShapes.push_back(childShape);
if (childShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* compound = (btCompoundShape*)childShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
{
m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i));
}
}
btTransform childTrans = col.m_linkLocalFrame;
compoundShape->addChildShape(localInertiaFrame.inverse() * childTrans, childShape);

View File

@@ -19,7 +19,7 @@ class BulletURDFImporter : public URDFImporterInterface
struct BulletURDFInternalData* m_data;
public:
BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, double globalScaling = 1, int flags = 0);
BulletURDFImporter(struct GUIHelperInterface* helper, UrdfRenderingInterface* customConverter, struct CommonFileIOInterface* fileIO,double globalScaling, int flags);
virtual ~BulletURDFImporter();

View File

@@ -171,7 +171,8 @@ void ImportUrdfSetup::initPhysics()
int flags = 0;
double globalScaling = 1;
BulletURDFImporter u2b(m_guiHelper, 0, globalScaling, flags);
BulletURDFImporter u2b(m_guiHelper, 0, 0, globalScaling, flags);
bool loadOk = u2b.loadURDF(m_fileName);

View File

@@ -0,0 +1,116 @@
#ifndef URDF_FIND_MESH_FILE_H
#define URDF_FIND_MESH_FILE_H
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include "Bullet3Common/b3Logging.h"
#include <string>
#include <list>
#include "UrdfParser.h"
static bool UrdfFindMeshFile(
CommonFileIOInterface* fileIO,
const std::string& urdf_path, std::string fn,
const std::string& error_message_prefix,
std::string* out_found_filename, int* out_type)
{
if (fn.size() <= 4)
{
b3Warning("%s: invalid mesh filename '%s'\n", error_message_prefix.c_str(), fn.c_str());
return false;
}
std::string ext;
std::string ext_ = fn.substr(fn.size() - 4);
for (std::string::iterator i = ext_.begin(); i != ext_.end(); ++i)
{
ext += char(tolower(*i));
}
if (ext == ".dae")
{
*out_type = UrdfGeometry::FILE_COLLADA;
}
else if (ext == ".stl")
{
*out_type = UrdfGeometry::FILE_STL;
}
else if (ext == ".obj")
{
*out_type = UrdfGeometry::FILE_OBJ;
}
else if (ext == ".cdf")
{
*out_type = UrdfGeometry::FILE_CDF;
}
else
{
b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
return false;
}
std::string drop_it_pack = "package://";
std::string drop_it_model = "model://";
if (fn.substr(0, drop_it_pack.length()) == drop_it_pack)
fn = fn.substr(drop_it_pack.length());
else if (fn.substr(0, drop_it_model.length()) == drop_it_model)
fn = fn.substr(drop_it_model.length());
std::list<std::string> shorter;
shorter.push_back("../..");
shorter.push_back("..");
shorter.push_back(".");
int cnt = urdf_path.size();
for (int i = 0; i < cnt; ++i)
{
if (urdf_path[i] == '/' || urdf_path[i] == '\\')
{
shorter.push_back(urdf_path.substr(0, i));
}
}
shorter.reverse();
std::string existing_file;
{
std::string attempt = fn;
int f = fileIO->fileOpen(attempt.c_str(), "rb");
if (f>=0)
{
existing_file = attempt;
fileIO->fileClose(f);
}
}
if (existing_file.empty())
{
for (std::list<std::string>::iterator x = shorter.begin(); x != shorter.end(); ++x)
{
std::string attempt = *x + "/" + fn;
int f = fileIO->fileOpen(attempt.c_str(), "rb");
if (f<0)
{
//b3Printf("%s: tried '%s'", error_message_prefix.c_str(), attempt.c_str());
continue;
}
fileIO->fileClose(f);
existing_file = attempt;
//b3Printf("%s: found '%s'", error_message_prefix.c_str(), attempt.c_str());
break;
}
}
if (existing_file.empty())
{
b3Warning("%s: cannot find '%s' in any directory in urdf path\n", error_message_prefix.c_str(), fn.c_str());
return false;
}
else
{
*out_found_filename = existing_file;
return true;
}
}
#endif //URDF_FIND_MESH_FILE_H

View File

@@ -2,12 +2,15 @@
#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
#include "urdfStringSplit.h"
#include "urdfLexicalCast.h"
#include "UrdfFindMeshFile.h"
using namespace tinyxml2;
UrdfParser::UrdfParser()
UrdfParser::UrdfParser(CommonFileIOInterface* fileIO)
: m_parseSDF(false),
m_activeSdfModel(-1),
m_urdfScaling(1)
m_urdfScaling(1),
m_fileIO(fileIO)
{
m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real
}
@@ -495,7 +498,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, XMLElement* g, ErrorLogger* l
}
geom.m_meshFileName = fn;
bool success = findExistingMeshFile(
bool success = UrdfFindMeshFile(m_fileIO,
m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape),
&geom.m_meshFileName, &geom.m_meshFileType);
if (!success)

View File

@@ -106,9 +106,6 @@ struct UrdfGeometry
}
};
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 UrdfShape
{
@@ -256,6 +253,9 @@ protected:
int m_activeSdfModel;
btScalar m_urdfScaling;
struct CommonFileIOInterface* m_fileIO;
bool parseTransform(btTransform& tr, tinyxml2::XMLElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseInertia(UrdfInertia& inertia, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, tinyxml2::XMLElement* g, ErrorLogger* logger);
@@ -270,7 +270,7 @@ protected:
bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
public:
UrdfParser();
UrdfParser(struct CommonFileIOInterface* fileIO);
virtual ~UrdfParser();
void setParseSDF(bool useSDF)

View File

@@ -15,7 +15,7 @@ struct UrdfRenderingInterface
virtual ~UrdfRenderingInterface() {}
///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape.
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUid, int bodyUniqueId) = 0;
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
///remove a visual shapes, based on the shape unique id (shapeUid)
virtual void removeVisualShape(int collisionObjectUid) = 0;