diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 84e87a4..f535df1 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -26,19 +26,13 @@ if(APPLE) set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") endif(APPLE) -add_executable(check_urdf src/check_urdf.cpp) -target_link_libraries(check_urdf ${PROJECT_NAME}) - -add_executable(urdf_to_graphiz src/urdf_to_graphiz.cpp) -target_link_libraries(urdf_to_graphiz ${PROJECT_NAME}) - catkin_add_gtest(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp) target_link_libraries(test_parser ${PROJECT_NAME}) # no idea how CATKIN does this # rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch) -install(TARGETS ${PROJECT_NAME} check_urdf urdf_to_graphiz +install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/urdf/src/check_urdf.cpp b/urdf/src/check_urdf.cpp deleted file mode 100644 index 7bedf8a..0000000 --- a/urdf/src/check_urdf.cpp +++ /dev/null @@ -1,92 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, 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: -* -* * Redistributions 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 -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * 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 -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#include "urdf/model.h" -#include -#include - -using namespace urdf; - -void printTree(boost::shared_ptr link,int level = 0) -{ - level+=2; - int count = 0; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - { - if (*child) - { - for(int j=0;jname << std::endl; - // first grandchild - printTree(*child,level); - } - else - { - for(int j=0;jname << " has a null child!" << *child << std::endl; - } - } - -} - - -int main(int argc, char** argv) -{ - if (argc < 2){ - std::cerr << "Expect URDF or COLLADA xml file to parse" << std::endl; - return -1; - } - - urdf::Model robot; - if (!robot.initFile(argv[1])){ - std::cerr << "ERROR: Model Parsing the xml failed" << std::endl; - return -1; - } - std::cout << "robot name is: " << robot.getName() << std::endl; - - // get info from parser - std::cout << "---------- Successfully Parsed XML ---------------" << std::endl; - // get root link - boost::shared_ptr root_link=robot.getRoot(); - if (!root_link) return -1; - - std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl; - - // print entire tree - printTree(root_link); - return 0; -} - diff --git a/urdf/src/urdf_to_graphiz.cpp b/urdf/src/urdf_to_graphiz.cpp deleted file mode 100644 index febcd2c..0000000 --- a/urdf/src/urdf_to_graphiz.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, 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 -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * 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 -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ - -/* Author: Wim Meeussen */ - -#include "urdf_parser/urdf_parser.h" -#include "collada_parser/collada_parser.h" -#include -#include -#include - -using namespace urdf; -using namespace std; - -void addChildLinkNames(boost::shared_ptr link, ofstream& os) -{ - os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++) - addChildLinkNames(*child, os); -} - -void addChildJointNames(boost::shared_ptr link, ofstream& os) -{ - double r, p, y; - for (std::vector >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){ - (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y); - os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name - << "\" [label=\"xyz: " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " " - << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " " - << "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl; - os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl; - addChildJointNames(*child, os); - } -} - - -void printTree(boost::shared_ptr link, string file) -{ - std::ofstream os; - os.open(file.c_str()); - os << "digraph G {" << endl; - - os << "node [shape=box];" << endl; - addChildLinkNames(link, os); - - os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl; - addChildJointNames(link, os); - - os << "}" << endl; - os.close(); -} - - - -int main(int argc, char** argv) -{ - if (argc != 2){ - std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl; - return -1; - } - - // get the entire file - std::string xml_string; - std::fstream xml_file(argv[1], std::fstream::in); - while ( xml_file.good() ) - { - std::string line; - std::getline( xml_file, line); - xml_string += (line + "\n"); - } - xml_file.close(); - - boost::shared_ptr robot; - if( xml_string.find("getName(); - - // print entire tree to file - printTree(robot->getRoot(), output+".gv"); - cout << "Created file " << output << ".gv" << endl; - - string command = "dot -Tpdf "+output+".gv -o "+output+".pdf"; - if (system(command.c_str()) != -1) - cout << "Created file " << output << ".pdf" << endl; - else - cout << "There was an error executing '" << command << "'" << endl; - return 0; -} -