Go to the documentation of this file.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 "kdl_parser/kdl_parser.hpp"
00038 #include <kdl/chainfksolverpos_recursive.hpp>
00039 #include <kdl/frames_io.hpp>
00040 #include <urdf/model.h>
00041 #include <iostream>
00042
00043 using namespace KDL;
00044 using namespace std;
00045 using namespace urdf;
00046
00047 void printLink(const SegmentMap::const_iterator& link, const std::string& prefix)
00048 {
00049 cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() << " has "
00050 << GetTreeElementChildren(link->second).size() << " children" << endl;
00051 for (unsigned int i=0; i < GetTreeElementChildren(link->second).size(); i++)
00052 printLink(GetTreeElementChildren(link->second)[i], prefix + " ");
00053 }
00054
00055
00056 int main(int argc, char** argv)
00057 {
00058 if (argc < 2){
00059 std::cerr << "Expect xml file to parse" << std::endl;
00060 return -1;
00061 }
00062 Model robot_model;
00063 if (!robot_model.initFile(argv[1]))
00064 {cerr << "Could not generate robot model" << endl; return false;}
00065
00066 Tree my_tree;
00067 if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree))
00068 {cerr << "Could not extract kdl tree" << endl; return false;}
00069
00070
00071 cout << " ======================================" << endl;
00072 cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << endl;
00073 cout << " ======================================" << endl;
00074 SegmentMap::const_iterator root = my_tree.getRootSegment();
00075 printLink(root, "");
00076 }
00077
00078