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 #ifndef KDL_TREE_HPP
00023 #define KDL_TREE_HPP
00024
00025 #include "config.h"
00026
00027 #include "segment.hpp"
00028 #include "chain.hpp"
00029
00030 #include <string>
00031 #include <map>
00032
00033 #ifdef KDL_USE_NEW_TREE_INTERFACE
00034 #include <boost/shared_ptr.hpp>
00035 #endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
00036
00037 namespace KDL
00038 {
00039 class TreeElement;
00040
00041 #ifdef KDL_USE_NEW_TREE_INTERFACE
00042
00043
00044 typedef boost::shared_ptr<TreeElement> TreeElementPtr;
00045 typedef boost::shared_ptr<const TreeElement> TreeElementConstPtr;
00046 typedef std::map<std::string, TreeElementPtr> SegmentMap;
00047 typedef TreeElementPtr TreeElementType;
00048
00049 #define GetTreeElementChildren(tree_element) (tree_element)->children
00050 #define GetTreeElementParent(tree_element) (tree_element)->parent
00051 #define GetTreeElementQNr(tree_element) (tree_element)->q_nr
00052 #define GetTreeElementSegment(tree_element) (tree_element)->segment
00053
00054 #else //#ifdef KDL_USE_NEW_TREE_INTERFACE
00055
00056 typedef std::map<std::string,TreeElement> SegmentMap;
00057 typedef TreeElement TreeElementType;
00058
00059 #define GetTreeElementChildren(tree_element) (tree_element).children
00060 #define GetTreeElementParent(tree_element) (tree_element).parent
00061 #define GetTreeElementQNr(tree_element) (tree_element).q_nr
00062 #define GetTreeElementSegment(tree_element) (tree_element).segment
00063
00064 #endif //#ifdef KDL_USE_NEW_TREE_INTERFACE
00065
00066 class TreeElement
00067 {
00068 public:
00069 TreeElement(const Segment& segment_in,const SegmentMap::const_iterator& parent_in,unsigned int q_nr_in):
00070 segment(segment_in),
00071 q_nr(q_nr_in),
00072 parent(parent_in)
00073 {}
00074
00075 static TreeElementType Root(const std::string& root_name)
00076 {
00077 #ifdef KDL_USE_NEW_TREE_INTERFACE
00078 return TreeElementType(new TreeElement(root_name));
00079 #else //#define KDL_USE_NEW_TREE_INTERFACE
00080 return TreeElementType(root_name);
00081 #endif
00082 }
00083
00084 Segment segment;
00085 unsigned int q_nr;
00086 SegmentMap::const_iterator parent;
00087 std::vector<SegmentMap::const_iterator > children;
00088
00089 private:
00090 TreeElement(const std::string& name):segment(name), q_nr(0) {}
00091 };
00092
00099 class Tree
00100 {
00101 private:
00102 SegmentMap segments;
00103 unsigned int nrOfJoints;
00104 unsigned int nrOfSegments;
00105
00106 std::string root_name;
00107
00108 bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name);
00109
00110 public:
00114 explicit Tree(const std::string& root_name="root");
00115 Tree(const Tree& in);
00116 Tree& operator= (const Tree& arg);
00117
00128 bool addSegment(const Segment& segment, const std::string& hook_name);
00129
00138 bool addChain(const Chain& chain, const std::string& hook_name);
00139
00149 bool addTree(const Tree& tree, const std::string& hook_name);
00150
00159 unsigned int getNrOfJoints()const
00160 {
00161 return nrOfJoints;
00162 };
00163
00168 unsigned int getNrOfSegments()const {return nrOfSegments;};
00169
00177 SegmentMap::const_iterator getSegment(const std::string& segment_name)const
00178 {
00179 return segments.find(segment_name);
00180 };
00186 SegmentMap::const_iterator getRootSegment()const
00187 {
00188 return segments.find(root_name);
00189 };
00190
00202 bool getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const;
00203
00204
00205 const SegmentMap& getSegments()const
00206 {
00207 return segments;
00208 }
00209
00210 virtual ~Tree(){};
00211
00212 };
00213 }
00214 #endif
00215
00216
00217
00218
00219