check_kdl_parser.cpp
Go to the documentation of this file.
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 <iostream>
38 #include <string>
39 
41 #include <kdl/chainfksolverpos_recursive.hpp>
42 #include <kdl/frames_io.hpp>
43 #include <urdf/model.h>
44 
45 void printLink(const KDL::SegmentMap::const_iterator & link, const std::string & prefix)
46 {
47  std::cout << prefix << "- Segment " << GetTreeElementSegment(link->second).getName() <<
48  " has " << GetTreeElementChildren(link->second).size() << " children" << std::endl;
49  for (unsigned int i = 0; i < GetTreeElementChildren(link->second).size(); i++) {
50  printLink(GetTreeElementChildren(link->second)[i], prefix + " ");
51  }
52 }
53 
54 
55 int main(int argc, char ** argv)
56 {
57  if (argc < 2) {
58  std::cerr << "Expect xml file to parse" << std::endl;
59  return -1;
60  }
61  urdf::Model robot_model;
62  if (!robot_model.initFile(argv[1])) {
63  std::cerr << "Could not generate robot model" << std::endl;
64  return false;
65  }
66 
67  KDL::Tree my_tree;
68  if (!kdl_parser::treeFromUrdfModel(robot_model, my_tree)) {
69  std::cerr << "Could not extract kdl tree" << std::endl;
70  return false;
71  }
72 
73  // walk through tree
74  std::cout << " ======================================" << std::endl;
75  std::cout << " Tree has " << my_tree.getNrOfSegments() << " link(s) and a root link" << std::endl;
76  std::cout << " ======================================" << std::endl;
77  KDL::SegmentMap::const_iterator root = my_tree.getRootSegment();
78  printLink(root, "");
79 }
URDF_EXPORT bool initFile(const std::string &filename)
#define GetTreeElementSegment(tree_element)
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
Definition: kdl_parser.cpp:196
#define GetTreeElementChildren(tree_element)
int main(int argc, char **argv)
void printLink(const KDL::SegmentMap::const_iterator &link, const std::string &prefix)
SegmentMap::const_iterator getRootSegment() const
unsigned int getNrOfSegments() const


kdl_parser
Author(s): Wim Meeussen , Ioan Sucan , Jackie Kay
autogenerated on Wed Jun 5 2019 21:10:57