urdf_parser: urdf_parser: urdf_to_graphiz.cpp Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redstributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Wim Meeussen */
00036 
00037 #include "urdf_parser/urdf_parser.h"
00038 #include <iostream>
00039 #include <fstream>
00040 
00041 using namespace urdf;
00042 using namespace std;
00043 
00044 void addChildLinkNames(boost::shared_ptr<const Link> link, ofstream& os)
00045 {
00046   os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl;
00047   for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
00048     addChildLinkNames(*child, os);
00049 }
00050 
00051 void addChildJointNames(boost::shared_ptr<const Link> link, ofstream& os)
00052 {
00053   double r, p, y;
00054   for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
00055     (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
00056     os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name 
00057        << "\" [label=\"xyz: "
00058        << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " " 
00059        << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " " 
00060        << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " " 
00061        << "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl;
00062     os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl;
00063     addChildJointNames(*child, os);
00064   }
00065 }
00066 
00067 
00068 void printTree(boost::shared_ptr<const Link> link, string file)
00069 {
00070   std::ofstream os;
00071   os.open(file.c_str());
00072   os << "digraph G {" << endl;
00073 
00074   os << "node [shape=box];" << endl;
00075   addChildLinkNames(link, os);
00076 
00077   os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl;
00078   addChildJointNames(link, os);
00079 
00080   os << "}" << endl;
00081   os.close();
00082 }
00083 
00084 
00085 
00086 int main(int argc, char** argv)
00087 {
00088   if (argc != 2){
00089     std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl;
00090     return -1;
00091   }
00092 
00093   // get the entire file
00094   std::string xml_string;
00095   std::fstream xml_file(argv[1], std::fstream::in);
00096   while ( xml_file.good() )
00097   {
00098     std::string line;
00099     std::getline( xml_file, line);
00100     xml_string += (line + "\n");
00101   }
00102   xml_file.close();
00103 
00104   boost::shared_ptr<ModelInterface> robot = parseURDF(xml_string);
00105   if (!robot){
00106     std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
00107     return -1;
00108   }
00109   string output = robot->getName();
00110 
00111   // print entire tree to file
00112   printTree(robot->getRoot(), output+".gv");
00113   cout << "Created file " << output << ".gv" << endl;
00114 
00115   string command = "dot -Tpdf "+output+".gv  -o "+output+".pdf";
00116   system(command.c_str());
00117   cout << "Created file " << output << ".pdf" << endl;
00118   return 0;
00119 }
00120