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
00040 using namespace urdf;
00041
00042 void printTree(boost::shared_ptr<const Link> link,int level = 0)
00043 {
00044 level+=2;
00045 int count = 0;
00046 for (std::vector<boost::shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
00047 {
00048 if (*child)
00049 {
00050 for(int j=0;j<level;j++) std::cout << " ";
00051 std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
00052
00053 printTree(*child,level);
00054 }
00055 else
00056 {
00057 for(int j=0;j<level;j++) std::cout << " ";
00058 std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
00059 }
00060 }
00061
00062 }
00063
00064
00065 int main(int argc, char** argv)
00066 {
00067 if (argc < 2){
00068 std::cerr << "Expect xml file to parse" << std::endl;
00069 return -1;
00070 }
00071 TiXmlDocument robot_model_xml;
00072 robot_model_xml.LoadFile(argv[1]);
00073 TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
00074 if (!robot_xml){
00075 std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
00076 return -1;
00077 }
00078
00079 Model robot;
00080 if (!robot.initXml(robot_xml)){
00081 std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
00082 return -1;
00083 }
00084
00085 std::cout << "robot name is: " << robot.getName() << std::endl;
00086
00087
00088 std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
00089
00090 boost::shared_ptr<const Link> root_link=robot.getRoot();
00091 if (!root_link) return -1;
00092
00093 std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
00094
00095
00096
00097 printTree(root_link);
00098 return 0;
00099 }
00100