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:
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
{
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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();
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
116
examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
Normal file
116
examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h
Normal 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
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user