mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 13:50:04 +00:00
URDF loader: resources path, "package://" removal, error messages, zero material path in .obj loader
This commit is contained in:
Oleg Klimov
parent
7ffbd35d42
commit
5b2a9d1a01
@ -32,6 +32,7 @@ static btScalar gUrdfDefaultCollisionMargin = 0.001;
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <list>
|
||||
#include "UrdfParser.h"
|
||||
|
||||
struct MyTexture
|
||||
@ -47,14 +48,27 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
|
||||
|
||||
UrdfParser m_urdfParser;
|
||||
struct GUIHelperInterface* m_guiHelper;
|
||||
std::string m_sourceFile;
|
||||
char m_pathPrefix[1024];
|
||||
int m_bodyId;
|
||||
btHashMap<btHashInt,btVector4> m_linkColors;
|
||||
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
||||
|
||||
LinkVisualShapesConverter* m_customVisualShapesConverter;
|
||||
};
|
||||
|
||||
void setSourceFile(const std::string& relativeFileName, const std::string& prefix)
|
||||
{
|
||||
m_sourceFile = relativeFileName;
|
||||
m_urdfParser.setSourceFile(relativeFileName);
|
||||
strncpy(m_pathPrefix, prefix.c_str(), sizeof(m_pathPrefix));
|
||||
m_pathPrefix[sizeof(m_pathPrefix)-1] = 0; // required, strncpy doesn't write zero on overflow
|
||||
}
|
||||
|
||||
BulletURDFInternalData()
|
||||
{
|
||||
m_pathPrefix[0] = 0;
|
||||
}
|
||||
};
|
||||
|
||||
void BulletURDFImporter::printTree()
|
||||
{
|
||||
@ -76,7 +90,6 @@ BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVi
|
||||
m_data = new BulletURDFInternalData;
|
||||
|
||||
m_data->m_guiHelper = helper;
|
||||
m_data->m_pathPrefix[0]=0;
|
||||
m_data->m_customVisualShapesConverter = customConverter;
|
||||
|
||||
|
||||
@ -111,7 +124,6 @@ struct BulletErrorLogger : public ErrorLogger
|
||||
|
||||
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
||||
{
|
||||
|
||||
if (strlen(fileName)==0)
|
||||
return false;
|
||||
|
||||
@ -124,17 +136,16 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
|
||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0;
|
||||
|
||||
std::string xml_string;
|
||||
m_data->m_pathPrefix[0] = 0;
|
||||
|
||||
if (!fileFound){
|
||||
std::cerr << "URDF file not found" << std::endl;
|
||||
b3Warning("URDF file '%s' not found\n", fileName);
|
||||
return false;
|
||||
} else
|
||||
{
|
||||
|
||||
int maxPathLen = 1024;
|
||||
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
|
||||
|
||||
char path[1024];
|
||||
fu.extractPath(relativeFileName, path, sizeof(path));
|
||||
m_data->setSourceFile(relativeFileName, path);
|
||||
|
||||
std::fstream xml_file(relativeFileName, std::fstream::in);
|
||||
while ( xml_file.good())
|
||||
@ -176,17 +187,16 @@ bool BulletURDFImporter::loadSDF(const char* fileName, bool forceFixedBase)
|
||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024))>0;
|
||||
|
||||
std::string xml_string;
|
||||
m_data->m_pathPrefix[0] = 0;
|
||||
|
||||
if (!fileFound){
|
||||
std::cerr << "SDF file not found" << std::endl;
|
||||
b3Warning("SDF file '%s' not found\n", fileName);
|
||||
return false;
|
||||
} else
|
||||
{
|
||||
|
||||
int maxPathLen = 1024;
|
||||
fu.extractPath(relativeFileName,m_data->m_pathPrefix,maxPathLen);
|
||||
|
||||
char path[1024];
|
||||
fu.extractPath(relativeFileName, path, sizeof(path));
|
||||
m_data->setSourceFile(relativeFileName, path);
|
||||
|
||||
std::fstream xml_file(relativeFileName, std::fstream::in);
|
||||
while ( xml_file.good() )
|
||||
@ -447,6 +457,69 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
|
||||
return compound;
|
||||
}
|
||||
|
||||
static
|
||||
bool findExistingMeshFile(
|
||||
const std::string& path_or_shorter, 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 = FILE_COLLADA;
|
||||
else if (ext==".stl") *out_type = FILE_STL;
|
||||
else if (ext==".obj") *out_type = FILE_OBJ;
|
||||
else
|
||||
{
|
||||
b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
std::string drop_it = "package://";
|
||||
if (fn.substr(0, drop_it.length())==drop_it)
|
||||
fn = fn.substr(drop_it.length());
|
||||
|
||||
std::list<std::string> shorter;
|
||||
int cnt = path_or_shorter.size();
|
||||
for (int i=0; i<cnt; ++i) {
|
||||
if (path_or_shorter[i]=='/' || path_or_shorter[i]=='\\')
|
||||
shorter.push_back(path_or_shorter.substr(0, i));
|
||||
else if (i==cnt-1)
|
||||
shorter.push_back(path_or_shorter.substr(0, cnt));
|
||||
}
|
||||
shorter.reverse();
|
||||
|
||||
std::string existing_file;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
|
||||
{
|
||||
BT_PROFILE("convertURDFToCollisionShape");
|
||||
@ -507,102 +580,51 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
}
|
||||
case URDF_GEOM_SPHERE:
|
||||
{
|
||||
|
||||
btScalar radius = collision->m_geometry.m_sphereRadius;
|
||||
btSphereShape* sphereShape = new btSphereShape(radius);
|
||||
shape = sphereShape;
|
||||
shape ->setMargin(gUrdfDefaultCollisionMargin);
|
||||
break;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_MESH:
|
||||
{
|
||||
if (collision->m_name.length())
|
||||
{
|
||||
//b3Printf("collision->name=%s\n",collision->m_name.c_str());
|
||||
}
|
||||
if (1)
|
||||
{
|
||||
if (collision->m_geometry.m_meshFileName.length())
|
||||
{
|
||||
const char* filename = collision->m_geometry.m_meshFileName.c_str();
|
||||
//b3Printf("mesh->filename=%s\n",filename);
|
||||
char fullPath[1024];
|
||||
int fileType = 0;
|
||||
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
|
||||
b3FileUtils::toLower(fullPath);
|
||||
char tmpPathPrefix[1024];
|
||||
int maxPathLen = 1024;
|
||||
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
|
||||
std::string existing_file;
|
||||
int fileType;
|
||||
bool success = findExistingMeshFile(urdfPathPrefix, collision->m_geometry.m_meshFileName, collision->m_sourceFileLocation, &existing_file, &fileType);
|
||||
if (!success) break; // error message already printed
|
||||
|
||||
char collisionPathPrefix[1024];
|
||||
sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
|
||||
|
||||
|
||||
|
||||
if (strstr(fullPath,".dae"))
|
||||
{
|
||||
fileType = FILE_COLLADA;
|
||||
}
|
||||
if (strstr(fullPath,".stl"))
|
||||
{
|
||||
fileType = FILE_STL;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = FILE_OBJ;
|
||||
}
|
||||
|
||||
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
|
||||
FILE* f = fopen(fullPath,"rb");
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
switch (fileType) {
|
||||
case FILE_OBJ:
|
||||
{
|
||||
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
|
||||
glmesh = LoadMeshFromObj(existing_file.c_str(), 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
|
||||
std::string err = tinyobj::LoadObj(shapes, existing_file.c_str());
|
||||
//create a convex hull for each shape, and store it in a btCompoundShape
|
||||
|
||||
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
|
||||
return shape;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
glmesh = LoadMeshFromSTL(existing_file.c_str());
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_COLLADA:
|
||||
{
|
||||
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans;upAxisTrans.setIdentity();
|
||||
float unitMeterScaling=1;
|
||||
int upAxis = 2;
|
||||
LoadMeshFromCollada(fullPath,
|
||||
visualShapes,
|
||||
visualShapeInstances,
|
||||
upAxisTrans,
|
||||
unitMeterScaling,
|
||||
upAxis );
|
||||
float unitMeterScaling = 1;
|
||||
LoadMeshFromCollada(existing_file.c_str(), visualShapes, visualShapeInstances, upAxisTrans, unitMeterScaling, 2);
|
||||
|
||||
glmesh = new GLInstanceGraphicsShape;
|
||||
// int index = 0;
|
||||
glmesh->m_indices = new b3AlignedObjectArray<int>();
|
||||
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
|
||||
|
||||
@ -640,7 +662,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
|
||||
//compensate upAxisTrans and unitMeterScaling here
|
||||
btMatrix4x4 upAxisMat;
|
||||
upAxisMat.setIdentity();
|
||||
upAxisMat.setIdentity();
|
||||
//upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
btMatrix4x4 unitMeterScalingMat;
|
||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
|
||||
@ -662,26 +684,21 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
}
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
//glmesh = LoadMeshFromCollada(fullPath);
|
||||
|
||||
//glmesh = LoadMeshFromCollada(success.c_str());
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Warning("Unsupported file type in Collision: %s\n",fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (glmesh && (glmesh->m_numvertices>0))
|
||||
if (!glmesh || glmesh->m_numvertices<=0)
|
||||
{
|
||||
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
|
||||
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
|
||||
//convex->setUserIndex(shapeId);
|
||||
b3Warning("%s: cannot extract mesh from '%s'\n", urdfPathPrefix, existing_file.c_str());
|
||||
delete glmesh;
|
||||
break;
|
||||
}
|
||||
|
||||
btAlignedObjectArray<btVector3> convertedVerts;
|
||||
convertedVerts.reserve(glmesh->m_numvertices);
|
||||
for (int i=0;i<glmesh->m_numvertices;i++)
|
||||
for (int i=0; i<glmesh->m_numvertices; i++)
|
||||
{
|
||||
convertedVerts.push_back(btVector3(
|
||||
glmesh->m_vertices->at(i).xyzw[0]*collision->m_geometry.m_meshScale[0],
|
||||
@ -693,23 +710,23 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
{
|
||||
BT_PROFILE("convert trimesh");
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||
for (int i=0; i<glmesh->m_numIndices/3; i++)
|
||||
{
|
||||
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
|
||||
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
|
||||
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
|
||||
meshInterface->addTriangle(
|
||||
btVector3(v0[0],v0[1],v0[2]),
|
||||
btVector3(v1[0],v1[1],v1[2]),
|
||||
btVector3(v2[0],v2[1],v2[2]));
|
||||
}
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||
trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
|
||||
shape = trimesh;
|
||||
|
||||
} else
|
||||
{
|
||||
BT_PROFILE("convert btConvexHullShape");
|
||||
|
||||
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
convexHull->optimizeConvexHull();
|
||||
//convexHull->initializePolyhedralFeatures();
|
||||
@ -717,29 +734,14 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
convexHull->setLocalScaling(collision->m_geometry.m_meshScale);
|
||||
shape = convexHull;
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
|
||||
}
|
||||
|
||||
delete glmesh;
|
||||
|
||||
} else
|
||||
{
|
||||
b3Warning("mesh geometry not found %s\n",fullPath);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
} // mesh case
|
||||
|
||||
default:
|
||||
{
|
||||
b3Warning("Error: unknown visual geometry type\n");
|
||||
}
|
||||
}
|
||||
return shape;
|
||||
}
|
||||
|
||||
@ -778,17 +780,17 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
convexColShape = cylZShape;
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_BOX:
|
||||
{
|
||||
|
||||
btVector3 extents = visual->m_geometry.m_boxSize;
|
||||
|
||||
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
|
||||
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
|
||||
convexColShape = boxShape;
|
||||
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_SPHERE:
|
||||
{
|
||||
btScalar radius = visual->m_geometry.m_sphereRadius;
|
||||
@ -796,63 +798,21 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
convexColShape = sphereShape;
|
||||
convexColShape->setMargin(gUrdfDefaultCollisionMargin);
|
||||
break;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_MESH:
|
||||
{
|
||||
if (visual->m_name.length())
|
||||
{
|
||||
//b3Printf("visual->name=%s\n", visual->m_name.c_str());
|
||||
}
|
||||
if (1)//visual->m_geometry)
|
||||
{
|
||||
if (visual->m_geometry.m_meshFileName.length())
|
||||
{
|
||||
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"))
|
||||
{
|
||||
fileType = FILE_COLLADA;
|
||||
}
|
||||
if (strstr(fullPath, ".stl"))
|
||||
{
|
||||
fileType = FILE_STL;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = FILE_OBJ;
|
||||
}
|
||||
|
||||
|
||||
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
|
||||
FILE* f = fopen(fullPath, "rb");
|
||||
if (f)
|
||||
{
|
||||
fclose(f);
|
||||
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)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
{
|
||||
// glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
|
||||
|
||||
b3ImportMeshData meshData;
|
||||
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
|
||||
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(existing_file, meshData))
|
||||
{
|
||||
|
||||
if (meshData.m_textureImage)
|
||||
@ -865,25 +825,24 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
}
|
||||
glmesh = meshData.m_gfxShape;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
glmesh = LoadMeshFromSTL(existing_file.c_str());
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_COLLADA:
|
||||
{
|
||||
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
btTransform upAxisTrans; upAxisTrans.setIdentity();
|
||||
float unitMeterScaling = 1;
|
||||
int upAxis = 2;
|
||||
|
||||
LoadMeshFromCollada(fullPath,
|
||||
LoadMeshFromCollada(existing_file.c_str(),
|
||||
visualShapes,
|
||||
visualShapeInstances,
|
||||
upAxisTrans,
|
||||
@ -930,7 +889,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
//compensate upAxisTrans and unitMeterScaling here
|
||||
btMatrix4x4 upAxisMat;
|
||||
upAxisMat.setIdentity();
|
||||
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
|
||||
btMatrix4x4 unitMeterScalingMat;
|
||||
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
|
||||
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
|
||||
@ -951,20 +910,18 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
}
|
||||
glmesh->m_numIndices = glmesh->m_indices->size();
|
||||
glmesh->m_numvertices = glmesh->m_vertices->size();
|
||||
//glmesh = LoadMeshFromCollada(fullPath);
|
||||
//glmesh = LoadMeshFromCollada(existing_file);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
} // switch file type
|
||||
|
||||
if (!glmesh || !glmesh->m_vertices || glmesh->m_numvertices<=0)
|
||||
{
|
||||
b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
|
||||
btAssert(0);
|
||||
}
|
||||
b3Warning("%s: cannot extract anything useful from mesh '%s'\n", urdfPathPrefix, existing_file.c_str());
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
|
||||
{
|
||||
//apply the geometry scaling
|
||||
for (int i=0;i<glmesh->m_vertices->size();i++)
|
||||
{
|
||||
@ -972,31 +929,12 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
b3Warning("Error: unknown visual geometry type\n");
|
||||
}
|
||||
}
|
||||
|
||||
//if we have a convex, tesselate into localVertices/localIndices
|
||||
if ((glmesh==0) && convexColShape)
|
||||
|
||||
@ -8,9 +8,9 @@ UrdfParser::UrdfParser()
|
||||
:m_parseSDF(false),
|
||||
m_activeSdfModel(-1)
|
||||
{
|
||||
m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real
|
||||
}
|
||||
|
||||
|
||||
UrdfParser::~UrdfParser()
|
||||
{
|
||||
cleanModel(&m_urdf2Model);
|
||||
@ -845,6 +845,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
|
||||
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
|
||||
{
|
||||
UrdfVisual visual;
|
||||
visual.m_sourceFileLocation = sourceFileLocation(vis_xml);
|
||||
|
||||
if (parseVisual(model, visual, vis_xml,logger))
|
||||
{
|
||||
@ -864,6 +865,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
|
||||
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
|
||||
{
|
||||
UrdfCollision col;
|
||||
col.m_sourceFileLocation = sourceFileLocation(col_xml);
|
||||
|
||||
if (parseCollision(col, col_xml,logger))
|
||||
{
|
||||
link.m_collisionArray.push_back(col);
|
||||
@ -1657,3 +1660,9 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string UrdfParser::sourceFileLocation(TiXmlElement* e)
|
||||
{
|
||||
char buf[1024];
|
||||
snprintf(buf, sizeof(buf), "%s:%i", m_urdf2Model.m_sourceFile.c_str(), e->Row());
|
||||
return buf;
|
||||
}
|
||||
|
||||
@ -77,6 +77,7 @@ struct UrdfGeometry
|
||||
|
||||
struct UrdfVisual
|
||||
{
|
||||
std::string m_sourceFileLocation;
|
||||
btTransform m_linkLocalFrame;
|
||||
UrdfGeometry m_geometry;
|
||||
std::string m_name;
|
||||
@ -90,6 +91,7 @@ struct UrdfVisual
|
||||
|
||||
struct UrdfCollision
|
||||
{
|
||||
std::string m_sourceFileLocation;
|
||||
btTransform m_linkLocalFrame;
|
||||
UrdfGeometry m_geometry;
|
||||
std::string m_name;
|
||||
@ -159,6 +161,7 @@ struct UrdfJoint
|
||||
struct UrdfModel
|
||||
{
|
||||
std::string m_name;
|
||||
std::string m_sourceFile;
|
||||
btTransform m_rootTransformInWorld;
|
||||
btHashMap<btHashString, UrdfMaterial*> m_materials;
|
||||
btHashMap<btHashString, UrdfLink*> m_links;
|
||||
@ -263,6 +266,13 @@ public:
|
||||
}
|
||||
return m_urdf2Model;
|
||||
}
|
||||
|
||||
std::string sourceFileLocation(TiXmlElement* e);
|
||||
|
||||
void setSourceFile(const std::string& sourceFile)
|
||||
{
|
||||
m_urdf2Model.m_sourceFile = sourceFile;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -507,6 +507,16 @@ LoadObj(
|
||||
const char* filename,
|
||||
const char* mtl_basepath)
|
||||
{
|
||||
std::string tmp = filename;
|
||||
if (!mtl_basepath) {
|
||||
int last_slash = 0;
|
||||
for (int c=0; c<(int)tmp.size(); ++c)
|
||||
if (tmp[c]=='/' || tmp[c]=='\\')
|
||||
last_slash = c;
|
||||
tmp = tmp.substr(0, last_slash);
|
||||
mtl_basepath = tmp.c_str();
|
||||
//fprintf(stderr, "MTL PATH '%s' orig '%s'\n", mtl_basepath, filename);
|
||||
}
|
||||
|
||||
shapes.resize(0);
|
||||
std::vector<vertex_index> allIndices;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user