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 * * Redistributions 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
printTree(LinkConstSharedPtr link,int level=0)43 void printTree(LinkConstSharedPtr link,int level = 0)
44 {
45 level+=2;
46 int count = 0;
47 for (std::vector<LinkSharedPtr>::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
48 {
49 if (*child)
50 {
51 for(int j=0;j<level;j++) std::cout << " "; //indent
52 std::cout << "child(" << (count++)+1 << "): " << (*child)->name << std::endl;
53 // first grandchild
54 printTree(*child,level);
55 }
56 else
57 {
58 for(int j=0;j<level;j++) std::cout << " "; //indent
59 std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
60 }
61 }
62
63 }
64
65
main(int argc,char ** argv)66 int main(int argc, char** argv)
67 {
68 if (argc < 2){
69 std::cerr << "Expect URDF xml file to parse" << std::endl;
70 return -1;
71 }
72
73 std::string xml_string;
74 std::fstream xml_file(argv[1], std::fstream::in);
75 while ( xml_file.good() )
76 {
77 std::string line;
78 std::getline( xml_file, line);
79 xml_string += (line + "\n");
80 }
81 xml_file.close();
82
83 ModelInterfaceSharedPtr robot = parseURDF(xml_string);
84 if (!robot){
85 std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
86 return -1;
87 }
88 std::cout << "robot name is: " << robot->getName() << std::endl;
89
90 // get info from parser
91 std::cout << "---------- Successfully Parsed XML ---------------" << std::endl;
92 // get root link
93 LinkConstSharedPtr root_link=robot->getRoot();
94 if (!root_link) return -1;
95
96 std::cout << "root Link: " << root_link->name << " has " << root_link->child_links.size() << " child(ren)" << std::endl;
97
98
99 // print entire tree
100 printTree(root_link);
101 return 0;
102 }
103
104