bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Do a few cleanup tasks in collada_urdf (#183) 

* Style cleanup within collada_urdf.

No functional change, just style.

Signed-off-by: Chris Lalancette <clalancette@osrfoundation.org>

* 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 <clalancette@osrfoundation.org>

* 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 <clalancette@osrfoundation.org>

* Add in another test for collada_urdf.

Signed-off-by: Chris Lalancette <clalancette@osrfoundation.org>
This commit is contained in:
Chris Lalancette 2017-02-01 16:53:04 -05:00 committed by GitHub
parent fc71f00e33
commit 4d03177186
4 changed files with 138 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
collada_urdf/CMakeLists.txt
View File

@ -60,13 +60,13 @@ include_directories(${TinyXML_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
link_directories(${catkin_LIBRARY_DIRS}) link_directories(${catkin_LIBRARY_DIRS})
add_library(${PROJECT_NAME} src/collada_urdf.cpp) 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}) ${Boost_LIBRARIES} ${ASSIMP_LIBRARIES})
set_target_properties(${PROJECT_NAME} PROPERTIES COMPILER_FLAGS "${ASSIMP_CXX_FLAGS} ${ASSIMP_CFLAGS_OTHER}") 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) 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}) ${Boost_LIBRARIES} ${PROJECT_NAME})
add_executable(collada_to_urdf src/collada_to_urdf.cpp) 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}") set_target_properties(collada_to_urdf PROPERTIES LINK_FLAGS "${ASSIMP_LINK_FLAGS}")
if(CATKIN_ENABLE_TESTING) if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(test_collada_writer test/test_collada_urdf.cpp) catkin_add_gtest(test_collada_urdf test/test_collada_urdf.cpp WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
target_link_libraries(test_collada_writer ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES} target_link_libraries(test_collada_urdf ${PROJECT_NAME} ${catkin_LIBRARIES} ${COLLADA_DOM_LIBRARIES}
${Boost_LIBRARIES}) ${Boost_LIBRARIES})
endif() endif()

234
collada_urdf/src/collada_urdf.cpp
View File

