00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include <mtconnect_task_parser/task_parser.h>
00018 #include <mtconnect_task_parser/exception.h>
00019
00020 #include <boost/function.hpp>
00021 #include <boost/lexical_cast.hpp>
00022 #include <boost/algorithm/string.hpp>
00023 #include "boost/make_shared.hpp"
00024 #include <ros/console.h>
00025
00026 namespace mtconnect
00027 {
00028
00029 bool evalXmlParse(bool parse_rtn, bool required, std::string name)
00030 {
00031 bool rtn = false;
00032
00033 if (parse_rtn)
00034 {
00035 rtn = true;
00036 }
00037 else
00038 {
00039 if (required)
00040 {
00041 ROS_ERROR_STREAM("Failed to parse REQUIRED attribute: " << name);
00042 rtn = false;
00043 }
00044 else
00045 {
00046 ROS_DEBUG_STREAM("Failed to parse OPTIONAL attribute: " << name);
00047 rtn = true;
00048 }
00049 }
00050 return rtn;
00051 }
00052
00053 template<typename T>
00054 bool attrFromXml(TiXmlElement* config, std::string name, T & member, bool required = true)
00055 {
00056 ROS_DEBUG_STREAM("XML element: " << config->Value());
00057
00058 int txml_code = config->QueryValueAttribute(name, &member);
00059 bool txml_bool = false;
00060 switch (txml_code)
00061 {
00062 case TIXML_SUCCESS:
00063 ROS_DEBUG_STREAM(
00064 "Element: " << config->Value() << " successfully parsed attribute: " << name << " to " << member);
00065 txml_bool = true;
00066 break;
00067 case TIXML_WRONG_TYPE:
00068 ROS_DEBUG_STREAM("Element: " << config->Value() << " failed to parse attribute: " << name << " , wrong type");
00069 txml_bool = false;
00070 break;
00071 case TIXML_NO_ATTRIBUTE:
00072 ROS_DEBUG_STREAM(
00073 "Element: " << config->Value() << " failed to parse attribute: " << name << ", attribute missing");
00074 txml_bool = false;
00075 break;
00076 default:
00077 ROS_WARN_STREAM("Element: " << config->Value() << " unexpected return code from tiny xml: " << txml_code);
00078 txml_bool = false;
00079 break;
00080 }
00081
00082 return evalXmlParse(txml_bool, required, name);
00083
00084 }
00085
00086 template<typename T>
00087 bool attrFromXml(TiXmlElement* config, std::string name, std::vector<T> & list, bool required = true)
00088 {
00089 bool txml_rtn = false;
00090
00091 const char *attr_list = config->Attribute(name.c_str());
00092
00093 if (attr_list)
00094 {
00095 std::vector<std::string> pieces;
00096 boost::split(pieces, attr_list, boost::is_any_of(" "));
00097 for (unsigned int i = 0; i < pieces.size(); ++i)
00098 {
00099 if (pieces[i] != "")
00100 {
00101 try
00102 {
00103 ROS_DEBUG_STREAM(
00104 "Element: " << config->Value() << " appending item: " << pieces[i] << " to " << name << " list");
00105 list.push_back(boost::lexical_cast<T>(pieces[i].c_str()));
00106 txml_rtn = true;
00107 }
00108 catch (boost::bad_lexical_cast &e)
00109 {
00110 ROS_ERROR_STREAM(
00111 "Element: " << config->Value() << " attribute list: " << name << ", item: " << pieces[i] << ") could not be recast");
00112 txml_rtn = false;
00113 break;
00114 }
00115 }
00116 }
00117
00118 }
00119
00120 return evalXmlParse(txml_rtn, required, name);
00121
00122 }
00123
00124 bool fromXml(MotionGroup & motion_group, TiXmlElement* config)
00125 {
00126
00127 ROS_DEBUG_STREAM("MotionGroup::from XML element");
00128
00129 motion_group.clear();
00130
00131 bool rtn = attrFromXml(config, "name", motion_group.name_)
00132 && attrFromXml(config, "joint_names", motion_group.joint_names_);
00133
00134 if (!rtn)
00135 {
00136 ROS_ERROR("Failed to parse MotionGroup");
00137 motion_group.clear();
00138 }
00139 return rtn;
00140
00141 }
00142
00143 bool fromXml(JointPoint & joint_point, TiXmlElement* config,
00144 const std::map<std::string, boost::shared_ptr<MotionGroup> >& motion_groups)
00145 {
00146 ROS_DEBUG_STREAM("JointPoint::from XML element");
00147 joint_point.clear();
00148
00149 bool rtn = attrFromXml(config, "name", joint_point.name_, false)
00150 && attrFromXml(config, "joint_values", joint_point.values_)
00151 && attrFromXml(config, "percent_velocity", joint_point.percent_velocity_, false);
00152
00153 if (rtn)
00154 {
00155 std::string group_name;
00156 if (attrFromXml(config, "group_name", group_name))
00157 {
00158 try
00159 {
00160 joint_point.group_ = motion_groups.at(group_name);
00161 unsigned int group_size = joint_point.group_->joint_names_.size();
00162 if (group_size != joint_point.values_.size())
00163 {
00164 ROS_ERROR_STREAM(
00165 "Joint group size: " << group_size << " doesn't match values size: " << joint_point.values_.size());
00166 rtn = false;
00167 }
00168 else
00169 {
00170 rtn = true;
00171 }
00172 }
00173 catch (const std::out_of_range& e)
00174 {
00175 ROS_ERROR_STREAM("Group name not valid: " << e.what());
00176 rtn = false;
00177 }
00178 }
00179 else
00180 {
00181 rtn = false;
00182 }
00183 }
00184
00185 if (!rtn)
00186 {
00187 ROS_ERROR("Failed to parse JointPoint");
00188 joint_point.clear();
00189 }
00190 return rtn;
00191 }
00192
00193 bool fromXml(JointMove & joint_move, TiXmlElement* config,
00194 const std::map<std::string, boost::shared_ptr<MotionGroup> >& motion_groups)
00195 {
00196 bool rtn = false;
00197
00198 ROS_DEBUG_STREAM("JointMove::from XML element");
00199 joint_move.clear();
00200 joint_move.point_ = boost::make_shared<JointPoint>();
00201
00202 if (attrFromXml(config, "name", joint_move.name_, false))
00203 {
00204
00205 TiXmlElement* joint_point_xml = config->FirstChildElement("joint_point");
00206 if (joint_point_xml)
00207 {
00208 if (fromXml(*joint_move.point_, joint_point_xml, motion_groups))
00209 {
00210 rtn = true;
00211 }
00212 }
00213 else
00214 {
00215 ROS_ERROR_STREAM("Failed to parse joint point element from JointMove");
00216 }
00217 }
00218
00219 if (!rtn)
00220 {
00221 ROS_ERROR("Failed to parse JointMove");
00222 joint_move.clear();
00223 }
00224 return rtn;
00225 }
00226
00227 bool fromXml(Path & path, TiXmlElement* config,
00228 const std::map<std::string, boost::shared_ptr<MotionGroup> >& motion_groups)
00229 {
00230 ROS_DEBUG_STREAM("Path::from XML element");
00231
00232 path.clear();
00233
00234 bool rtn = false;
00235 if (attrFromXml(config, "name", path.name_))
00236 {
00237 for (TiXmlElement* joint_move_xml = config->FirstChildElement("joint_move"); joint_move_xml; joint_move_xml =
00238 joint_move_xml->NextSiblingElement("joint_move"))
00239 {
00240 JointMove move;
00241 move.clear();
00242 if (fromXml(move, joint_move_xml, motion_groups))
00243 {
00244 path.moves_.push_back(move);
00245 ROS_DEBUG_STREAM("Adding move to path, total size: " << path.moves_.size());
00246 rtn = true;
00247 }
00248 else
00249 {
00250 ROS_ERROR_STREAM("Failed to parse move element of path");
00251 rtn = false;
00252 break;
00253 }
00254 }
00255 }
00256
00257 if (!rtn)
00258 {
00259 ROS_ERROR("Failed to parse Path");
00260 path.clear();
00261 }
00262 return rtn;
00263
00264 }
00265
00266 bool fromXml(Task & task, TiXmlElement* config)
00267 {
00268 ROS_DEBUG_STREAM("Task::from XML element");
00269
00270 task.clear();
00271
00272 bool rtn = false;
00273
00274
00275 for (TiXmlElement* motion_group_xml = config->FirstChildElement("motion_group"); motion_group_xml; motion_group_xml =
00276 motion_group_xml->NextSiblingElement("motion_group"))
00277 {
00278 MotionGroup motion_group;
00279 motion_group.clear();
00280 if (fromXml(motion_group, motion_group_xml))
00281 {
00282 task.motion_groups_[motion_group.name_] = boost::make_shared<MotionGroup>(motion_group);
00283 ROS_DEBUG_STREAM("Adding motion group to task, total size: " << task.motion_groups_.size());
00284 }
00285 else
00286 {
00287
00288
00289 ROS_WARN_STREAM("Failed to parse motion group element of task, ignoring, will fail later if group is needed");
00290 }
00291 }
00292
00293
00294 for (TiXmlElement* joint_point_xml = config->FirstChildElement("joint_point"); joint_point_xml; joint_point_xml =
00295 joint_point_xml->NextSiblingElement("joint_point"))
00296 {
00297 JointPoint joint_point;
00298 joint_point.clear();
00299 if (fromXml(joint_point, joint_point_xml, task.motion_groups_))
00300 {
00301 if (joint_point.name_.empty())
00302 {
00303
00304
00305 ROS_WARN_STREAM("Failed to add joint point to task level, task level points must be named");
00306 }
00307 else
00308 {
00309 task.points_[joint_point.name_] = boost::make_shared<JointPoint>(joint_point);
00310 ROS_DEBUG_STREAM("Adding named joint point to task, total size: " << task.points_.size());
00311 }
00312 }
00313 else
00314 {
00315
00316
00317 ROS_WARN_STREAM("Failed to parse motion group element of task, ignoring, will fail later if group is needed");
00318 }
00319 }
00320
00321
00322 for (TiXmlElement* path_xml = config->FirstChildElement("path"); path_xml;
00323 path_xml = path_xml->NextSiblingElement("path"))
00324 {
00325 Path path;
00326 path.clear();
00327 if (fromXml(path, path_xml, task.motion_groups_))
00328 {
00329 task.paths_[path.name_] = boost::make_shared<Path>(path);
00330 ROS_DEBUG_STREAM("Adding path to task, total size: " << task.paths_.size());
00331 rtn = true;
00332 }
00333 else
00334 {
00335
00336 ROS_WARN_STREAM("Failed to parse motion group element of task, ignoring, will fail later if group is needed");
00337 rtn = false;
00338 break;
00339 }
00340 }
00341
00342 if (!rtn)
00343 {
00344 ROS_ERROR("Failed to parse Task");
00345 task.clear();
00346 }
00347 return rtn;
00348
00349 }
00350
00351
00352 bool fromXml(Task & task, const std::string & xml)
00353 {
00354 TiXmlDocument xml_doc;
00355 xml_doc.Parse(xml.c_str());
00356 TiXmlElement *xml_t = xml_doc.FirstChildElement("task");
00357 return fromXml(task, xml_t);
00358 }
00359
00360 }