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


kdl_parser
Author(s): Wim Meeussen , Ioan Sucan , Jackie Kay
autogenerated on Fri Apr 15 2022 03:01:26