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

remove check_urdf and urdf_to_graphviz (part of urdfdom)

This commit is contained in:
Ioan Sucan 2012-12-04 22:04:25 +02:00
parent aee2805f74
commit cb816dc872
3 changed files with 1 additions and 233 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
urdf/CMakeLists.txt
View File

@ -26,19 +26,13 @@ if(APPLE)
set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup") set_target_properties(${PROJECT_NAME} PROPERTIES LINK_FLAGS "-undefined dynamic_lookup")
endif(APPLE) 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) catkin_add_gtest(test_parser EXCLUDE_FROM_ALL test/test_robot_model_parser.cpp)
target_link_libraries(test_parser ${PROJECT_NAME}) target_link_libraries(test_parser ${PROJECT_NAME})
# no idea how CATKIN does this # no idea how CATKIN does this
# rosbuild_add_rostest(${PROJECT_SOURCE_DIR}/test/test_robot_model_parser.launch) # 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} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

92
urdf/src/check_urdf.cpp
View File

@ -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 <iostream>
#include <fstream>
using namespace urdf;
void printTree(boost::shared_ptr<const Link> link,int level = 0)
{
level+=2;
int count = 0;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
{
if (*child)
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
// first grandchild
printTree(*child,level);
}
else
{
for(int j=0;j<level;j++) std::cout << " "; //indent
std::cout << "root link: " << link->name << " 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<const Link> 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;
}

134
urdf/src/urdf_to_graphiz.cpp
View File

@ -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 <ros/console.h>
#include <iostream>
#include <fstream>
using namespace urdf;
using namespace std;
void addChildLinkNames(boost::shared_ptr<const Link> link, ofstream& os)
{
os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl;
for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
addChildLinkNames(*child, os);
}
void addChildJointNames(boost::shared_ptr<const Link> link, ofstream& os)
{
double r, p, y;
for (std::vector<boost::shared_ptr<Link> >::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<const Link> 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<ModelInterface> robot;
if( xml_string.find("<COLLADA") != std::string::npos )
{
ROS_DEBUG("Parsing robot collada xml string");
robot = parseCollada(xml_string);
}
else
{
ROS_DEBUG("Parsing robot urdf xml string");
robot = parseURDF(xml_string);
}
if (!robot){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return -1;
}
string output = robot->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;
}