From 4ed52cd6f70377327f952b242a1bd8c9db0ae4f8 Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Sun, 21 Apr 2013 23:11:46 +0200 Subject: [PATCH] no longer construct aiScene directly; instead build and parse an STL; this avoids the linking problem due to hidden symbols in aiScene --- collada_urdf/CMakeLists.txt | 12 +-- collada_urdf/package.xml | 2 + collada_urdf/src/collada_urdf.cpp | 156 +++++++----------------------- 3 files changed, 43 insertions(+), 127 deletions(-) diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt index a797a6b..9b5be44 100644 --- a/collada_urdf/CMakeLists.txt +++ b/collada_urdf/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(collada_urdf) -find_package(catkin REQUIRED COMPONENTS angles collada_parser resource_retriever urdf) +find_package(catkin REQUIRED COMPONENTS angles collada_parser resource_retriever urdf geometric_shapes) catkin_package( LIBRARIES ${PROJECT_NAME} @@ -51,17 +51,17 @@ include_directories(${catkin_INCLUDE_DIRS}) link_directories(${catkin_LIBRARY_DIRS}) add_library(${PROJECT_NAME} src/collada_urdf.cpp) -target_link_libraries(${PROJECT_NAME} ${tinyxml_library} ${ASSIMP_LIBRARIES} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} - ${Boost_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${tinyxml_library} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} + ${Boost_LIBRARIES} ${ASSIMP_LIBRARIES}) set_target_properties(${PROJECT_NAME} PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FLAGS} ${ASSIMP_CFLAGS_OTHER}") set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}") add_executable(urdf_to_collada src/urdf_to_collada.cpp) -target_link_libraries(urdf_to_collada ${PROJECT_NAME} ${ASSIMP_LIBRARIES} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} - ${Boost_LIBRARIES}) +target_link_libraries(urdf_to_collada ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} + ${Boost_LIBRARIES} ${PROJECT_NAME}) catkin_add_gtest(test_collada_writer test/test_collada_urdf.cpp) -target_link_libraries(test_collada_writer ${PROJECT_NAME} ${ASSIMP_LIBRARIES} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} +target_link_libraries(test_collada_writer ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES}) install(TARGETS ${PROJECT_NAME} urdf_to_collada diff --git a/collada_urdf/package.xml b/collada_urdf/package.xml index 34e8670..f111933 100644 --- a/collada_urdf/package.xml +++ b/collada_urdf/package.xml @@ -25,6 +25,7 @@ collada_parser roscpp urdf + geometric_shapes angles assimp @@ -33,5 +34,6 @@ resource_retriever roscpp urdf + geometric_shapes diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp index 79e50f6..31eb07a 100644 --- a/collada_urdf/src/collada_urdf.cpp +++ b/collada_urdf/src/collada_urdf.cpp @@ -59,6 +59,8 @@ #include #include #include +#include +#include #if defined(IS_ASSIMP3) #include @@ -75,9 +77,11 @@ #include #include #include - #endif +#include +#include + #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++) #define FOREACHC FOREACH @@ -1243,94 +1247,24 @@ protected: break; } case urdf::Geometry::SPHERE: { - urdf::Sphere* urdf_sphere = (urdf::Sphere *) geometry.get(); - double r = urdf_sphere->radius; - double phi, phid; - int seg = 64, ring = 64; - phid = M_PI * 2 / seg; - phi = 0; - - double theta, thetad; - thetad = M_PI / (ring + 1); - theta = 0; - - std::vector sphere_vertices; - std::vector points; - - for ( unsigned int i = 0; i < ring; ++ i ) { - double theta_ = theta + thetad * ( i + 1 ); - for (unsigned int j = 0; j < seg; ++ j ) { - points.push_back(urdf::Vector3(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_))); - } - } - points.push_back(urdf::Vector3(0, 0, r)); - points.push_back(urdf::Vector3(0, 0, -r)); - for(unsigned int i = 0; i < ring - 1; ++i) { - for(unsigned int j = 0; j < seg; ++j) { - unsigned int a, b, c, d; - a = i * seg + j; - b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1); - c = (i + 1) * seg + j; - d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1); - sphere_vertices.push_back(Triangle(points[a], points[c], points[b])); - sphere_vertices.push_back(Triangle(points[b], points[c], points[d])); - } - } - - for(unsigned int j = 0; j < seg; ++j) { - unsigned int a, b; - a = j; - b = (j == seg - 1) ? 0 : (j + 1); - sphere_vertices.push_back(Triangle(points[ring*seg], points[a], points[b])); - a = (ring - 1) * seg + j; - b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1); - sphere_vertices.push_back(Triangle(points[a], points[ring*seg+1], points[b])); - } - - _loadVertices(sphere_vertices, cgeometry); + shapes::Sphere sphere(static_cast(geometry.get())->radius); + boost::scoped_ptr mesh(shapes::createMeshFromShape(sphere)); + _loadVertices(mesh.get(), cgeometry); break; } case urdf::Geometry::BOX: { - urdf::Box* urdf_box = (urdf::Box*) geometry.get(); - double x = urdf_box->dim.x/2; - double y = urdf_box->dim.y/2; - double z = urdf_box->dim.z/2; - std::vector box_vertices; - box_vertices.push_back(Triangle(urdf::Vector3( x, y, -z), urdf::Vector3(-x, -y, -z), urdf::Vector3(-x, y, -z))); - box_vertices.push_back(Triangle(urdf::Vector3(-x, -y, -z), urdf::Vector3( x, y, -z), urdf::Vector3( x, -y, -z))); - box_vertices.push_back(Triangle(urdf::Vector3(-x, y, z), urdf::Vector3( x, -y, z), urdf::Vector3( x, y, z))); - box_vertices.push_back(Triangle(urdf::Vector3( x, -y, z), urdf::Vector3(-x, y, z), urdf::Vector3(-x, -y, z))); - - box_vertices.push_back(Triangle(urdf::Vector3( x, y, -z), urdf::Vector3(-x, y, z), urdf::Vector3( x, y, z))); - box_vertices.push_back(Triangle(urdf::Vector3(-x, y, z), urdf::Vector3( x, y, -z), urdf::Vector3(-x, y, -z))); - box_vertices.push_back(Triangle(urdf::Vector3( x, -y, -z), urdf::Vector3( x, y, z), urdf::Vector3( x, -y, z))); - box_vertices.push_back(Triangle(urdf::Vector3( x, y, z), urdf::Vector3( x, -y, -z), urdf::Vector3( x, y, -z))); - - box_vertices.push_back(Triangle(urdf::Vector3(-x, -y, -z), urdf::Vector3( x, -y, z), urdf::Vector3(-x, -y, z))); - box_vertices.push_back(Triangle(urdf::Vector3( x, -y, z), urdf::Vector3(-x, -y, -z), urdf::Vector3( x, -y, -z))); - box_vertices.push_back(Triangle(urdf::Vector3(-x, y, -z), urdf::Vector3(-x, -y, z), urdf::Vector3(-x, y, z))); - box_vertices.push_back(Triangle(urdf::Vector3(-x, -y, z), urdf::Vector3(-x, y, -z), urdf::Vector3(-x, -y, -z))); - - _loadVertices(box_vertices, cgeometry); + shapes::Box box(static_cast(geometry.get())->dim.x / 2.0, + static_cast(geometry.get())->dim.y / 2.0, + static_cast(geometry.get())->dim.z / 2.0); + boost::scoped_ptr mesh(shapes::createMeshFromShape(box)); + _loadVertices(mesh.get(), cgeometry); break; } case urdf::Geometry::CYLINDER: { - urdf::Cylinder* urdf_cylinder = (urdf::Cylinder*) geometry.get(); - double l = urdf_cylinder->length; - double r = urdf_cylinder->radius; - std::vector cylinder_vertices; - for(int i = 0; i < 32; i++ ) { - double s1 = sin(2*M_PI*i/32); - double c1 = cos(2*M_PI*i/32); - double s2 = sin(2*M_PI*(i+1)/32); - double c2 = cos(2*M_PI*(i+1)/32); - cylinder_vertices.push_back(Triangle(urdf::Vector3(0, 0,-l/2), urdf::Vector3(r*s2, r*c2,-l/2), urdf::Vector3(r*s1, r*c1,-l/2))); - cylinder_vertices.push_back(Triangle(urdf::Vector3(r*s1, r*c1,-l/2), urdf::Vector3(r*s2, r*c2,-l/2), urdf::Vector3(r*s2, r*c2, l/2))); - cylinder_vertices.push_back(Triangle(urdf::Vector3(r*s1, r*c1,-l/2), urdf::Vector3(r*s2, r*c2, l/2), urdf::Vector3(r*s1, r*c1, l/2))); - cylinder_vertices.push_back(Triangle(urdf::Vector3(0, 0, l/2), urdf::Vector3(r*s1, r*c1, l/2), urdf::Vector3(r*s2, r*c2, l/2))); - } - _loadVertices(cylinder_vertices, cgeometry); - + shapes::Cylinder cyl(static_cast(geometry.get())->radius, + static_cast(geometry.get())->length); + boost::scoped_ptr mesh(shapes::createMeshFromShape(cyl)); + _loadVertices(mesh.get(), cgeometry); break; } default: { @@ -1459,44 +1393,24 @@ protected: return pmout; } - void _loadVertices(const std::vector &vertices, domGeometryRef pdomgeom) { - aiScene* scene = new aiScene(); - scene->mRootNode = new aiNode(); - scene->mRootNode->mNumMeshes = 1; - scene->mRootNode->mMeshes = (unsigned int*)malloc(sizeof(unsigned int)); - scene->mRootNode->mMeshes[0] = 0; - scene->mNumMeshes = 1; - scene->mMeshes = (aiMesh **)malloc(sizeof(aiMesh*)); - scene->mMeshes[0] = new aiMesh(); - scene->mMeshes[0]->mNumFaces = 0; - scene->mMeshes[0]->mFaces = (aiFace *)malloc(sizeof(aiFace)*vertices.size()); - scene->mMeshes[0]->mNumVertices = 0; - scene->mMeshes[0]->mVertices = (aiVector3D *)malloc(sizeof(aiVector3D)*vertices.size()*3); - - FOREACH(it, vertices) { - scene->mMeshes[0]->mFaces[scene->mMeshes[0]->mNumFaces].mNumIndices = 3; - scene->mMeshes[0]->mFaces[scene->mMeshes[0]->mNumFaces].mIndices = (unsigned int *)malloc(sizeof(unsigned int)*3); - - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].x = it->p1.x; - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].y = it->p1.y; - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].z = it->p1.z; - scene->mMeshes[0]->mFaces[scene->mMeshes[0]->mNumFaces].mIndices[0] = scene->mMeshes[0]->mNumVertices; - scene->mMeshes[0]->mNumVertices++; - - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].x = it->p2.x; - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].y = it->p2.y; - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].z = it->p2.z; - scene->mMeshes[0]->mFaces[scene->mMeshes[0]->mNumFaces].mIndices[1] = scene->mMeshes[0]->mNumVertices; - scene->mMeshes[0]->mNumVertices++; - - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].x = it->p3.x; - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].y = it->p3.y; - scene->mMeshes[0]->mVertices[scene->mMeshes[0]->mNumVertices].z = it->p3.z; - scene->mMeshes[0]->mFaces[scene->mMeshes[0]->mNumFaces].mIndices[2] = scene->mMeshes[0]->mNumVertices; - scene->mMeshes[0]->mNumVertices++; - - scene->mMeshes[0]->mNumFaces++; - } + void _loadVertices(const shapes::Mesh *mesh, domGeometryRef pdomgeom) { + + // convert the mesh into an STL binary (in memory) + std::vector buffer; + shapes::writeSTLBinary(mesh, buffer); + + // Create an instance of the Importer class + Assimp::Importer importer; + + // And have it read the given file with some postprocessing + const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast(&buffer[0]), buffer.size(), + aiProcess_Triangulate | + aiProcess_JoinIdenticalVertices | + aiProcess_SortByPType | + aiProcess_OptimizeGraph | + aiProcess_OptimizeMeshes, "stl"); + + // Note: we do this mesh -> STL -> assimp mesh because the aiScene::aiScene symbol is hidden by default domMeshRef pdommesh = daeSafeCast(pdomgeom->add(COLLADA_ELEMENT_MESH)); domSourceRef pvertsource = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_SOURCE));