From 4d03177186cc7190f853d5132d75c1596a420d38 Mon Sep 17 00:00:00 2001 From: Chris Lalancette Date: Wed, 1 Feb 2017 16:53:04 -0500 Subject: [PATCH] Do a few cleanup tasks in collada_urdf (#183) * Style cleanup within collada_urdf. No functional change, just style. Signed-off-by: Chris Lalancette * Make sure to quit out of urdf_to_collada when invalid file is found. Otherwise, we'll just end up segfaulting later on. Signed-off-by: Chris Lalancette * Re-enable one of the collada-urdf tests. In point of fact, we delete the rest of the tests because they don't make much sense anymore. Just enable this one for now; we'll enable further ones in the future. Signed-off-by: Chris Lalancette * Add in another test for collada_urdf. Signed-off-by: Chris Lalancette --- collada_urdf/CMakeLists.txt | 10 +- collada_urdf/src/collada_urdf.cpp | 234 ++++++++++++------------ collada_urdf/src/urdf_to_collada.cpp | 13 +- collada_urdf/test/test_collada_urdf.cpp | 45 +---- 4 files changed, 138 insertions(+), 164 deletions(-) diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt index 113dee0..d230de8 100644 --- a/collada_urdf/CMakeLists.txt +++ b/collada_urdf/CMakeLists.txt @@ -60,13 +60,13 @@ include_directories(${TinyXML_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) link_directories(${catkin_LIBRARY_DIRS}) add_library(${PROJECT_NAME} src/collada_urdf.cpp) -target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} +target_link_libraries(${PROJECT_NAME} ${TinyXML_LIBRARIES} ${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}") +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 ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} +target_link_libraries(urdf_to_collada ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES} ${PROJECT_NAME}) add_executable(collada_to_urdf src/collada_to_urdf.cpp) @@ -75,8 +75,8 @@ set_target_properties(collada_to_urdf PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FL set_target_properties(collada_to_urdf PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}") if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(test_collada_writer test/test_collada_urdf.cpp) - target_link_libraries(test_collada_writer ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} + catkin_add_gtest(test_collada_urdf test/test_collada_urdf.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test) + target_link_libraries(test_collada_urdf ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} ${Boost_LIBRARIES}) endif() diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp index 30a7a85..bef2c0c 100644 --- a/collada_urdf/src/collada_urdf.cpp +++ b/collada_urdf/src/collada_urdf.cpp @@ -949,17 +949,17 @@ protected: // Declare all the joints _mapjointindices.clear(); - int index=0; + int index = 0; FOREACHC(itj, _robot.joints_) { _mapjointindices[itj->second] = index++; } _maplinkindices.clear(); - index=0; + index = 0; FOREACHC(itj, _robot.links_) { _maplinkindices[itj->second] = index++; } _mapmaterialindices.clear(); - index=0; + index = 0; FOREACHC(itj, _robot.materials_) { _mapmaterialindices[itj->second] = index++; } @@ -980,8 +980,8 @@ protected: pdomjoint->setName(pjoint->name.c_str()); domAxis_constraintRef axis; if( !!pjoint->limits ) { - lmin=pjoint->limits->lower; - lmax=pjoint->limits->upper; + lmin = pjoint->limits->lower; + lmax = pjoint->limits->upper; } else { lmin = lmax = 0; @@ -993,8 +993,8 @@ protected: case urdf::Joint::CONTINUOUS: axis = daeSafeCast(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE)); fmult = 180.0f/M_PI; - lmin*=fmult; - lmax*=fmult; + lmin *= fmult; + lmax *= fmult; break; case urdf::Joint::PRISMATIC: axis = daeSafeCast(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC)); @@ -1165,48 +1165,49 @@ protected: if( !!geometry ) { bool write_visual = false; if ( !!plink->visual ) { - if (plink->visual_array.size() > 1) { - int igeom = 0; - for (std::vector::const_iterator it = plink->visual_array.begin(); - it != plink->visual_array.end(); it++) { - // geom - string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); - igeom++; - domGeometryRef pdomgeom; - if ( it != plink->visual_array.begin() ) { - urdf::Pose org_trans = _poseMult(geometry_origin_inv, (*it)->origin); - pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans); - } else { - pdomgeom = _WriteGeometry((*it)->geometry, geomid); - } - domInstance_geometryRef pinstgeom = daeSafeCast(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); - pinstgeom->setUrl((string("#") + geomid).c_str()); - // material - _WriteMaterial(pdomgeom->getID(), (*it)->material); - domBind_materialRef pmat = daeSafeCast(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); - domBind_material::domTechnique_commonRef pmattec = daeSafeCast(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - domInstance_materialRef pinstmat = daeSafeCast(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); - pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat"))); - pinstmat->setSymbol("mat0"); - write_visual = true; - } - } - } + if (plink->visual_array.size() > 1) { + int igeom = 0; + for (std::vector::const_iterator it = plink->visual_array.begin(); + it != plink->visual_array.end(); it++) { + // geom + string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); + igeom++; + domGeometryRef pdomgeom; + if ( it != plink->visual_array.begin() ) { + urdf::Pose org_trans = _poseMult(geometry_origin_inv, (*it)->origin); + pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans); + } + else { + pdomgeom = _WriteGeometry((*it)->geometry, geomid); + } + domInstance_geometryRef pinstgeom = daeSafeCast(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); + pinstgeom->setUrl((string("#") + geomid).c_str()); + // material + _WriteMaterial(pdomgeom->getID(), (*it)->material); + domBind_materialRef pmat = daeSafeCast(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); + domBind_material::domTechnique_commonRef pmattec = daeSafeCast(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + domInstance_materialRef pinstmat = daeSafeCast(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); + pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat"))); + pinstmat->setSymbol("mat0"); + write_visual = true; + } + } + } if (!write_visual) { - // just 1 visual - int igeom = 0; - string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); - domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid); - domInstance_geometryRef pinstgeom = daeSafeCast(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); - pinstgeom->setUrl((string("#")+geomid).c_str()); + // just 1 visual + int igeom = 0; + string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); + domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid); + domInstance_geometryRef pinstgeom = daeSafeCast(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); + pinstgeom->setUrl((string("#")+geomid).c_str()); - // material - _WriteMaterial(pdomgeom->getID(), material); - domBind_materialRef pmat = daeSafeCast(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); - domBind_material::domTechnique_commonRef pmattec = daeSafeCast(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - domInstance_materialRef pinstmat = daeSafeCast(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); - pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat"))); - pinstmat->setSymbol("mat0"); + // material + _WriteMaterial(pdomgeom->getID(), material); + domBind_materialRef pmat = daeSafeCast(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); + domBind_material::domTechnique_commonRef pmattec = daeSafeCast(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + domInstance_materialRef pinstmat = daeSafeCast(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); + pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat"))); + pinstmat->setSymbol("mat0"); } } @@ -1282,28 +1283,28 @@ protected: switch (geometry->type) { case urdf::Geometry::MESH: { urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get(); - cgeometry->setName(urdf_mesh->filename.c_str()); + cgeometry->setName(urdf_mesh->filename.c_str()); _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale, org_trans); break; } case urdf::Geometry::SPHERE: { - shapes::Sphere sphere(static_cast(geometry.get())->radius); - boost::scoped_ptr mesh(shapes::createMeshFromShape(sphere)); + shapes::Sphere sphere(static_cast(geometry.get())->radius); + boost::scoped_ptr mesh(shapes::createMeshFromShape(sphere)); _loadVertices(mesh.get(), cgeometry); break; } case urdf::Geometry::BOX: { - shapes::Box box(static_cast(geometry.get())->dim.x, - static_cast(geometry.get())->dim.y, - static_cast(geometry.get())->dim.z); - boost::scoped_ptr mesh(shapes::createMeshFromShape(box)); + shapes::Box box(static_cast(geometry.get())->dim.x, + static_cast(geometry.get())->dim.y, + static_cast(geometry.get())->dim.z); + boost::scoped_ptr mesh(shapes::createMeshFromShape(box)); _loadVertices(mesh.get(), cgeometry); break; } case urdf::Geometry::CYLINDER: { - shapes::Cylinder cyl(static_cast(geometry.get())->radius, - static_cast(geometry.get())->length); - boost::scoped_ptr mesh(shapes::createMeshFromShape(cyl)); + 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; } @@ -1434,56 +1435,56 @@ protected: } 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)); - domAccessorRef pacc; - domFloat_arrayRef parray; - { - pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str()); + // convert the mesh into an STL binary (in memory) + std::vector buffer; + shapes::writeSTLBinary(mesh, buffer); - parray = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY)); - parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str()); - parray->setDigits(6); // 6 decimal places + // Create an instance of the Importer class + Assimp::Importer importer; - domSource::domTechnique_commonRef psourcetec = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); - pacc = daeSafeCast(psourcetec->add(COLLADA_ELEMENT_ACCESSOR)); - pacc->setSource(xsAnyURI(*parray, std::string("#")+string(parray->getID()))); - pacc->setStride(3); + // 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"); - domParamRef px = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); - px->setName("X"); px->setType("float"); - domParamRef py = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); - py->setName("Y"); py->setType("float"); - domParamRef pz = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); - pz->setName("Z"); pz->setType("float"); - } - domVerticesRef pverts = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_VERTICES)); - { - pverts->setId("vertices"); - domInput_localRef pvertinput = daeSafeCast(pverts->add(COLLADA_ELEMENT_INPUT)); - pvertinput->setSemantic("POSITION"); - pvertinput->setSource(domUrifragment(*pvertsource, std::string("#")+std::string(pvertsource->getID()))); - } - _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(), urdf::Vector3(1,1,1)); - pacc->setCount(parray->getCount()); + // 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)); + domAccessorRef pacc; + domFloat_arrayRef parray; + { + pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str()); + + parray = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY)); + parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str()); + parray->setDigits(6); // 6 decimal places + + domSource::domTechnique_commonRef psourcetec = daeSafeCast(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); + pacc = daeSafeCast(psourcetec->add(COLLADA_ELEMENT_ACCESSOR)); + pacc->setSource(xsAnyURI(*parray, std::string("#")+string(parray->getID()))); + pacc->setStride(3); + + domParamRef px = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + px->setName("X"); px->setType("float"); + domParamRef py = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + py->setName("Y"); py->setType("float"); + domParamRef pz = daeSafeCast(pacc->add(COLLADA_ELEMENT_PARAM)); + pz->setName("Z"); pz->setType("float"); + } + domVerticesRef pverts = daeSafeCast(pdommesh->add(COLLADA_ELEMENT_VERTICES)); + { + pverts->setId("vertices"); + domInput_localRef pvertinput = daeSafeCast(pverts->add(COLLADA_ELEMENT_INPUT)); + pvertinput->setSemantic("POSITION"); + pvertinput->setSource(domUrifragment(*pvertsource, std::string("#")+std::string(pvertsource->getID()))); + } + _buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(), urdf::Vector3(1,1,1)); + pacc->setCount(parray->getCount()); } void _loadMesh(std::string const& filename, domGeometryRef pdomgeom, const urdf::Vector3& scale, urdf::Pose *org_trans) @@ -1568,18 +1569,19 @@ protected: aiVector3D p = input_mesh->mVertices[j]; p *= transform; if (org_trans) { - urdf::Vector3 vv; - vv.x = p.x*scale.x; - vv.y = p.y*scale.y; - vv.z = p.z*scale.z; - urdf::Vector3 nv = _poseMult(*org_trans, vv); - parray->getValue().append(nv.x); - parray->getValue().append(nv.y); - parray->getValue().append(nv.z); - } else { - parray->getValue().append(p.x*scale.x); - parray->getValue().append(p.y*scale.y); - parray->getValue().append(p.z*scale.z); + urdf::Vector3 vv; + vv.x = p.x*scale.x; + vv.y = p.y*scale.y; + vv.z = p.z*scale.z; + urdf::Vector3 nv = _poseMult(*org_trans, vv); + parray->getValue().append(nv.x); + parray->getValue().append(nv.y); + parray->getValue().append(nv.z); + } + else { + parray->getValue().append(p.x*scale.x); + parray->getValue().append(p.y*scale.y); + parray->getValue().append(p.z*scale.z); } } } @@ -1935,7 +1937,7 @@ ColladaUrdfException::ColladaUrdfException(std::string const& what) } bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, string const& file) { - ColladaWriter writer(robot_model,0); + ColladaWriter writer(robot_model, 0); if ( ! writer.convert() ) { std::cerr << std::endl << "Error converting document" << std::endl; return -1; diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index 0ab0bf9..e2eccd1 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -1,13 +1,13 @@ /********************************************************************* * Software License Agreement (BSD License) -* +* * Copyright (c) 2010, Willow Garage, Inc. * All rights reserved. -* +* * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: -* +* * * Redstributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above @@ -17,7 +17,7 @@ * * Neither the name of the Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. -* +* * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS @@ -50,14 +50,15 @@ int main(int argc, char** argv) return -1; } - ros::init(argc, argv, "urdf_to_collada"); + ros::init(argc, argv, "urdf_to_collada"); std::string input_filename(argv[1]); std::string output_filename(argv[2]); urdf::Model robot_model; if( !robot_model.initFile(input_filename) ) { - ROS_ERROR("failed to open urdf file %s",input_filename.c_str()); + ROS_ERROR("failed to open urdf file %s", input_filename.c_str()); + return -2; } collada_urdf::WriteUrdfModelToColladaFile(robot_model, output_filename); diff --git a/collada_urdf/test/test_collada_urdf.cpp b/collada_urdf/test/test_collada_urdf.cpp index 239ec0e..1542834 100644 --- a/collada_urdf/test/test_collada_urdf.cpp +++ b/collada_urdf/test/test_collada_urdf.cpp @@ -34,52 +34,23 @@ #include #include #include -/* -std::string readTestUrdfString() { - std::ifstream file("test/pr2.urdf"); - std::stringstream ss; - ss << file.rdbuf(); - return ss.str(); -} TEST(collada_urdf, collada_from_urdf_file_works) { - boost::shared_ptr dom; - ASSERT_TRUE(collada_urdf::colladaFromUrdfFile("test/pr2.urdf", dom)); - ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); + urdf::Model robot_model; + + ASSERT_TRUE(robot_model.initFile("pr2.urdf")); + ASSERT_TRUE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "pr2.dae")); } -TEST(collada_urdf, collada_from_urdf_string_works) -{ - std::string urdf_str = readTestUrdfString(); - - boost::shared_ptr dom; - ASSERT_TRUE(collada_urdf::colladaFromUrdfString(urdf_str, dom)); - ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); -} - -TEST(collada_urdf, collada_from_urdf_xml_works) -{ - TiXmlDocument urdf_xml; - ASSERT_TRUE(urdf_xml.Parse(readTestUrdfString().c_str()) > 0); - - boost::shared_ptr dom; - ASSERT_TRUE(collada_urdf::colladaFromUrdfXml(&urdf_xml, dom)); - ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); -} - -TEST(collada_urdf, collada_from_urdf_model_works) +TEST(collada_urdf, collada_output_dir_does_not_exist) { urdf::Model robot_model; - TiXmlDocument urdf_xml; - ASSERT_TRUE(urdf_xml.Parse(readTestUrdfString().c_str()) > 0); - ASSERT_TRUE(robot_model.initXml(&urdf_xml)); - boost::shared_ptr dom; - ASSERT_TRUE(collada_urdf::colladaFromUrdfModel(robot_model, dom)); - ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); + ASSERT_TRUE(robot_model.initFile("pr2.urdf")); + ASSERT_FALSE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "a/very/long/directory/path/that/should/not/exist/pr2.dae")); } -*/ + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS();