$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Wim Meeussen */ 00036 00037 #include <boost/algorithm/string.hpp> 00038 #include <ros/console.h> 00039 #include <vector> 00040 #include "urdf_parser/urdf_parser.h" 00041 00042 namespace urdf{ 00043 00044 00045 00046 boost::shared_ptr<ModelInterface> parseURDF(const std::string &xml_string) 00047 { 00048 boost::shared_ptr<ModelInterface> model(new ModelInterface); 00049 model->clear(); 00050 00051 TiXmlDocument xml_doc; 00052 xml_doc.Parse(xml_string.c_str()); 00053 00054 TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot"); 00055 if (!robot_xml) 00056 { 00057 ROS_ERROR("Could not find the 'robot' element in the xml file"); 00058 model.reset(); 00059 return model; 00060 } 00061 00062 // Get robot name 00063 const char *name = robot_xml->Attribute("name"); 00064 if (!name) 00065 { 00066 ROS_ERROR("No name given for the robot."); 00067 model.reset(); 00068 return model; 00069 } 00070 model->name_ = std::string(name); 00071 00072 // Get all Material elements 00073 for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) 00074 { 00075 boost::shared_ptr<Material> material; 00076 material.reset(new Material); 00077 00078 if (material->initXml(material_xml)) 00079 { 00080 if (model->getMaterial(material->name)) 00081 { 00082 ROS_ERROR("material '%s' is not unique.", material->name.c_str()); 00083 material.reset(); 00084 model.reset(); 00085 return model; 00086 } 00087 else 00088 { 00089 model->materials_.insert(make_pair(material->name,material)); 00090 ROS_DEBUG("successfully added a new material '%s'", material->name.c_str()); 00091 } 00092 } 00093 else 00094 { 00095 ROS_ERROR("material xml is not initialized correctly"); 00096 material.reset(); 00097 model.reset(); 00098 return model; 00099 } 00100 } 00101 00102 // Get all Link elements 00103 for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) 00104 { 00105 boost::shared_ptr<Link> link; 00106 link.reset(new Link); 00107 00108 if (link->initXml(link_xml)) 00109 { 00110 if (model->getLink(link->name)) 00111 { 00112 ROS_ERROR("link '%s' is not unique.", link->name.c_str()); 00113 model.reset(); 00114 return model; 00115 } 00116 else 00117 { 00118 // set link visual material 00119 ROS_DEBUG("setting link '%s' material", link->name.c_str()); 00120 if (link->visual) 00121 { 00122 if (!link->visual->material_name.empty()) 00123 { 00124 if (model->getMaterial(link->visual->material_name)) 00125 { 00126 ROS_DEBUG("setting link '%s' material to '%s'", link->name.c_str(),link->visual->material_name.c_str()); 00127 link->visual->material = model->getMaterial( link->visual->material_name.c_str() ); 00128 } 00129 else 00130 { 00131 if (link->visual->material) 00132 { 00133 ROS_DEBUG("link '%s' material '%s' defined in Visual.", link->name.c_str(),link->visual->material_name.c_str()); 00134 model->materials_.insert(make_pair(link->visual->material->name,link->visual->material)); 00135 } 00136 else 00137 { 00138 ROS_ERROR("link '%s' material '%s' undefined.", link->name.c_str(),link->visual->material_name.c_str()); 00139 model.reset(); 00140 return model; 00141 } 00142 } 00143 } 00144 } 00145 00146 model->links_.insert(make_pair(link->name,link)); 00147 ROS_DEBUG("successfully added a new link '%s'", link->name.c_str()); 00148 } 00149 } 00150 else 00151 { 00152 ROS_ERROR("link xml is not initialized correctly"); 00153 model.reset(); 00154 return model; 00155 } 00156 } 00157 if (model->links_.empty()){ 00158 ROS_ERROR("No link elements found in urdf file"); 00159 model.reset(); 00160 return model; 00161 } 00162 00163 // Get all Joint elements 00164 for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint")) 00165 { 00166 boost::shared_ptr<Joint> joint; 00167 joint.reset(new Joint); 00168 00169 if (joint->initXml(joint_xml)) 00170 { 00171 if (model->getJoint(joint->name)) 00172 { 00173 ROS_ERROR("joint '%s' is not unique.", joint->name.c_str()); 00174 model.reset(); 00175 return model; 00176 } 00177 else 00178 { 00179 model->joints_.insert(make_pair(joint->name,joint)); 00180 ROS_DEBUG("successfully added a new joint '%s'", joint->name.c_str()); 00181 } 00182 } 00183 else 00184 { 00185 ROS_ERROR("joint xml is not initialized correctly"); 00186 model.reset(); 00187 return model; 00188 } 00189 } 00190 00191 00192 // every link has children links and joints, but no parents, so we create a 00193 // local convenience data structure for keeping child->parent relations 00194 std::map<std::string, std::string> parent_link_tree; 00195 parent_link_tree.clear(); 00196 00197 // building tree: name mapping 00198 if (!model->initTree(parent_link_tree)) 00199 { 00200 ROS_ERROR("failed to build tree"); 00201 model.reset(); 00202 return model; 00203 } 00204 00205 // find the root link 00206 if (!model->initRoot(parent_link_tree)) 00207 { 00208 ROS_ERROR("failed to find root link"); 00209 model.reset(); 00210 return model; 00211 } 00212 00213 return model; 00214 } 00215 00216 } 00217