$search
00001 #include <boost/lexical_cast.hpp> 00002 #include <boost/shared_ptr.hpp> 00003 #include <boost/make_shared.hpp> 00004 00005 #include <tinyxml.h> 00006 00007 #include <ros/console.h> 00008 00009 #include "urdf_interface/link.h" 00010 #include "rcpdf_interface/contact.hh" 00011 #include "rcpdf_interface/model.hh" 00012 #include "rcpdf/parser.hh" 00013 00014 namespace rcpdf 00015 { 00016 using rcpdf_interface::Geometry; 00017 using urdf::Sphere; 00018 using urdf::Box; 00019 using urdf::Cylinder; 00020 using urdf::Mesh; 00021 00022 namespace 00023 { 00024 boost::shared_ptr<Geometry> parseGeometry(TiXmlElement *g) 00025 { 00026 boost::shared_ptr<Geometry> geom; 00027 if (!g) return geom; 00028 00029 TiXmlElement *shape = g->FirstChildElement(); 00030 if (!shape) 00031 { 00032 ROS_ERROR("Geometry tag contains no child element."); 00033 return geom; 00034 } 00035 00036 std::string type_name = shape->ValueStr(); 00037 if (type_name == "sphere") 00038 geom.reset(new Sphere); 00039 else if (type_name == "box") 00040 geom.reset(new Box); 00041 else if (type_name == "cylinder") 00042 geom.reset(new Cylinder); 00043 else if (type_name == "mesh") 00044 geom.reset(new Mesh); 00045 else 00046 { 00047 ROS_ERROR("Unknown geometry type '%s'", type_name.c_str()); 00048 return geom; 00049 } 00050 00051 // clear geom object when fails to initialize 00052 if (!geom->initXml(shape)){ 00053 ROS_ERROR("Geometry failed to parse"); 00054 geom.reset(); 00055 } 00056 00057 return geom; 00058 } 00059 00060 00061 boost::shared_ptr<rcpdf_interface::Limit> 00062 parseLimit(TiXmlElement* limit_element) 00063 { 00064 boost::shared_ptr<rcpdf_interface::Limit> limit = 00065 boost::make_shared<rcpdf_interface::Limit>(); 00066 00067 // Get maximum normal force. 00068 const char* normal_force = limit_element->Attribute("normal_force"); 00069 limit->normal_force_ = boost::lexical_cast<double>(normal_force); 00070 return limit; 00071 } 00072 00073 boost::shared_ptr<rcpdf_interface::Contact> 00074 parseContact(TiXmlElement* contact_xml) 00075 { 00076 boost::shared_ptr<rcpdf_interface::Contact> contact = 00077 boost::make_shared<rcpdf_interface::Contact>(); 00078 00079 // Get contact name 00080 const char *name = contact_xml->Attribute("name"); 00081 if (!name) 00082 { 00083 ROS_ERROR("No name given for the contact."); 00084 contact.reset(); 00085 return contact; 00086 } 00087 contact->name_ = std::string(name); 00088 00089 // Get associated link 00090 const char *link = contact_xml->Attribute("link"); 00091 if (!link) 00092 { 00093 ROS_ERROR("No associated link given for the contact."); 00094 contact.reset(); 00095 return contact; 00096 } 00097 contact->link_ = std::string(link); 00098 00099 // Parse origin. 00100 TiXmlElement* origin_xml = contact_xml->FirstChildElement("origin"); 00101 if (!origin_xml) 00102 { 00103 ROS_ERROR("No origin for the contact."); 00104 contact.reset(); 00105 return contact; 00106 } 00107 if (!contact->origin_.initXml(origin_xml)) 00108 { 00109 ROS_ERROR("Contact has a malformed origin tag"); 00110 contact.reset(); 00111 return contact; 00112 } 00113 00114 // Parse geometry. 00115 TiXmlElement* geometry_xml = contact_xml->FirstChildElement("geometry"); 00116 if (!geometry_xml) 00117 { 00118 ROS_ERROR("No geometry for the contact."); 00119 contact.reset(); 00120 return contact; 00121 } 00122 contact->geometry_ = parseGeometry(geometry_xml); 00123 00124 // Parse limit. 00125 TiXmlElement* limit_xml = contact_xml->FirstChildElement("limit"); 00126 if (limit_xml) 00127 contact->limit_ = parseLimit(limit_xml); 00128 00129 return contact; 00130 } 00131 } // end of anonymous namespace. 00132 00133 boost::shared_ptr<rcpdf_interface::ModelInterface> 00134 parseRCPDF(const std::string& xml_string) 00135 { 00136 boost::shared_ptr<rcpdf_interface::ModelInterface> model = 00137 boost::make_shared<rcpdf_interface::ModelInterface>(); 00138 00139 TiXmlDocument xml_doc; 00140 xml_doc.Parse(xml_string.c_str()); 00141 00142 TiXmlElement* robot_xml = xml_doc.FirstChildElement("robot"); 00143 if (!robot_xml) 00144 { 00145 ROS_ERROR("Could not find the 'robot' element in the xml file"); 00146 model.reset(); 00147 return model; 00148 } 00149 00150 // Get robot name 00151 const char *name = robot_xml->Attribute("name"); 00152 if (!name) 00153 { 00154 ROS_ERROR("No name given for the robot."); 00155 model.reset(); 00156 return model; 00157 } 00158 model->name_ = std::string(name); 00159 00160 // Get all contact elements 00161 for (TiXmlElement* contact_xml = robot_xml->FirstChildElement("contact"); 00162 contact_xml; 00163 contact_xml = contact_xml->NextSiblingElement("contact")) 00164 { 00165 boost::shared_ptr<rcpdf_interface::Contact> contact = 00166 parseContact(contact_xml); 00167 if (contact) 00168 model->contacts_.push_back(contact); 00169 } 00170 return model; 00171 } 00172 } // end of namespace rcpdf.