1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redstributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35 /* Author: Wim Meeussen */
36
37 #include "urdf_parser/urdf_parser.h"
38 #include <iostream>
39 #include <fstream>
40
41 using namespace urdf;
42 using namespace std;
43
addChildLinkNames(LinkConstSharedPtr link,ofstream & os)44 void addChildLinkNames(LinkConstSharedPtr link, ofstream& os)
45 {
46 os << "\"" << link->name << "\" [label=\"" << link->name << "\"];" << endl;
47 for (std::vector<LinkSharedPtr>::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
48 addChildLinkNames(*child, os);
49 }
50
addChildJointNames(LinkConstSharedPtr link,ofstream & os)51 void addChildJointNames(LinkConstSharedPtr link, ofstream& os)
52 {
53 double r, p, y;
54 for (std::vector<LinkSharedPtr>::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
55 (*child)->parent_joint->parent_to_joint_origin_transform.rotation.getRPY(r,p,y);
56 os << "\"" << link->name << "\" -> \"" << (*child)->parent_joint->name
57 << "\" [label=\"xyz: "
58 << (*child)->parent_joint->parent_to_joint_origin_transform.position.x << " "
59 << (*child)->parent_joint->parent_to_joint_origin_transform.position.y << " "
60 << (*child)->parent_joint->parent_to_joint_origin_transform.position.z << " "
61 << "\\nrpy: " << r << " " << p << " " << y << "\"]" << endl;
62 os << "\"" << (*child)->parent_joint->name << "\" -> \"" << (*child)->name << "\"" << endl;
63 addChildJointNames(*child, os);
64 }
65 }
66
67
printTree(LinkConstSharedPtr link,string file)68 void printTree(LinkConstSharedPtr link, string file)
69 {
70 std::ofstream os;
71 os.open(file.c_str());
72 os << "digraph G {" << endl;
73
74 os << "node [shape=box];" << endl;
75 addChildLinkNames(link, os);
76
77 os << "node [shape=ellipse, color=blue, fontcolor=blue];" << endl;
78 addChildJointNames(link, os);
79
80 os << "}" << endl;
81 os.close();
82 }
83
84
85
main(int argc,char ** argv)86 int main(int argc, char** argv)
87 {
88 if (argc != 2){
89 std::cerr << "Usage: urdf_to_graphiz input.xml" << std::endl;
90 return -1;
91 }
92
93 // get the entire file
94 std::string xml_string;
95 std::fstream xml_file(argv[1], std::fstream::in);
96 while ( xml_file.good() )
97 {
98 std::string line;
99 std::getline( xml_file, line);
100 xml_string += (line + "\n");
101 }
102 xml_file.close();
103
104 ModelInterfaceSharedPtr robot = parseURDF(xml_string);
105 if (!robot){
106 std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
107 return -1;
108 }
109 string output = robot->getName();
110
111 // print entire tree to file
112 printTree(robot->getRoot(), output+".gv");
113 cout << "Created file " << output << ".gv" << endl;
114
115 string command = "dot -Tpdf "+output+".gv -o "+output+".pdf";
116 if (system(command.c_str()) != -1)
117 cout << "Created file " << output << ".pdf" << endl;
118 else
119 cout << "There was an error executing '" << command << "'" << endl;
120 return 0;
121 }
122
123