37 #ifndef URDF_INTERFACE_MODEL_H 38 #define URDF_INTERFACE_MODEL_H 42 #include "urdf_link.h" 43 #include "urdf_exception.h" 57 std::shared_ptr<const Link>
getRoot(
void)
const{
64 std::shared_ptr<const Link>
getLink(
const std::string& name)
const {
65 std::shared_ptr<const Link> ptr;
69 ptr = this->
links_.find(name)->second;
76 std::shared_ptr<const Joint>
getJoint(
const std::string& name)
const {
77 std::shared_ptr<const Joint> ptr;
81 ptr = this->
joints_.find(name)->second;
95 void getLinks(std::vector<std::shared_ptr<Link> >& links)
const {
96 for (std::map<std::string,std::shared_ptr<Link> >::const_iterator link = this->
links_.begin();
97 link != this->
links_.end(); link++)
99 links.push_back(link->second);
118 void getLink(
const std::string& name,std::shared_ptr<Link> &link)
const {
119 std::shared_ptr<Link> ptr;
123 ptr = this->
links_.find(name)->second;
131 std::shared_ptr<Material>
getMaterial(
const std::string& name)
const {
132 std::shared_ptr<Material> ptr;
144 void initTree(std::map<std::string, std::string> &parent_link_tree) {
146 for (std::map<std::string,std::shared_ptr<Joint> >::iterator joint = this->
joints_.begin();
147 joint != this->
joints_.end(); joint++) {
148 std::string parent_link_name = joint->second->parent_link_name;
149 std::string child_link_name = joint->second->child_link_name;
151 if (parent_link_name.empty() || child_link_name.empty())
153 throw ParseError(
"Joint [" + joint->second->name +
"] is missing a parent and/or child link specification.");
158 std::shared_ptr<Link> child_link, parent_link;
159 this->
getLink(child_link_name, child_link);
162 throw ParseError(
"child link [" + child_link_name +
"] of joint [" + joint->first +
"] not found");
164 this->
getLink(parent_link_name, parent_link);
167 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.");
171 child_link->setParent(parent_link);
174 child_link->parent_joint = joint->second;
177 parent_link->child_joints.push_back(joint->second);
180 parent_link->child_links.push_back(child_link);
183 parent_link_tree[child_link->name] = parent_link_name;
191 void initRoot(
const std::map<std::string, std::string> &parent_link_tree) {
195 for (std::map<std::string, std::shared_ptr<Link> >::const_iterator l=this->
links_.begin(); l!=this->
links_.end(); l++)
197 std::map<std::string, std::string >::const_iterator parent = parent_link_tree.find(l->first);
198 if (parent == parent_link_tree.end())
203 getLink(l->first, this->root_link_);
208 throw ParseError(
"Two root links found: [" + this->
root_link_->name +
"] and [" + l->first +
"]");
214 throw ParseError(
"No root link found. The robot xml is not a valid tree.");
220 std::map<std::string, std::shared_ptr<Link> >
links_;
222 std::map<std::string, std::shared_ptr<Joint> >
joints_;
224 std::map<std::string, std::shared_ptr<Material> >
materials_;
Definition: urdf_model.h:50
std::shared_ptr< Link > root_link_
Definition: urdf_model.h:232
std::shared_ptr< const Link > getLink(const std::string &name) const
Definition: urdf_model.h:64
std::map< std::string, std::shared_ptr< Material > > materials_
complete list of Materials
Definition: urdf_model.h:224
void clear()
Definition: urdf_model.h:106
std::shared_ptr< const Joint > getJoint(const std::string &name) const
Definition: urdf_model.h:76
Definition: urdf_exception.h:11
void getLink(const std::string &name, std::shared_ptr< Link > &link) const
non-const getLink()
Definition: urdf_model.h:118
void getLinks(std::vector< std::shared_ptr< Link > > &links) const
Definition: urdf_model.h:95
std::shared_ptr< Material > getMaterial(const std::string &name) const
non-const getMaterial()
Definition: urdf_model.h:131
Definition: urdf_color.h:46
std::shared_ptr< const Link > getRoot(void) const
Definition: urdf_model.h:57
std::map< std::string, std::shared_ptr< Joint > > joints_
complete list of Joints
Definition: urdf_model.h:222
void initTree(std::map< std::string, std::string > &parent_link_tree)
Loop through all joints, for every link, assign children links and children joints.
Definition: urdf_model.h:144
std::map< std::string, std::shared_ptr< Link > > links_
complete list of Links
Definition: urdf_model.h:220
const std::string & getName() const
Definition: urdf_model.h:88
void initRoot(const std::map< std::string, std::string > &parent_link_tree)
Definition: urdf_model.h:191