diff --git a/collada_parser/CHANGELOG.rst b/collada_parser/CHANGELOG.rst index bd40244f..22220b5f 100644 --- a/collada_parser/CHANGELOG.rst +++ b/collada_parser/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package collada_parser ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.12.7 (2017-01-26) +------------------- + 1.12.6 (2017-01-04) ------------------- * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) diff --git a/collada_parser/CMakeLists.txt b/collada_parser/CMakeLists.txt index e35694e7..961ac9c0 100644 --- a/collada_parser/CMakeLists.txt +++ b/collada_parser/CMakeLists.txt @@ -6,6 +6,8 @@ find_package(Boost REQUIRED system) find_package(catkin REQUIRED COMPONENTS urdf_parser_plugin roscpp class_loader urdf) find_package(urdfdom_headers REQUIRED) +add_compile_options(-std=c++11) + catkin_package( LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include diff --git a/collada_parser/package.xml b/collada_parser/package.xml index 951add6f..41c0afa5 100644 --- a/collada_parser/package.xml +++ b/collada_parser/package.xml @@ -1,6 +1,6 @@ collada_parser - 1.12.6 + 1.12.7 This package contains a C++ parser for the Collada robot description format. The parser reads a Collada XML robot @@ -8,10 +8,14 @@ to directly use this parser when working with Collada robot descriptions, the preferred user API is found in the urdf package. + Rosen Diankov Kei Okada - Ioan Sucan - Jackie Kay + Ioan Sucan + Jackie Kay + Chris Lalancette + Shane Loretz + BSD http://ros.org/wiki/collada_parser diff --git a/collada_parser/src/collada_parser.cpp b/collada_parser/src/collada_parser.cpp index e67f15be..ef5fde15 100644 --- a/collada_parser/src/collada_parser.cpp +++ b/collada_parser/src/collada_parser.cpp @@ -42,8 +42,13 @@ #include #include +/* disable deprecated auto_ptr warnings */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" #include #include +#pragma GCC diagnostic pop + #include #include #include @@ -67,6 +72,7 @@ #include #endif +#define typeof __typeof__ #define FOREACH(it, v) for(typeof((v).begin())it = (v).begin(); it != (v).end(); (it)++) #define FOREACHC FOREACH diff --git a/collada_urdf/CHANGELOG.rst b/collada_urdf/CHANGELOG.rst index e3e495bb..ee03fe3c 100644 --- a/collada_urdf/CHANGELOG.rst +++ b/collada_urdf/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package collada_urdf ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.12.7 (2017-01-26) +------------------- + 1.12.6 (2017-01-04) ------------------- * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) diff --git a/collada_urdf/CMakeLists.txt b/collada_urdf/CMakeLists.txt index 113dee0e..d230de8e 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/package.xml b/collada_urdf/package.xml index 2b81e503..691139c1 100644 --- a/collada_urdf/package.xml +++ b/collada_urdf/package.xml @@ -1,6 +1,6 @@ collada_urdf - 1.12.6 + 1.12.7 This package contains a tool to convert Unified Robot Description Format (URDF) documents into COLLAborative Design Activity (COLLADA) documents. @@ -10,8 +10,10 @@ Tim Field Rosen Diankov - Ioan Sucan - Jackie Kay + Ioan Sucan + Jackie Kay + Chris Lalancette + Shane Loretz BSD diff --git a/collada_urdf/src/collada_to_urdf.cpp b/collada_urdf/src/collada_to_urdf.cpp index 14e9741a..616f58ac 100644 --- a/collada_urdf/src/collada_to_urdf.cpp +++ b/collada_urdf/src/collada_to_urdf.cpp @@ -31,10 +31,8 @@ #include #include -#undef GAZEBO_1_0 #undef GAZEBO_1_3 -//#define GAZEBO_1_0 #define GAZEBO_1_3 using namespace urdf; @@ -385,17 +383,6 @@ void addChildLinkNamesXML(urdf::LinkConstSharedPtr link, ofstream& os) } os << " " << endl; -#ifdef GAZEBO_1_0 - if ( add_gazebo_description ) { - os << " name << "\">" << endl; - os << " Gazebo/Grey" << endl; - //os << " 0.9" << endl; - //os << " 0.9" << endl; - os << " false" << endl; - os << " " << endl; - } -#endif - #ifdef GAZEBO_1_3 if ( add_gazebo_description ) { os << " name << "\">" << endl; @@ -507,37 +494,12 @@ void printTreeXML(urdf::LinkConstSharedPtr link, string name, string file) os.open(file.c_str()); os << "" << endl; os << "" << endl; + os << " xmlns:xi=\"http://www.w3.org/2001/XInclude\">" << endl; addChildLinkNamesXML(link, os); addChildJointNamesXML(link, os); - if ( add_gazebo_description ) { -#ifdef GAZEBO_1_0 - // old gazebo (gazebo on ROS Fuerte) - os << " " << endl; - os << " " << endl; - os << " true" << endl; - os << " 1000.0" << endl; - os << " " << endl; - os << " " << endl; -#endif - } - os << "" << endl; os.close(); } diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp index fc1a7209..bef2c0cc 100644 --- a/collada_urdf/src/collada_urdf.cpp +++ b/collada_urdf/src/collada_urdf.cpp @@ -949,17 +949,17 @@ class ColladaWriter : public daeErrorHandler // 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 @@ class ColladaWriter : public daeErrorHandler 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 @@ class ColladaWriter : public daeErrorHandler 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 @@ class ColladaWriter : public daeErrorHandler 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()); - - // 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"); + // 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"); } } @@ -1282,28 +1283,28 @@ class ColladaWriter : public daeErrorHandler 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 / 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)); + 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 @@ class ColladaWriter : public daeErrorHandler } 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()); - - 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()); + + // 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()); + + 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 @@ class ColladaWriter : public daeErrorHandler 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 0ab0bf9b..e2eccd16 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 239ec0e9..15428343 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")); -} - -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); + urdf::Model robot_model; - boost::shared_ptr dom; - ASSERT_TRUE(collada_urdf::colladaFromUrdfXml(&urdf_xml, 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_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(); diff --git a/joint_state_publisher/CHANGELOG.rst b/joint_state_publisher/CHANGELOG.rst index c43e6a71..8dfa3fd5 100644 --- a/joint_state_publisher/CHANGELOG.rst +++ b/joint_state_publisher/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package joint_state_publisher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.12.7 (2017-01-26) +------------------- +* Fixed a crash which happened when there were ``0`` free joints, opens empty window (`#178 `_) +* Contributors: Bence Magyar + 1.12.6 (2017-01-04) ------------------- * Migrated slots in joint state publisher gui to Qt5 (`#147 `_) diff --git a/joint_state_publisher/CMakeLists.txt b/joint_state_publisher/CMakeLists.txt index 1c50f935..c6b3808b 100644 --- a/joint_state_publisher/CMakeLists.txt +++ b/joint_state_publisher/CMakeLists.txt @@ -6,3 +6,10 @@ find_package(catkin REQUIRED) catkin_package() install(PROGRAMS joint_state_publisher/joint_state_publisher DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + + +if(CATKIN_ENABLE_TESTING) + find_package(catkin REQUIRED COMPONENTS rostest) + add_rostest(test/test_mimic_chain.launch) + add_rostest(test/test_mimic_cycle.launch) +endif() diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher b/joint_state_publisher/joint_state_publisher/joint_state_publisher index a2e8d311..7badd65a 100755 --- a/joint_state_publisher/joint_state_publisher/joint_state_publisher +++ b/joint_state_publisher/joint_state_publisher/joint_state_publisher @@ -214,9 +214,21 @@ class JointStatePublisher(): elif name in self.dependent_joints: param = self.dependent_joints[name] parent = param['parent'] - joint = self.free_joints[parent] factor = param.get('factor', 1) offset = param.get('offset', 0) + # Handle recursive mimic chain + recursive_mimic_chain_joints = [name] + while parent in self.dependent_joints: + if parent in recursive_mimic_chain_joints: + error_message = "Found an infinite recursive mimic chain" + rospy.logerr("%s: [%s, %s]", error_message, ', '.join(recursive_mimic_chain_joints), parent) + sys.exit(-1) + recursive_mimic_chain_joints.append(parent) + param = self.dependent_joints[parent] + parent = param['parent'] + offset += factor * param.get('offset', 0) + factor *= param.get('factor', 1) + joint = self.free_joints[parent] if has_position and 'position' in joint: msg.position[i] = joint['position'] * factor + offset @@ -380,6 +392,8 @@ class JointStatePublisherGui(QWidget): self.gridlayout.addLayout(item, *pos) def generate_grid_positions(self, num_items, num_rows): + if num_rows==0: + return [] positions = [(y, x) for x in range(int((math.ceil(float(num_items) / num_rows)))) for y in range(num_rows)] positions = positions[:num_items] return positions diff --git a/joint_state_publisher/package.xml b/joint_state_publisher/package.xml index f3933e3d..d6d5832f 100644 --- a/joint_state_publisher/package.xml +++ b/joint_state_publisher/package.xml @@ -1,6 +1,6 @@ joint_state_publisher - 1.12.6 + 1.12.7 This package contains a tool for setting and publishing joint state values for a given URDF. diff --git a/joint_state_publisher/test/mimic_chain.urdf b/joint_state_publisher/test/mimic_chain.urdf new file mode 100644 index 00000000..9cf0c8ba --- /dev/null +++ b/joint_state_publisher/test/mimic_chain.urdf @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/joint_state_publisher/test/mimic_cycle.urdf b/joint_state_publisher/test/mimic_cycle.urdf new file mode 100644 index 00000000..233a7c56 --- /dev/null +++ b/joint_state_publisher/test/mimic_cycle.urdf @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/joint_state_publisher/test/test_mimic_chain.launch b/joint_state_publisher/test/test_mimic_chain.launch new file mode 100644 index 00000000..385edd0b --- /dev/null +++ b/joint_state_publisher/test/test_mimic_chain.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/joint_state_publisher/test/test_mimic_cycle.launch b/joint_state_publisher/test/test_mimic_cycle.launch new file mode 100644 index 00000000..cfec4c35 --- /dev/null +++ b/joint_state_publisher/test/test_mimic_cycle.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/kdl_parser/CHANGELOG.rst b/kdl_parser/CHANGELOG.rst index b4555c8d..b32e954b 100644 --- a/kdl_parser/CHANGELOG.rst +++ b/kdl_parser/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package kdl_parser ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.12.7 (2017-01-26) +------------------- + 1.12.6 (2017-01-04) ------------------- * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) diff --git a/kdl_parser/CMakeLists.txt b/kdl_parser/CMakeLists.txt index 43b96b7a..70124c1a 100644 --- a/kdl_parser/CMakeLists.txt +++ b/kdl_parser/CMakeLists.txt @@ -19,6 +19,8 @@ link_directories(${Boost_LIBRARY_DIRS}) find_library(KDL_LIBRARY REQUIRED NAMES orocos-kdl HINTS ${orocos_kdl_LIBRARY_DIRS}) +add_compile_options(-std=c++11) + catkin_package( LIBRARIES ${PROJECT_NAME} ${KDL_LIBRARY} INCLUDE_DIRS include diff --git a/kdl_parser/package.xml b/kdl_parser/package.xml index c6c32fa8..0ccdf573 100644 --- a/kdl_parser/package.xml +++ b/kdl_parser/package.xml @@ -1,6 +1,6 @@ kdl_parser - 1.12.6 + 1.12.7 The Kinematics and Dynamics Library (KDL) defines a tree structure to represent the kinematic and dynamic parameters of a robot @@ -9,8 +9,10 @@ Wim Meeussen - Ioan Sucan - Jackie Kay + Ioan Sucan + Jackie Kay + Chris Lalancette + Shane Loretz BSD diff --git a/kdl_parser_py/CHANGELOG.rst b/kdl_parser_py/CHANGELOG.rst index dc64133e..cb7b06b7 100644 --- a/kdl_parser_py/CHANGELOG.rst +++ b/kdl_parser_py/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package kdl_parser_py ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.12.7 (2017-01-26) +------------------- + 1.12.6 (2017-01-04) ------------------- diff --git a/kdl_parser_py/package.xml b/kdl_parser_py/package.xml index 5182a2fe..94fb02f0 100644 --- a/kdl_parser_py/package.xml +++ b/kdl_parser_py/package.xml @@ -1,6 +1,6 @@ kdl_parser_py - 1.12.6 + 1.12.7 The Kinematics and Dynamics Library (KDL) defines a tree structure to represent the kinematic and dynamic parameters of a robot diff --git a/robot_model/CHANGELOG.rst b/robot_model/CHANGELOG.rst index 0d4c265d..09804297 100644 --- a/robot_model/CHANGELOG.rst +++ b/robot_model/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package robot_model ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.12.7 (2017-01-26) +------------------- + 1.12.6 (2017-01-04) ------------------- diff --git a/robot_model/package.xml b/robot_model/package.xml index c7133f97..280a0b25 100644 --- a/robot_model/package.xml +++ b/robot_model/package.xml @@ -1,6 +1,6 @@ robot_model - 1.12.6 + 1.12.7 robot_model contains packages for modeling various aspects of robot information, specified in the Xml Robot @@ -10,8 +10,9 @@ Ioan Sucan - Ioan Sucan - Jackie Kay + Jackie Kay + Chris Lalancette + Shane Loretz BSD diff --git a/urdf/CHANGELOG.rst b/urdf/CHANGELOG.rst index 79ee5d1e..cd172d66 100644 --- a/urdf/CHANGELOG.rst +++ b/urdf/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package urdf ^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.12.7 (2017-01-26) +------------------- + 1.12.6 (2017-01-04) ------------------- * Addressed gcc6 build error in the urdf package, forward port of `#156 `_ (`#173 `_) diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index eeed0bda..6d706bc1 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -21,6 +21,8 @@ set(generated_compat_header "${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/urdf include_directories("${CATKIN_DEVEL_PREFIX}/include") configure_file(urdfdom_compatibility.h.in "${generated_compat_header}" @ONLY) +add_compile_options(-std=c++11) + catkin_package( LIBRARIES ${PROJECT_NAME} INCLUDE_DIRS include ${TinyXML_INLCLUDE_DIRS} ${CATKIN_DEVEL_PREFIX}/include diff --git a/urdf/include/urdf/model.h b/urdf/include/urdf/model.h index f9e676a8..e9041d9d 100644 --- a/urdf/include/urdf/model.h +++ b/urdf/include/urdf/model.h @@ -44,6 +44,7 @@ #include #include #include +#include namespace urdf{ @@ -58,6 +59,8 @@ class Model: public ModelInterface bool initFile(const std::string& filename); /// \brief Load Model given the name of a parameter on the parameter server bool initParam(const std::string& param); + /// \brief Load Model given the name of a parameter on the parameter server using provided nodehandle + bool initParamWithNodeHandle(const std::string& param, const ros::NodeHandle& nh = ros::NodeHandle()); /// \brief Load Model from a XML-string bool initString(const std::string& xmlstring); }; diff --git a/urdf/package.xml b/urdf/package.xml index ff618fe7..dc81b2e0 100644 --- a/urdf/package.xml +++ b/urdf/package.xml @@ -1,16 +1,17 @@ urdf - 1.12.6 + 1.12.7 This package contains a C++ parser for the Unified Robot Description - Format (URDF), which is an XML format for representing a robot model. + Format (URDF), which is an XML format for representing a robot model. The code API of the parser has been through our review process and will remain backwards compatible in future releases. - Ioan Sucan - Ioan Sucan - Jackie Kay + Ioan Sucan + Jackie Kay + Chris Lalancette + Shane Loretz BSD diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp index 69f96b80..03cc774f 100644 --- a/urdf/src/model.cpp +++ b/urdf/src/model.cpp @@ -36,8 +36,6 @@ #include "urdf/model.h" -#include - /* we include the default parser for plain URDF files; other parsers are loaded via plugins (if available) */ #include @@ -88,7 +86,11 @@ bool Model::initFile(const std::string& filename) bool Model::initParam(const std::string& param) { - ros::NodeHandle nh; + return initParamWithNodeHandle(param, ros::NodeHandle()); +} + +bool Model::initParamWithNodeHandle(const std::string& param, const ros::NodeHandle& nh) +{ std::string xml_string; // gets the location of the robot description on the parameter server diff --git a/urdf/test/test_robot_model_parser.cpp b/urdf/test/test_robot_model_parser.cpp index 81f90af8..a730eb51 100644 --- a/urdf/test/test_robot_model_parser.cpp +++ b/urdf/test/test_robot_model_parser.cpp @@ -94,7 +94,7 @@ class TestParser : public testing::Test double roll,pitch,yaw; (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(roll,pitch,yaw); - if (isnan(roll) || isnan(pitch) || isnan(yaw)) + if (std::isnan(roll) || std::isnan(pitch) || std::isnan(yaw)) { ROS_ERROR("getRPY() returned nan!"); return false; diff --git a/urdf_parser_plugin/CHANGELOG.rst b/urdf_parser_plugin/CHANGELOG.rst index ede14dff..6f7985d0 100644 --- a/urdf_parser_plugin/CHANGELOG.rst +++ b/urdf_parser_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package urdf_parser_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.12.7 (2017-01-26) +------------------- + 1.12.6 (2017-01-04) ------------------- * Now using ``urdf::*ShredPtr`` instead of ``boost::shared_ptr`` (`#144 `_) diff --git a/urdf_parser_plugin/package.xml b/urdf_parser_plugin/package.xml index ab7f823c..535436c0 100644 --- a/urdf_parser_plugin/package.xml +++ b/urdf_parser_plugin/package.xml @@ -1,13 +1,14 @@ urdf_parser_plugin - 1.12.6 + 1.12.7 This package contains a C++ base class for URDF parsers. Ioan Sucan - Ioan Sucan - Jackie Kay + Jackie Kay + Chris Lalancette + Shane Loretz BSD