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
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "urdf/model.h"
00038
00039 #include <ros/ros.h>
00040
00041
00042
00043 #include <urdf_parser/urdf_parser.h>
00044 #include <urdf_parser_plugin/parser.h>
00045 #include <pluginlib/class_loader.h>
00046
00047 #include <boost/algorithm/string.hpp>
00048 #include <boost/scoped_ptr.hpp>
00049 #include <boost/thread.hpp>
00050
00051 #include <vector>
00052 #include <fstream>
00053 #include <iostream>
00054
00055 namespace urdf{
00056
00057 static bool IsColladaData(const std::string& data)
00058 {
00059 return data.find("<COLLADA") != std::string::npos;
00060 }
00061
00062
00063 bool Model::initFile(const std::string& filename)
00064 {
00065
00066
00067 std::string xml_string;
00068 std::fstream xml_file(filename.c_str(), std::fstream::in);
00069 if (xml_file.is_open())
00070 {
00071 while ( xml_file.good() )
00072 {
00073 std::string line;
00074 std::getline( xml_file, line);
00075 xml_string += (line + "\n");
00076 }
00077 xml_file.close();
00078 return Model::initString(xml_string);
00079 }
00080 else
00081 {
00082 ROS_ERROR("Could not open file [%s] for parsing.",filename.c_str());
00083 return false;
00084 }
00085
00086 }
00087
00088
00089 bool Model::initParam(const std::string& param)
00090 {
00091 return initParamWithNodeHandle(param, ros::NodeHandle());
00092 }
00093
00094 bool Model::initParamWithNodeHandle(const std::string& param, const ros::NodeHandle& nh)
00095 {
00096 std::string xml_string;
00097
00098
00099 std::string full_param;
00100 if (!nh.searchParam(param, full_param)){
00101 ROS_ERROR("Could not find parameter %s on parameter server", param.c_str());
00102 return false;
00103 }
00104
00105
00106 if (!nh.getParam(full_param, xml_string)){
00107 ROS_ERROR("Could not read parameter %s on parameter server", full_param.c_str());
00108 return false;
00109 }
00110 return Model::initString(xml_string);
00111 }
00112
00113 bool Model::initXml(TiXmlDocument *xml_doc)
00114 {
00115 if (!xml_doc)
00116 {
00117 ROS_ERROR("Could not parse the xml document");
00118 return false;
00119 }
00120
00121 std::stringstream ss;
00122 ss << *xml_doc;
00123
00124 return Model::initString(ss.str());
00125 }
00126
00127 bool Model::initXml(TiXmlElement *robot_xml)
00128 {
00129 if (!robot_xml)
00130 {
00131 ROS_ERROR("Could not parse the xml element");
00132 return false;
00133 }
00134
00135 std::stringstream ss;
00136 ss << (*robot_xml);
00137
00138 return Model::initString(ss.str());
00139 }
00140
00141 bool Model::initString(const std::string& xml_string)
00142 {
00143 urdf::ModelInterfaceSharedPtr model;
00144
00145
00146 if( IsColladaData(xml_string) ) {
00147 ROS_DEBUG("Parsing robot collada xml string");
00148
00149 static boost::mutex PARSER_PLUGIN_LOCK;
00150 static boost::scoped_ptr<pluginlib::ClassLoader<urdf::URDFParser> > PARSER_PLUGIN_LOADER;
00151 boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK);
00152
00153 try
00154 {
00155 if (!PARSER_PLUGIN_LOADER)
00156 PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader<urdf::URDFParser>("urdf_parser_plugin", "urdf::URDFParser"));
00157 const std::vector<std::string> &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses();
00158 bool found = false;
00159 for (std::size_t i = 0 ; i < classes.size() ; ++i)
00160 if (classes[i].find("urdf/ColladaURDFParser") != std::string::npos)
00161 {
00162 boost::shared_ptr<urdf::URDFParser> instance = PARSER_PLUGIN_LOADER->createInstance(classes[i]);
00163 if (instance)
00164 model = instance->parse(xml_string);
00165 found = true;
00166 break;
00167 }
00168 if (!found)
00169 ROS_ERROR_STREAM("No URDF parser plugin found for Collada files. Did you install the corresponding package?");
00170 }
00171 catch(pluginlib::PluginlibException& ex)
00172 {
00173 ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what() << ". Will not parse Collada file.");
00174 }
00175 }
00176 else {
00177 ROS_DEBUG("Parsing robot urdf xml string");
00178 model = parseURDF(xml_string);
00179 }
00180
00181
00182 if (model){
00183 this->links_ = model->links_;
00184 this->joints_ = model->joints_;
00185 this->materials_ = model->materials_;
00186 this->name_ = model->name_;
00187 this->root_link_ = model->root_link_;
00188 return true;
00189 }
00190 return false;
00191 }
00192
00193
00194
00195 }