00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "urdf/model.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 TiXmlDocument robot_model_xml;
00094 robot_model_xml.LoadFile(argv[1]);
00095 TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
00096 if (!robot_xml){
00097 std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
00098 return -1;
00099 }
00100
00101 Model robot;
00102 if (!robot.initXml(robot_xml)){
00103 std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
00104 return -1;
00105 }
00106 string output = robot.getName();
00107
00108
00109 printTree(robot.getRoot(), output+".gv");
00110 cout << "Created file " << output << ".gv" << endl;
00111
00112 string command = "dot -Tpdf "+output+".gv -o "+output+".pdf";
00113 system(command.c_str());
00114 cout << "Created file " << output << ".pdf" << endl;
00115 return 0;
00116 }
00117