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 ros::NodeHandle nh;
00092 std::string xml_string;
00093
00094
00095 std::string full_param;
00096 if (!nh.searchParam(param, full_param)){
00097 ROS_ERROR("Could not find parameter %s on parameter server", param.c_str());
00098 return false;
00099 }
00100
00101
00102 if (!nh.getParam(full_param, xml_string)){
00103 ROS_ERROR("Could not read parameter %s on parameter server", full_param.c_str());
00104 return false;
00105 }
00106 return Model::initString(xml_string);
00107 }
00108
00109 bool Model::initXml(TiXmlDocument *xml_doc)
00110 {
00111 if (!xml_doc)
00112 {
00113 ROS_ERROR("Could not parse the xml document");
00114 return false;
00115 }
00116
00117 std::stringstream ss;
00118 ss << *xml_doc;
00119
00120 return Model::initString(ss.str());
00121 }
00122
00123 bool Model::initXml(TiXmlElement *robot_xml)
00124 {
00125 if (!robot_xml)
00126 {
00127 ROS_ERROR("Could not parse the xml element");
00128 return false;
00129 }
00130
00131 std::stringstream ss;
00132 ss << (*robot_xml);
00133
00134 return Model::initString(ss.str());
00135 }
00136
00137 bool Model::initString(const std::string& xml_string)
00138 {
00139 boost::shared_ptr<ModelInterface> model;
00140
00141
00142 if( IsColladaData(xml_string) ) {
00143 ROS_DEBUG("Parsing robot collada xml string");
00144
00145 static boost::mutex PARSER_PLUGIN_LOCK;
00146 static boost::scoped_ptr<pluginlib::ClassLoader<urdf::URDFParser> > PARSER_PLUGIN_LOADER;
00147 boost::mutex::scoped_lock _(PARSER_PLUGIN_LOCK);
00148
00149 try
00150 {
00151 if (!PARSER_PLUGIN_LOADER)
00152 PARSER_PLUGIN_LOADER.reset(new pluginlib::ClassLoader<urdf::URDFParser>("urdf_parser_plugin", "urdf::URDFParser"));
00153 const std::vector<std::string> &classes = PARSER_PLUGIN_LOADER->getDeclaredClasses();
00154 bool found = false;
00155 for (std::size_t i = 0 ; i < classes.size() ; ++i)
00156 if (classes[i].find("urdf/ColladaURDFParser") != std::string::npos)
00157 {
00158 boost::shared_ptr<urdf::URDFParser> instance = PARSER_PLUGIN_LOADER->createInstance(classes[i]);
00159 if (instance)
00160 model = instance->parse(xml_string);
00161 found = true;
00162 break;
00163 }
00164 if (!found)
00165 ROS_ERROR_STREAM("No URDF parser plugin found for Collada files. Did you install the corresponding package?");
00166 }
00167 catch(pluginlib::PluginlibException& ex)
00168 {
00169 ROS_ERROR_STREAM("Exception while creating planning plugin loader " << ex.what() << ". Will not parse Collada file.");
00170 }
00171 }
00172 else {
00173 ROS_DEBUG("Parsing robot urdf xml string");
00174 model = parseURDF(xml_string);
00175 }
00176
00177
00178 if (model){
00179 this->links_ = model->links_;
00180 this->joints_ = model->joints_;
00181 this->materials_ = model->materials_;
00182 this->name_ = model->name_;
00183 this->root_link_ = model->root_link_;
00184 return true;
00185 }
00186 else
00187 return false;
00188 }
00189
00190
00191
00192 }