$search
00001 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00002 00003 // Version: 1.0 00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be> 00006 // URL: http://www.orocos.org/kdl 00007 00008 // This library is free software; you can redistribute it and/or 00009 // modify it under the terms of the GNU Lesser General Public 00010 // License as published by the Free Software Foundation; either 00011 // version 2.1 of the License, or (at your option) any later version. 00012 00013 // This library is distributed in the hope that it will be useful, 00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00016 // Lesser General Public License for more details. 00017 00018 // You should have received a copy of the GNU Lesser General Public 00019 // License along with this library; if not, write to the Free Software 00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 00021 00022 #ifndef KDL_TREE_HPP 00023 #define KDL_TREE_HPP 00024 00025 #include "segment.hpp" 00026 #include "chain.hpp" 00027 00028 #include <string> 00029 #include <map> 00030 00031 namespace KDL 00032 { 00033 //Forward declaration 00034 class TreeElement; 00035 typedef std::map<std::string,TreeElement> SegmentMap; 00036 00037 class TreeElement 00038 { 00039 private: 00040 TreeElement(const std::string& name):segment(name), q_nr(0) 00041 {}; 00042 public: 00043 Segment segment; 00044 unsigned int q_nr; 00045 SegmentMap::const_iterator parent; 00046 std::vector<SegmentMap::const_iterator > children; 00047 TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in) 00048 { 00049 q_nr=q_nr_in; 00050 segment=segment_in; 00051 parent=parent_in; 00052 }; 00053 static TreeElement Root(const std::string& root_name) 00054 { 00055 return TreeElement(root_name); 00056 }; 00057 }; 00058 00065 class Tree 00066 { 00067 private: 00068 SegmentMap segments; 00069 int nrOfJoints; 00070 int nrOfSegments; 00071 00072 std::string root_name; 00073 00074 bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name); 00075 00076 public: 00080 explicit Tree(const std::string& root_name="root"); 00081 Tree(const Tree& in); 00082 Tree& operator= (const Tree& arg); 00083 00094 bool addSegment(const Segment& segment, const std::string& hook_name); 00095 00104 bool addChain(const Chain& chain, const std::string& hook_name); 00105 00115 bool addTree(const Tree& tree, const std::string& hook_name); 00116 00125 unsigned int getNrOfJoints()const 00126 { 00127 return nrOfJoints; 00128 }; 00129 00134 unsigned int getNrOfSegments()const {return nrOfSegments;}; 00135 00143 SegmentMap::const_iterator getSegment(const std::string& segment_name)const 00144 { 00145 return segments.find(segment_name); 00146 }; 00152 SegmentMap::const_iterator getRootSegment()const 00153 { 00154 return segments.find(root_name); 00155 }; 00156 00168 bool getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const; 00169 00170 00171 const SegmentMap& getSegments()const 00172 { 00173 return segments; 00174 } 00175 00176 virtual ~Tree(){}; 00177 00178 }; 00179 } 00180 #endif 00181 00182 00183 00184 00185