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 #ifndef URDF_INTERFACE_MODEL_H
38 #define URDF_INTERFACE_MODEL_H
39 
40 #include <string>
41 #include <map>
42 #include <urdf_model/link.h>
43 #include <urdf_model/types.h>
44 #include <urdf_exception/exception.h>
45 
46 namespace urdf {
47 
48 class ModelInterface
49 {
50 public:
getRoot(void)51   LinkConstSharedPtr getRoot(void) const{return this->root_link_;};
getLink(const std::string & name)52   LinkConstSharedPtr getLink(const std::string& name) const
53   {
54     LinkConstSharedPtr ptr;
55     if (this->links_.find(name) == this->links_.end())
56       ptr.reset();
57     else
58       ptr = this->links_.find(name)->second;
59     return ptr;
60   };
61 
getJoint(const std::string & name)62   JointConstSharedPtr getJoint(const std::string& name) const
63   {
64     JointConstSharedPtr ptr;
65     if (this->joints_.find(name) == this->joints_.end())
66       ptr.reset();
67     else
68       ptr = this->joints_.find(name)->second;
69     return ptr;
70   };
71 
72 
getName()73   const std::string& getName() const {return name_;};
getLinks(std::vector<LinkSharedPtr> & links)74   void getLinks(std::vector<LinkSharedPtr >& links) const
75   {
76     for (std::map<std::string,LinkSharedPtr>::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
77     {
78       links.push_back(link->second);
79     }
80   };
81 
clear()82   void clear()
83   {
84     name_.clear();
85     this->links_.clear();
86     this->joints_.clear();
87     this->materials_.clear();
88     this->root_link_.reset();
89   };
90 
91   /// non-const getLink()
getLink(const std::string & name,LinkSharedPtr & link)92   void getLink(const std::string& name, LinkSharedPtr &link) const
93   {
94     LinkSharedPtr ptr;
95     if (this->links_.find(name) == this->links_.end())
96       ptr.reset();
97     else
98       ptr = this->links_.find(name)->second;
99     link = ptr;
100   };
101 
102   /// non-const getMaterial()
getMaterial(const std::string & name)103   MaterialSharedPtr getMaterial(const std::string& name) const
104   {
105     MaterialSharedPtr ptr;
106     if (this->materials_.find(name) == this->materials_.end())
107       ptr.reset();
108     else
109       ptr = this->materials_.find(name)->second;
110     return ptr;
111   };
112 
initTree(std::map<std::string,std::string> & parent_link_tree)113   void initTree(std::map<std::string, std::string> &parent_link_tree)
114   {
115     // loop through all joints, for every link, assign children links and children joints
116     for (std::map<std::string, JointSharedPtr>::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
117     {
118       std::string parent_link_name = joint->second->parent_link_name;
119       std::string child_link_name = joint->second->child_link_name;
120 
121       if (parent_link_name.empty() || child_link_name.empty())
122       {
123         throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
124       }
125       else
126       {
127         // find child and parent links
128         LinkSharedPtr child_link, parent_link;
129         this->getLink(child_link_name, child_link);
130         if (!child_link)
131         {
132           throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
133         }
134         this->getLink(parent_link_name, parent_link);
135         if (!parent_link)
136         {
137           throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found.  This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file.");
138         }
139 
140         //set parent link for child link
141         child_link->setParent(parent_link);
142 
143         //set parent joint for child link
144         child_link->parent_joint = joint->second;
145 
146         //set child joint for parent link
147         parent_link->child_joints.push_back(joint->second);
148 
149         //set child link for parent link
150         parent_link->child_links.push_back(child_link);
151 
152         // fill in child/parent string map
153         parent_link_tree[child_link->name] = parent_link_name;
154       }
155     }
156   }
157 
initRoot(const std::map<std::string,std::string> & parent_link_tree)158   void initRoot(const std::map<std::string, std::string> &parent_link_tree)
159   {
160     this->root_link_.reset();
161 
162     // find the links that have no parent in the tree
163     for (std::map<std::string, LinkSharedPtr>::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
164     {
165       std::map<std::string, std::string >::const_iterator parent = parent_link_tree.find(l->first);
166       if (parent == parent_link_tree.end())
167       {
168         // store root link
169         if (!this->root_link_)
170         {
171           getLink(l->first, this->root_link_);
172         }
173         // we already found a root link
174         else
175         {
176           throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
177         }
178       }
179     }
180     if (!this->root_link_)
181     {
182       throw ParseError("No root link found. The robot xml is not a valid tree.");
183     }
184   }
185 
186 
187   /// \brief complete list of Links
188   std::map<std::string, LinkSharedPtr> links_;
189   /// \brief complete list of Joints
190   std::map<std::string, JointSharedPtr> joints_;
191   /// \brief complete list of Materials
192   std::map<std::string, MaterialSharedPtr> materials_;
193 
194   /// \brief The name of the robot model
195   std::string name_;
196 
197   /// \brief The root is always a link (the parent of the tree describing the robot)
198   LinkSharedPtr root_link_;
199 
200 
201 
202 };
203 
204 }
205 
206 #endif
207