@ -949,17 +949,17 @@ protected:
// Declare all the joints // Declare all the joints
_mapjointindices.clear(); _mapjointindices.clear();
int index=0; int index = 0;
FOREACHC(itj, _robot.joints_) { FOREACHC(itj, _robot.joints_) {
_mapjointindices[itj->second] = index++; _mapjointindices[itj->second] = index++;
} }
_maplinkindices.clear(); _maplinkindices.clear();
index=0; index = 0;
FOREACHC(itj, _robot.links_) { FOREACHC(itj, _robot.links_) {
_maplinkindices[itj->second] = index++; _maplinkindices[itj->second] = index++;
} }
_mapmaterialindices.clear(); _mapmaterialindices.clear();
index=0; index = 0;
FOREACHC(itj, _robot.materials_) { FOREACHC(itj, _robot.materials_) {
_mapmaterialindices[itj->second] = index++; _mapmaterialindices[itj->second] = index++;
} }
@ -980,8 +980,8 @@ protected:
pdomjoint->setName(pjoint->name.c_str()); pdomjoint->setName(pjoint->name.c_str());
domAxis_constraintRef axis; domAxis_constraintRef axis;
if( !!pjoint->limits ) { if( !!pjoint->limits ) {
lmin=pjoint->limits->lower; lmin = pjoint->limits->lower;
lmax=pjoint->limits->upper; lmax = pjoint->limits->upper;
} }
else { else {
lmin = lmax = 0; lmin = lmax = 0;
@ -993,8 +993,8 @@ protected:
case urdf::Joint::CONTINUOUS: case urdf::Joint::CONTINUOUS:
axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE)); axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_REVOLUTE));
fmult = 180.0f/M_PI; fmult = 180.0f/M_PI;
lmin*=fmult; lmin *= fmult;
lmax*=fmult; lmax *= fmult;
break; break;
case urdf::Joint::PRISMATIC: case urdf::Joint::PRISMATIC:
axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC)); axis = daeSafeCast<domAxis_constraint>(pdomjoint->add(COLLADA_ELEMENT_PRISMATIC));
@ -1165,48 +1165,49 @@ protected:
if( !!geometry ) { if( !!geometry ) {
bool write_visual = false; bool write_visual = false;
if ( !!plink->visual ) { if ( !!plink->visual ) {
if (plink->visual_array.size() > 1) { if (plink->visual_array.size() > 1) {
int igeom = 0; int igeom = 0;
for (std::vector<urdf::VisualSharedPtr >::const_iterator it = plink->visual_array.begin(); for (std::vector<urdf::VisualSharedPtr >::const_iterator it = plink->visual_array.begin();
it != plink->visual_array.end(); it++) { it != plink->visual_array.end(); it++) {
// geom // geom
string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
igeom++; igeom++;
domGeometryRef pdomgeom; domGeometryRef pdomgeom;
if ( it != plink->visual_array.begin() ) { if ( it != plink->visual_array.begin() ) {
urdf::Pose org_trans = _poseMult(geometry_origin_inv, (*it)->origin); urdf::Pose org_trans = _poseMult(geometry_origin_inv, (*it)->origin);
pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans); pdomgeom = _WriteGeometry((*it)->geometry, geomid, &org_trans);
} else { }
pdomgeom = _WriteGeometry((*it)->geometry, geomid); else {
} pdomgeom = _WriteGeometry((*it)->geometry, geomid);
domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); }
pinstgeom->setUrl((string("#") + geomid).c_str()); domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
// material pinstgeom->setUrl((string("#") + geomid).c_str());
_WriteMaterial(pdomgeom->getID(), (*it)->material); // material
domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); _WriteMaterial(pdomgeom->getID(), (*it)->material);
domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat"))); domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
pinstmat->setSymbol("mat0"); pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
write_visual = true; pinstmat->setSymbol("mat0");
} write_visual = true;
} }
} }
}
if (!write_visual) { if (!write_visual) {
// just 1 visual // just 1 visual
int igeom = 0; int igeom = 0;
string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom)); string geomid = _ComputeId(str(boost::format("g%s_%s_geom%d")%strModelUri%linksid%igeom));
domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid); domGeometryRef pdomgeom = _WriteGeometry(geometry, geomid);
domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); domInstance_geometryRef pinstgeom = daeSafeCast<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
pinstgeom->setUrl((string("#")+geomid).c_str()); pinstgeom->setUrl((string("#")+geomid).c_str());
// material // material
_WriteMaterial(pdomgeom->getID(), material); _WriteMaterial(pdomgeom->getID(), material);
domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL)); domBind_materialRef pmat = daeSafeCast<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domBind_material::domTechnique_commonRef pmattec = daeSafeCast<domBind_material::domTechnique_common>(pmat->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL)); domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(pmattec->add(COLLADA_ELEMENT_INSTANCE_MATERIAL));
pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat"))); pinstmat->setTarget(xsAnyURI(*pdomgeom, string("#")+geomid+string("_mat")));
pinstmat->setSymbol("mat0"); pinstmat->setSymbol("mat0");
} }
} }
@ -1282,28 +1283,28 @@ protected:
switch (geometry->type) { switch (geometry->type) {
case urdf::Geometry::MESH: { case urdf::Geometry::MESH: {
urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get(); 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); _loadMesh(urdf_mesh->filename, cgeometry, urdf_mesh->scale, org_trans);
break; break;
} }
case urdf::Geometry::SPHERE: { case urdf::Geometry::SPHERE: {
shapes::Sphere sphere(static_cast<urdf::Sphere*>(geometry.get())->radius); shapes::Sphere sphere(static_cast<urdf::Sphere*>(geometry.get())->radius);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(sphere)); boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(sphere));
_loadVertices(mesh.get(), cgeometry); _loadVertices(mesh.get(), cgeometry);
break; break;
} }
case urdf::Geometry::BOX: { case urdf::Geometry::BOX: {
shapes::Box box(static_cast<urdf::Box*>(geometry.get())->dim.x, shapes::Box box(static_cast<urdf::Box*>(geometry.get())->dim.x,
static_cast<urdf::Box*>(geometry.get())->dim.y, static_cast<urdf::Box*>(geometry.get())->dim.y,
static_cast<urdf::Box*>(geometry.get())->dim.z); static_cast<urdf::Box*>(geometry.get())->dim.z);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(box)); boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(box));
_loadVertices(mesh.get(), cgeometry); _loadVertices(mesh.get(), cgeometry);
break; break;
} }
case urdf::Geometry::CYLINDER: { case urdf::Geometry::CYLINDER: {
shapes::Cylinder cyl(static_cast<urdf::Cylinder*>(geometry.get())->radius, shapes::Cylinder cyl(static_cast<urdf::Cylinder*>(geometry.get())->radius,
static_cast<urdf::Cylinder*>(geometry.get())->length); static_cast<urdf::Cylinder*>(geometry.get())->length);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(cyl)); boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(cyl));
_loadVertices(mesh.get(), cgeometry); _loadVertices(mesh.get(), cgeometry);
break; break;
} }
@ -1434,56 +1435,56 @@ protected:
} }
void _loadVertices(const shapes::Mesh *mesh, domGeometryRef pdomgeom) { void _loadVertices(const shapes::Mesh *mesh, domGeometryRef pdomgeom) {
// convert the mesh into an STL binary (in memory)
std::vector<char> 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<const void*>(&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<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH)); // convert the mesh into an STL binary (in memory)
domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE)); std::vector<char> buffer;
domAccessorRef pacc; shapes::writeSTLBinary(mesh, buffer);
domFloat_arrayRef parray;
{
pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY)); // Create an instance of the Importer class
parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str()); Assimp::Importer importer;
parray->setDigits(6); // 6 decimal places
domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON)); // And have it read the given file with some postprocessing
pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR)); const aiScene* scene = importer.ReadFileFromMemory(reinterpret_cast<const void*>(&buffer[0]), buffer.size(),
pacc->setSource(xsAnyURI(*parray, std::string("#")+string(parray->getID()))); aiProcess_Triangulate |
pacc->setStride(3); aiProcess_JoinIdenticalVertices |
aiProcess_SortByPType |
aiProcess_OptimizeGraph |
aiProcess_OptimizeMeshes, "stl");
domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM)); // Note: we do this mesh -> STL -> assimp mesh because the aiScene::aiScene symbol is hidden by default
px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM)); domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
py->setName("Y"); py->setType("float"); domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM)); domAccessorRef pacc;
pz->setName("Z"); pz->setType("float"); domFloat_arrayRef parray;
} {
domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES)); pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
{
pverts->setId("vertices"); parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
domInput_localRef pvertinput = daeSafeCast<domInput_local>(pverts->add(COLLADA_ELEMENT_INPUT)); parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
pvertinput->setSemantic("POSITION"); parray->setDigits(6); // 6 decimal places
pvertinput->setSource(domUrifragment(*pvertsource, std::string("#")+std::string(pvertsource->getID())));
} domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
_buildAiMesh(scene,scene->mRootNode,pdommesh,parray, pdomgeom->getID(), urdf::Vector3(1,1,1)); pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
pacc->setCount(parray->getCount()); pacc->setSource(xsAnyURI(*parray, std::string("#")+string(parray->getID())));
pacc->setStride(3);
domParamRef px = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
px->setName("X"); px->setType("float");
domParamRef py = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
py->setName("Y"); py->setType("float");
domParamRef pz = daeSafeCast<domParam>(pacc->add(COLLADA_ELEMENT_PARAM));
pz->setName("Z"); pz->setType("float");
}
domVerticesRef pverts = daeSafeCast<domVertices>(pdommesh->add(COLLADA_ELEMENT_VERTICES));
{
pverts->setId("vertices");
domInput_localRef pvertinput = daeSafeCast<domInput_local>(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) 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]; aiVector3D p = input_mesh->mVertices[j];
p *= transform; p *= transform;
if (org_trans) { if (org_trans) {
urdf::Vector3 vv; urdf::Vector3 vv;
vv.x = p.x*scale.x; vv.x = p.x*scale.x;
vv.y = p.y*scale.y; vv.y = p.y*scale.y;
vv.z = p.z*scale.z; vv.z = p.z*scale.z;
urdf::Vector3 nv = _poseMult(*org_trans, vv); urdf::Vector3 nv = _poseMult(*org_trans, vv);
parray->getValue().append(nv.x); parray->getValue().append(nv.x);
parray->getValue().append(nv.y); parray->getValue().append(nv.y);
parray->getValue().append(nv.z); parray->getValue().append(nv.z);
} else { }
parray->getValue().append(p.x*scale.x); else {
parray->getValue().append(p.y*scale.y); parray->getValue().append(p.x*scale.x);
parray->getValue().append(p.z*scale.z); 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) { bool WriteUrdfModelToColladaFile(urdf::Model const& robot_model, string const& file) {
ColladaWriter writer(robot_model,0); ColladaWriter writer(robot_model, 0);
if ( ! writer.convert() ) { if ( ! writer.convert() ) {
std::cerr << std::endl << "Error converting document" << std::endl; std::cerr << std::endl << "Error converting document" << std::endl;
return -1; return -1;

13
collada_urdf/src/urdf_to_collada.cpp
View File

@ -1,13 +1,13 @@
/********************************************************************* /*********************************************************************
* Software License Agreement (BSD License) * Software License Agreement (BSD License)
* *
* Copyright (c) 2010, Willow Garage, Inc. * Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved. * All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
* are met: * are met:
* *
* * Redstributions of source code must retain the above copyright * * Redstributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer. * notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above * * Redistributions in binary form must reproduce the above
@ -17,7 +17,7 @@
* * Neither the name of the Willow Garage nor the names of its * * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived * contributors may be used to endorse or promote products derived
* from this software without specific prior written permission. * from this software without specific prior written permission.
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
@ -50,14 +50,15 @@ int main(int argc, char** argv)
return -1; return -1;
} }
ros::init(argc, argv, "urdf_to_collada"); ros::init(argc, argv, "urdf_to_collada");
std::string input_filename(argv[1]); std::string input_filename(argv[1]);
std::string output_filename(argv[2]); std::string output_filename(argv[2]);
urdf::Model robot_model; urdf::Model robot_model;
if( !robot_model.initFile(input_filename) ) { 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); collada_urdf::WriteUrdfModelToColladaFile(robot_model, output_filename);

45
collada_urdf/test/test_collada_urdf.cpp
View File

@ -34,52 +34,23 @@
#include <fstream> #include <fstream>
#include <sstream> #include <sstream>
#include <string> #include <string>
/*
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) TEST(collada_urdf, collada_from_urdf_file_works)
{ {
boost::shared_ptr<DAE> dom; urdf::Model robot_model;
ASSERT_TRUE(collada_urdf::colladaFromUrdfFile("test/pr2.urdf", dom));
ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae")); ASSERT_TRUE(robot_model.initFile("pr2.urdf"));
ASSERT_TRUE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "pr2.dae"));
} }
TEST(collada_urdf, collada_from_urdf_string_works) TEST(collada_urdf, collada_output_dir_does_not_exist)
{
std::string urdf_str = readTestUrdfString();
boost::shared_ptr<DAE> 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<DAE> 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)
{ {
urdf::Model robot_model; 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<DAE> dom; ASSERT_TRUE(robot_model.initFile("pr2.urdf"));
ASSERT_TRUE(collada_urdf::colladaFromUrdfModel(robot_model, dom)); ASSERT_FALSE(collada_urdf::WriteUrdfModelToColladaFile(robot_model, "a/very/long/directory/path/that/should/not/exist/pr2.dae"));
ASSERT_TRUE(collada_urdf::colladaToFile(dom, "test/pr2.dae"));
} }
*/
int main(int argc, char **argv) { int main(int argc, char **argv) {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();