ObjectModelParser.cpp
Go to the documentation of this file.
00001 /*
00002  * ObjectModelParser.cpp
00003  *
00004  *  Created on: Mar 6, 2012
00005  *      Author: sdries
00006  */
00007 
00008 #include "wire/util/ObjectModelParser.h"
00009 #include "wire/core/ClassModel.h"
00010 #include "wire/storage/KnowledgeDatabase.h"
00011 
00012 #include <problib/pdfs/Uniform.h>
00013 #include <problib/pdfs/PMF.h>
00014 
00015 using namespace std;
00016 
00017 namespace mhf {
00018 
00019 ObjectModelParser::ObjectModelParser(const std::string& filename) : filename_(filename),
00020     object_model_loader_(new pluginlib::ClassLoader<IStateEstimator>("wire_core", "IStateEstimator")) {
00021 
00022 }
00023 
00024 ObjectModelParser::~ObjectModelParser() {
00025     // TODO: change ownership of the class_loader, as it must persist throughout the whole lifetime of the world model
00026     //delete object_model_loader_;
00027 }
00028 
00029 std::string ObjectModelParser::getErrorMessage() const {
00030     return parse_errors_.str();
00031 }
00032 
00033 string ObjectModelParser::getPropertyValue(const TiXmlElement* elem, string prop_name, double& value, stringstream& error, bool optional) {
00034     const TiXmlElement* p = elem->FirstChildElement(prop_name);
00035     if (p) {
00036         return p->Attribute("value", &value);
00037     }
00038     if (!optional) {
00039         error << "Could not find property '" << prop_name << "' of element '" << elem->Value() << "'" << endl;
00040     }
00041     return "";
00042 }
00043 
00044 bool ObjectModelParser::getAttributeValue(const TiXmlElement* elem, string att_name, string& att_value, stringstream& error) {
00045     if (!elem) return false;
00046 
00047     const char* value = elem->Attribute(att_name.c_str());
00048     if (!value) {
00049         error << "Could not find attribute '" << att_name << "' of element '" << elem->Value() << "'" << endl;
00050         return false;
00051     }
00052 
00053     att_value = value;
00054     return true;
00055 }
00056 
00057 bool ObjectModelParser::getAttributeValue(const TiXmlElement* elem, string att_name, double& att_value, stringstream& error) {
00058     if (!elem) return false;
00059 
00060     const char* value = elem->Attribute(att_name.c_str(), &att_value);
00061 
00062     if (!value) {
00063         error << "Could not find attribute '" << att_name << "' of element '" << elem->Value() << "'" << endl;
00064         return false;
00065     }
00066 
00067     return true;
00068 }
00069 
00070 bool ObjectModelParser::hasAttributeValue(const TiXmlElement* elem, string att_name, string att_value) {
00071     if (!elem) return false;
00072 
00073     const char* value = elem->Attribute(att_name.c_str());
00074     if (!value) return false;
00075 
00076     string valueStr = value;
00077     return (att_value == valueStr);
00078 }
00079 
00080 pbl::PDF* ObjectModelParser::parsePDF(const TiXmlElement* pdf_elem, std::stringstream& error) {
00081     const char* pdf_type = pdf_elem->Attribute("type");
00082     if (pdf_type) {
00083         if (string(pdf_type) == "uniform") {
00084             double dim = 0;
00085             double density = 0;
00086             if (getAttributeValue(pdf_elem, "dimensions", dim, error)
00087                     && getAttributeValue(pdf_elem, "density", density, error)) {
00088                 return new pbl::Uniform((int)dim, density);
00089             }
00090         } else if (string(pdf_type) == "discrete") {
00091             double domain_size;
00092             if (getAttributeValue(pdf_elem, "domain_size", domain_size, error)) {
00093                 return new pbl::PMF((int)domain_size);
00094             }
00095         } else {
00096             error << "Unknown pdf type: " << pdf_type << endl;
00097         }
00098     } else {
00099         error << "PDF specification should contain 'type' attribute" << endl;
00100     }
00101     return 0;
00102 }
00103 
00104 bool ObjectModelParser::parseStateEstimator(ClassModel* obj_model, const TiXmlElement* elem, std::stringstream& error) {
00105 
00106     // check behavior model's attribute and model type
00107     string attribute_name, model_type;
00108     if (!getAttributeValue(elem, "attribute", attribute_name, error)
00109             | !getAttributeValue(elem, "model", model_type, error)) {
00110         return false;
00111     }
00112 
00113     Attribute attribute = AttributeConv::attribute(attribute_name);
00114 
00115     if (!object_model_loader_->isClassAvailable(model_type)){
00116         std::vector<std::string> classes = object_model_loader_->getDeclaredClasses();
00117         for(unsigned int i = 0; i < classes.size(); ++i){
00118             if(model_type == object_model_loader_->getName(classes[i])){
00119                 //if we've found a match... we'll get the fully qualified name and break out of the loop
00120                 ROS_WARN("Planner specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.",
00121                         model_type.c_str(), classes[i].c_str());
00122                 model_type = classes[i];
00123                 break;
00124             }
00125         }
00126     }
00127 
00128     IStateEstimator* estimator;
00129 
00130     if (object_model_loader_->isClassAvailable(model_type)) {
00131         estimator = object_model_loader_->createClassInstance(model_type)->clone();
00132     } else {
00133         error << "Unknown model: " << model_type << endl;
00134         return false;
00135     }
00136 
00137     // set estimator parameters
00138     const TiXmlElement* param = elem->FirstChildElement("param");
00139     while (param) {
00140         const char* param_name = param->Attribute("name");
00141 
00142         if (param_name) {
00143             bool v_bool;
00144             int v_int;
00145             double v_double;
00146 
00147             bool set_param_ok = true;
00148             if (param->QueryDoubleAttribute("value", &v_double) == TIXML_SUCCESS) {
00149                 set_param_ok = estimator->setParameter(string(param_name), v_double);
00150             } else if (param->QueryIntAttribute("value", &v_int) == TIXML_SUCCESS) {
00151                 set_param_ok = estimator->setParameter(string(param_name), (double)v_int);
00152             } else if (param->QueryBoolAttribute("value", &v_bool) == TIXML_SUCCESS) {
00153                 set_param_ok = estimator->setParameter(string(param_name), v_bool);
00154             } else {
00155                 const char* v_str = param->Attribute("value");
00156                 if (v_str) {
00157                     set_param_ok = estimator->setParameter(string(param_name), string(v_str));
00158                 } else {
00159                     error << "State estimator parameters should always have a 'name' and 'value' attribute." << endl;
00160                 }
00161             }
00162 
00163             if (!set_param_ok) {
00164                 error << "Unknown parameter for estimator '" << model_type << "': " << param_name << endl;
00165             }
00166 
00167         } else {
00168             error << "State estimator parameters should always have a 'name' and 'value' attribute." << endl;
00169         }
00170 
00171         param = param->NextSiblingElement("param");
00172     }
00173 
00174     const TiXmlElement* pnew = elem->FirstChildElement("pnew");
00175     if (pnew) {
00176         pbl::PDF* pdf_new = parsePDF(pnew, error);
00177         if (pdf_new) {
00178             obj_model->setNewPDF(attribute, *pdf_new);
00179 
00180             estimator->update(*pdf_new, 0);
00181 
00182             delete pdf_new;
00183         } else {
00184             return false;
00185         }
00186     } else {
00187         error << "Estimator specification does not contain 'pnew'." << endl;
00188         return false;
00189     }
00190 
00191     const TiXmlElement* pclutter = elem->FirstChildElement("pclutter");
00192     if (pnew) {
00193         pbl::PDF* pdf_clutter = parsePDF(pclutter, error);
00194         if (pdf_clutter) {
00195             obj_model->setClutterPDF(attribute, *pdf_clutter);
00196 
00197             delete pdf_clutter;
00198         } else {
00199             return false;
00200         }
00201     } else {
00202         error << "Estimator specification does not contain 'pclutter'." << endl;
00203         return false;
00204     }
00205 
00206     obj_model->setEstimator(attribute, *estimator);
00207 
00208     return true;
00209 }
00210 
00211 bool ObjectModelParser::getStateEstimatorParameter(const TiXmlElement* elem, const string& param_name, double& value) {
00212 
00213     const TiXmlElement* param = elem->FirstChildElement("param");
00214     while (param) {
00215         const char* v = param->Attribute("name");
00216         if (v && (string)v == param_name) {
00217             param->Attribute("value", &value);
00218             return true;
00219         }
00220         param = param->NextSiblingElement("param");
00221     }
00222 
00223     return false;
00224 }
00225 
00226 bool ObjectModelParser::parse(KnowledgeDatabase& knowledge_db) {
00227 
00228     TiXmlDocument doc(filename_);
00229     doc.LoadFile();
00230 
00231     if (doc.Error()) {
00232         ROS_ERROR_STREAM("While parsing '" << filename_ << "': " << endl << endl << doc.ErrorDesc() << " at line " << doc.ErrorRow() << ", col " << doc.ErrorCol());
00233         return false;
00234     }
00235 
00236     const TiXmlElement* root = doc.RootElement();
00237 
00238     double prior_new;
00239     const TiXmlElement* prior_new_elem = root->FirstChildElement("prior_new");
00240     if (prior_new_elem) {
00241         if (getAttributeValue(prior_new_elem, "value", prior_new, parse_errors_)) {
00242             knowledge_db.setPriorNew(prior_new);
00243         }
00244     } else {
00245         parse_errors_ << "Knowledge file does not contain 'prior_new'" << endl;
00246     }
00247 
00248     double prior_existing;
00249     const TiXmlElement* prior_existing_elem = root->FirstChildElement("prior_existing");
00250     if (prior_existing_elem) {
00251         if (getAttributeValue(prior_existing_elem, "value", prior_existing, parse_errors_)) {
00252             knowledge_db.setPriorExisting(prior_existing);
00253         }
00254     } else {
00255         parse_errors_ << "Knowledge file does not contain 'prior_existing'" << endl;
00256     }
00257 
00258     double prior_clutter;
00259     const TiXmlElement* prior_clutter_elem = root->FirstChildElement("prior_clutter");
00260     if (prior_clutter_elem) {
00261         if (getAttributeValue(prior_clutter_elem, "value", prior_clutter, parse_errors_)) {
00262             knowledge_db.setPriorClutter(prior_clutter);
00263         }
00264     } else {
00265         parse_errors_ << "Knowledge file does not contain 'prior_clutter'" << endl;
00266     }
00267 
00268 
00269     const TiXmlElement* class_element = root->FirstChildElement("object_class");
00270 
00271     /* PARSE ALL OBJECT MODELS */
00272 
00273     while(class_element) {
00274         ClassModel* class_model = 0;
00275 
00276         string model_name;
00277         getAttributeValue(class_element, "name", model_name, parse_errors_);
00278 
00279         cout << "Parsing model for class " << model_name << endl;
00280 
00281         string base_class = "";
00282         const char* value = class_element->Attribute("base");
00283 
00284         if (value) {
00285             // class derives from base class
00286             base_class = value;
00287 
00288             const ClassModel* base_model = knowledge_db.getClassModel(base_class);
00289             if (base_model) {
00290                 class_model = new ClassModel(*base_model);
00291                 class_model->setModelName(model_name);
00292             } else {
00293                 parse_errors_ << "Error in class definition of '" << model_name << "': unknown base class '" << base_class << "'." << endl;
00294                 class_model = new ClassModel(model_name);
00295             }
00296         } else {
00297             class_model = new ClassModel(model_name);
00298         }
00299 
00300         // parse properties
00301         const TiXmlElement* prop = class_element->FirstChildElement();
00302         while(prop) {
00303             string prop_name = prop->Value();
00304             if (prop_name == "behavior_model") {
00305                 stringstream bh_errors;
00306                 parseStateEstimator(class_model, prop, bh_errors);
00307                 if (bh_errors.str() != "") {
00308                     parse_errors_ << "In class description for '" << class_model->getModelName() << "': " << bh_errors.str() << endl;
00309                 }
00310             } else {
00311                 parse_errors_ << "In class description for '" << class_model->getModelName() << "': Unknown class property: '" << prop_name << "'" << endl;
00312             }
00313             prop = prop->NextSiblingElement();
00314         }
00315 
00316         knowledge_db.addClassModel(class_model->getModelName(), class_model);
00317 
00318         class_element = class_element->NextSiblingElement("object_class");
00319     }
00320 
00321     if (parse_errors_.str() != "") {
00322         return false;
00323     }
00324 
00325     return true;
00326 }
00327 
00328 }


wire_core
Author(s): Sjoerd van den Dries, Jos Elfring
autogenerated on Tue Jan 7 2014 11:43:19