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

4
collada_urdf/CMakeLists.txt
View File

@ -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()

226
collada_urdf/src/collada_urdf.cpp
View File

@ -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<domAxis_constraint>(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<domAxis_constraint>(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<urdf::VisualSharedPtr >::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<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
pinstgeom->setUrl((string("#") + geomid).c_str());
// material
_WriteMaterial(pdomgeom->getID(), (*it)->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));
domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(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<urdf::VisualSharedPtr >::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<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
pinstgeom->setUrl((string("#") + geomid).c_str());
// material
_WriteMaterial(pdomgeom->getID(), (*it)->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));
domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(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<domInstance_geometry>(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<domInstance_geometry>(pnode->add(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
pinstgeom->setUrl((string("#")+geomid).c_str());
// material
_WriteMaterial(pdomgeom->getID(), 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));
domInstance_materialRef pinstmat = daeSafeCast<domInstance_material>(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<domBind_material>(pinstgeom->add(COLLADA_ELEMENT_BIND_MATERIAL));
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));
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<urdf::Sphere*>(geometry.get())->radius);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(sphere));
shapes::Sphere sphere(static_cast<urdf::Sphere*>(geometry.get())->radius);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(sphere));
_loadVertices(mesh.get(), cgeometry);
break;
}
case urdf::Geometry::BOX: {
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.z);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(box));
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.z);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(box));
_loadVertices(mesh.get(), cgeometry);
break;
}
case urdf::Geometry::CYLINDER: {
shapes::Cylinder cyl(static_cast<urdf::Cylinder*>(geometry.get())->radius,
static_cast<urdf::Cylinder*>(geometry.get())->length);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(cyl));
shapes::Cylinder cyl(static_cast<urdf::Cylinder*>(geometry.get())->radius,
static_cast<urdf::Cylinder*>(geometry.get())->length);
boost::scoped_ptr<shapes::Mesh> mesh(shapes::createMeshFromShape(cyl));
_loadVertices(mesh.get(), cgeometry);
break;
}
@ -1435,55 +1436,55 @@ protected:
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);
// 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;
// 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");
// 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
// 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));
domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
domAccessorRef pacc;
domFloat_arrayRef parray;
{
pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
domMeshRef pdommesh = daeSafeCast<domMesh>(pdomgeom->add(COLLADA_ELEMENT_MESH));
domSourceRef pvertsource = daeSafeCast<domSource>(pdommesh->add(COLLADA_ELEMENT_SOURCE));
domAccessorRef pacc;
domFloat_arrayRef parray;
{
pvertsource->setId(str(boost::format("%s_positions")%pdomgeom->getID()).c_str());
parray = daeSafeCast<domFloat_array>(pvertsource->add(COLLADA_ELEMENT_FLOAT_ARRAY));
parray->setId(str(boost::format("%s_positions-array")%pdomgeom->getID()).c_str());
parray->setDigits(6); // 6 decimal places
parray = daeSafeCast<domFloat_array>(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<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
pacc->setSource(xsAnyURI(*parray, std::string("#")+string(parray->getID())));
pacc->setStride(3);
domSource::domTechnique_commonRef psourcetec = daeSafeCast<domSource::domTechnique_common>(pvertsource->add(COLLADA_ELEMENT_TECHNIQUE_COMMON));
pacc = daeSafeCast<domAccessor>(psourcetec->add(COLLADA_ELEMENT_ACCESSOR));
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());
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)
@ -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;

5
collada_urdf/src/urdf_to_collada.cpp
View File

@ -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);

45
collada_urdf/test/test_collada_urdf.cpp
View File

@ -34,52 +34,23 @@
#include <fstream>
#include <sstream>
#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)
{
boost::shared_ptr<DAE> 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<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)
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<DAE> 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();