00001 #include "Config.h"
00002 #include <libxml/xmlmemory.h>
00003 #include <libxml/parser.h>
00004 #include <iostream>
00005 #include <fstream>
00006 #include <cstdio>
00007 #include <cerrno>
00008 #include <cstring>
00009 #include <cctype>
00010 #include <string>
00011 #ifdef PLATFORM_APERIOS
00012 # include <OPENR/OPENRAPI.h>
00013 #else
00014 #include <sys/param.h>
00015 #include <unistd.h>
00016 #endif
00017 #include "Shared/ProjectInterface.h"
00018 #include "Shared/string_util.h"
00019
00020 #include "Vision/RawCameraGenerator.h"
00021
00022
00023 #include "Shared/debuget.h"
00024
00025
00026
00027
00028
00029
00030
00031 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::main_config::consoleMode_t);
00032 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::sound_config::volume_levels);
00033 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::transports);
00034 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::RawCamConfig::compression_t);
00035 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::RawCamConfig::encoding_t);
00036 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::SegCamConfig::compression_t);
00037 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::gain_levels);
00038 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::shutter_speeds);
00039 INSTANTIATE_NAMEDENUMERATION_STATICS(Config::vision_config::white_balance_levels);
00040 INSTANTIATE_NAMEDENUMERATION_STATICS(J_DCT_METHOD);
00041
00042
00043 Config* config=NULL;
00044 const std::locale& Config::curLocale=std::locale::classic();
00045
00046 const std::string ConfigDictionary::msPrefix="Model";
00047 const std::string ConfigDictionary::msSep=":";
00048
00049 std::string ConfigDictionary::curModel;
00050
00051 const char * Config::xmlIntro1="<?xml version";
00052 const char * Config::xmlIntro2="<!DOCTYPE ";
00053 const char * Config::xmlIntro3="<plist";
00054
00055 const char * Config::transport_names[] = { "UDP", "TCP", "" };
00056
00057 const char * Config::vision_config::dct_method_names[4] = { "islow", "ifast", "float", "" };
00058 const char * Config::vision_config::RawCamConfig::encoding_names[Config::vision_config::RawCamConfig::NUM_ENCODINGS+1] = { "color", "grayscale", "" };
00059 const char * Config::vision_config::RawCamConfig::compression_names[Config::vision_config::RawCamConfig::NUM_COMPRESSIONS+1] = { "none", "jpeg", "" };
00060 const char * Config::vision_config::SegCamConfig::compression_names[Config::vision_config::SegCamConfig::NUM_COMPRESSIONS+1] = { "none", "rle", "" };
00061
00062 const char * Config::main_config::consoleModeNames[Config::main_config::NUM_CONSOLE_MODES+1] = { "controller", "textmsg", "auto", "" };
00063
00064 bool ConfigDictionary::loadXMLNode(const std::string& key, xmlNode* val, const std::string& comment) {
00065
00066 if(key.substr(0,msPrefix.size())!=msPrefix)
00067 return plist::Dictionary::loadXMLNode(key,val,comment);
00068
00069
00070
00071 const std::string& k=key;
00072
00073
00074 if(k.size()<=msPrefix.size() || k.substr(msPrefix.size(),msSep.size())!=msSep) {
00075
00076 return plist::Dictionary::loadXMLNode(key,val,comment);
00077 }
00078
00079
00080 std::string::size_type patStart=msPrefix.size()+msSep.size();
00081 std::string::size_type patEnd=k.find(msSep,patStart);
00082 std::string pattern=k.substr(patStart,patEnd-patStart);
00083 if(!matchNoCase(curModel,pattern))
00084 return false;
00085 if(patEnd==std::string::npos) {
00086
00087 loadXML(val);
00088 return true;
00089 } else {
00090
00091 return plist::Dictionary::loadXMLNode(k.substr(patEnd+1),val,comment);
00092 }
00093 }
00094 bool ConfigDictionary::saveOverXMLNode(xmlNode* k, xmlNode* val, const std::string& key, std::string comment, const std::string& indentStr, std::set<std::string>& seen) const {
00095
00096 if(key.substr(0,msPrefix.size())!=msPrefix)
00097 return plist::Dictionary::saveOverXMLNode(k,val,key,comment,indentStr,seen);
00098
00099
00100
00101 const std::string& ks=key;
00102
00103
00104 if(ks.size()<=msPrefix.size() || ks.substr(msPrefix.size(),msSep.size())!=msSep) {
00105
00106 return plist::Dictionary::saveOverXMLNode(k,val,key,comment,indentStr,seen);
00107 }
00108
00109
00110 std::string::size_type patStart=msPrefix.size()+msSep.size();
00111 std::string::size_type patEnd=ks.find(msSep,patStart);
00112 std::string pattern=ks.substr(patStart,patEnd-patStart);
00113 if(!matchNoCase(curModel,pattern))
00114 return true;
00115 if(patEnd==std::string::npos) {
00116
00117 saveXML(val,true,seen);
00118 return true;
00119 } else {
00120
00121 const std::string skey=ks.substr(patEnd+1);
00122 return plist::Dictionary::saveOverXMLNode(k,val,skey,comment,indentStr,seen);
00123 }
00124 }
00125
00126 bool ConfigDictionary::matchNoCase(const std::string& model, const std::string& pattern) {
00127 unsigned int i=0;
00128 if(i==pattern.size() && i==model.size())
00129 return true;
00130 if(i==pattern.size() || i==model.size())
00131 return false;
00132 while(pattern[i]!='*') {
00133 if(toupper(pattern[i])!=toupper(model[i]))
00134 return false;
00135 i++;
00136 if(i==pattern.size() && i==model.size())
00137 return true;
00138 if(i==pattern.size() || i==model.size())
00139 return false;
00140 }
00141 i=pattern.size()-1;
00142 unsigned int j=model.size()-1;
00143 while(pattern[i]!='*') {
00144 if(toupper(pattern[i])!=toupper(model[j]))
00145 return false;
00146 i--; j--;
00147 }
00148 return true;
00149 }
00150
00151 void Config::saveXML(xmlNode* node, bool onlyOverwrite, std::set<std::string>& seen) const {
00152 if(node==NULL)
00153 return;
00154 if(node->children==NULL) {
00155 std::string indentStr=getIndentationPrefix(node);
00156 const char* headerComment="\n"
00157 "##################################################################\n"
00158 "###################### Tekkotsu config #######################\n"
00159 "##################################################################\n"
00160
00161 "##################### $" "Revision: $ ######################\n"
00162 "################ $" "Date: $ ################\n"
00163 "##################################################################\n"
00164 "\n"
00165 "This is an XML-based format using the Property List (plist) layout.\n"
00166 "\n"
00167 "As a slight extension to standard plists, you can specify\n"
00168 "model-specific settings by prepending a key with:\n"
00169 " Model:MODEL_PATTERN:KEY_NAME\n"
00170 "For example, to use different 'thresh' settings on the ERS-2xx\n"
00171 "series vs. the ERS-7 model, you would use the keys:\n"
00172 " Model:ERS-2*:thresh\n"
00173 " Model:ERS-7:thresh\n"
00174 "You can filter several items in a group by leaving off the second\n"
00175 "':' and name, and providing a dictionary as the value. If the\n"
00176 "model matches, all entries from the dictionary are imported into\n"
00177 "the parent dictionary.\n"
00178 "\n"
00179 "You can change these settings at run time from the Controller\n"
00180 "by entering the command:\n"
00181 " !set section_name.variable=value\n"
00182 "or by using the configuration editor found in the \"File Access\"\n"
00183 "menu. Paths are assumed to be relative to the 'project/ms/'\n"
00184 "directory. For example, the primary configuration file would be\n"
00185 "referenced as 'config/tekkotsu.xml'\n"
00186 "\n"
00187 "See also the 'simulator.xml' file, which provides you the ability\n"
00188 "to override settings when running in the simulator\n";
00189 xmlAddChild(node,xmlNewComment((const xmlChar*)headerComment));
00190 xmlAddChild(node,xmlNewText((const xmlChar*)("\n"+indentStr).c_str()));
00191 }
00192 ConfigDictionary::saveXML(node,onlyOverwrite,seen);
00193 }
00194
00195 unsigned int Config::loadBuffer(const char buf[], unsigned int len) {
00196 if(strncmp(buf,xmlIntro1,strlen(xmlIntro1))==0 || strncmp(buf,xmlIntro2,strlen(xmlIntro2))==0 || strncmp(buf,xmlIntro3,strlen(xmlIntro3))==0)
00197 return plist::Dictionary::loadBuffer(buf,len);
00198 return loadOldFormat(buf,len);
00199 }
00200
00201 unsigned int Config::loadFile(const char* filename) {
00202 std::string path=portPath(filename);
00203 bool isXML;
00204 {
00205 std::ifstream in(path.c_str());
00206 std::string line;
00207 std::getline(in,line);
00208 isXML = (strncmp(line.c_str(),xmlIntro1,strlen(xmlIntro1))==0 || strncmp(line.c_str(),xmlIntro2,strlen(xmlIntro2))==0 || strncmp(line.c_str(),xmlIntro3,strlen(xmlIntro3))==0);
00209 }
00210 if(isXML)
00211 return plist::Dictionary::loadFile(path.c_str());
00212 FILE* fp = fopen(path.c_str(), "r");
00213 if (fp==NULL) {
00214 std::cerr << "ERROR: Config could not open file '" << path.c_str() << "' for loading." << std::endl;
00215 return 0;
00216 }
00217 try {
00218 unsigned int ans=loadOldFormat(fp);
00219 fclose(fp);
00220 return ans;
00221 } catch(...) {
00222 fclose(fp);
00223 throw;
00224 }
00225 }
00226
00227 unsigned int Config::loadFileStream(FILE* f) {
00228 std::string line;
00229 char c=fgetc(f);
00230 while(c!='\n' && c!=EOF) {
00231 line+=c;
00232 c=fgetc(f);
00233 }
00234 bool isXML = (strncmp(line.c_str(),xmlIntro1,strlen(xmlIntro1))==0 || strncmp(line.c_str(),xmlIntro2,strlen(xmlIntro2))==0 || strncmp(line.c_str(),xmlIntro3,strlen(xmlIntro3))==0);
00235 if(fseek(f,line.size()+1,SEEK_CUR)!=0) {
00236 #ifdef PLATFORM_APERIOS
00237 int merrno=errno;
00238 std::cerr << "Warning: error on seek: " << strerror(merrno) << std::endl;
00239 #else
00240 char err[100];
00241 strerror_r(errno,err,100);
00242 std::cerr << "Warning: error on seek: " << err << std::endl;
00243 #endif
00244 }
00245 if(isXML)
00246 return plist::Dictionary::loadFileStream(f);
00247 return loadOldFormat(f);
00248 }
00249
00250 void Config::setFileSystemRoot(const std::string& fsr) {
00251 #ifdef PLATFORM_APERIOS
00252 fsRoot=fsr;
00253 #else
00254 char buf[MAXPATHLEN+2];
00255 if(getcwd(buf,MAXPATHLEN+2)==NULL)
00256 perror("Config::setFileSystemRoot(): getcwd");
00257 buf[MAXPATHLEN+1]='\0';
00258 std::string sbuf(buf);
00259 if(sbuf[sbuf.size()-1]!='/' && fsr[0]!='/')
00260 fsRoot=sbuf+'/'+fsr;
00261 else if(sbuf[sbuf.size()-1]=='/' && fsr[0]=='/')
00262 fsRoot=sbuf+fsr.substr(1);
00263 else
00264 fsRoot=sbuf+fsr;
00265 #endif
00266 }
00267
00268
00269 std::string Config::portPath(const std::string& path) const {
00270 if(fsRoot.size()==0)
00271 return path;
00272 if(path.size()==0)
00273 return fsRoot;
00274 if(path.substr(0,fsRoot.size())==fsRoot)
00275 return path;
00276 else if(fsRoot[fsRoot.size()-1]=='/') {
00277 if(path[0]!='/')
00278 return fsRoot+path;
00279 else
00280 return fsRoot+path.substr(1);
00281 } else {
00282 if(path[0]!='/')
00283 return fsRoot+'/'+path;
00284 else
00285 return fsRoot+path;
00286 }
00287 }
00288
00289 void* Config::setValue(const std::string& section, std::string key, const std::string& value) {
00290 if(section=="vision") {
00291 if (key=="resolution") {
00292 if (value=="full") {
00293 vision.resolution=1;
00294 } else if (value=="half") {
00295 vision.resolution=2;
00296 } else if (value=="quarter") {
00297 vision.resolution=3;
00298 }
00299 return &vision.resolution;
00300 } else if (key=="rawcam_encoding") {
00301 if (value=="color") {
00302 vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_COLOR;
00303 vision.rawcam.channel=RawCameraGenerator::CHAN_Y;
00304 } else if (value=="y_only") {
00305 vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00306 vision.rawcam.channel=RawCameraGenerator::CHAN_Y;
00307 } else if (value=="uv_only") {
00308 vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_COLOR;
00309 vision.rawcam.channel=-1;
00310 } else if (value=="u_only") {
00311 vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00312 vision.rawcam.channel=RawCameraGenerator::CHAN_U;
00313 } else if (value=="v_only") {
00314 vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00315 vision.rawcam.channel=RawCameraGenerator::CHAN_V;
00316 } else if (value=="y_dx_only") {
00317 vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00318 vision.rawcam.channel=RawCameraGenerator::CHAN_Y_DX;
00319 } else if (value=="y_dy_only") {
00320 vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00321 vision.rawcam.channel=RawCameraGenerator::CHAN_Y_DY;
00322 } else if (value=="y_dxdy_only") {
00323 vision.rawcam.encoding=vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL;
00324 vision.rawcam.channel=RawCameraGenerator::CHAN_Y_DXDY;
00325 }
00326 return &vision.rawcam.encoding;
00327 } else if (key=="focal_len_x" || key=="focal_len_y"
00328 || key=="principle_point_x" || key=="principle_point_y"
00329 || key=="skew" || key=="kc1_r2" || key=="kc2_r4"
00330 || key=="kc5_r6" || key=="kc3_tan1" || key=="kc4_tan2"
00331 || key=="calibration_res_x" || key=="calibration_res_y" )
00332 {
00333 key="calibration."+key;
00334 } else if (key.compare(0,7,"rawcam_")==0) {
00335 key[6]='.';
00336 } else if (key.compare(0,7,"segcam_")==0) {
00337 key[6]='.';
00338 } else if (key.compare(0,10,"regioncam_")==0) {
00339 key[9]='.';
00340 } else if (key=="region_port" || key=="region_transport") {
00341 key.replace(0,7,"regioncam.");
00342 }
00343 } else if(section=="motion") {
00344 std::string calibratePrefix="calibrate:";
00345 if (key.substr(0,calibratePrefix.size())==calibratePrefix) {
00346 std::string keyval = key.substr(calibratePrefix.size());
00347 OutputConfig<plist::Primitive<float> >::const_iterator it = motion.calibration_scale.findEntry(keyval);
00348 plist::Primitive<float>* prim = NULL;
00349 if(it!=motion.calibration_scale.end())
00350 prim = dynamic_cast<plist::Primitive<float> *>(it->second);
00351 if(prim==NULL) {
00352 std::cout << "WARNING: Could not match '" << key.substr(10) << "' as calibration parameter" << std::endl;
00353 return NULL;
00354 }
00355
00356 prim->set(value);
00357 *prim = 1 / *(prim);
00358 return prim;
00359 }
00360 } else if(section=="sound") {
00361 if (key=="streaming.mic_bits") {
00362 key="streaming.mic_sample_bits";
00363 } else if (key=="streaming.speaker_frame_len") {
00364 key="streaming.speaker_frame_length";
00365 }
00366 } else if(section=="main") {
00367 if (key=="debug_level" || key=="error_level" || key=="verbose_level")
00368 return NULL;
00369 }
00370
00371 plist::Collection * sct=dynamic_cast<plist::Collection*>(resolveEntry(section));
00372 if(sct==NULL)
00373 return NULL;
00374 plist::ObjectBase * entry=sct->resolveEntry(key);
00375 if(entry==NULL) {
00376 std::cerr << "Unknown " << section << " key '" << key << "' for value " << value << std::endl;
00377 return NULL;
00378 } else if(plist::PrimitiveBase * prim=dynamic_cast<plist::PrimitiveBase*>(entry)) {
00379 try {
00380 prim->set(value);
00381 } catch(...) {
00382 std::cerr << "Error setting " << section << " key '" << key << "' to value " << value << std::endl;
00383 return NULL;
00384 }
00385 return prim;
00386 } else if(plist::ArrayBase::StringConversion * arr=dynamic_cast<plist::ArrayBase::StringConversion*>(entry)) {
00387 try {
00388 arr->addValue(value);
00389 } catch(...) {
00390 std::cerr << "Error setting " << section << " key '" << key << "' to value " << value << std::endl;
00391 return NULL;
00392 }
00393 return arr;
00394 } else {
00395 std::cerr << "Unknown type of " << section << " key '" << key << "' for value " << value << std::endl;
00396 return NULL;
00397 }
00398 }
00399
00400 unsigned int Config::loadOldFormat(const char buf[], unsigned int len) {
00401 if (buf==NULL) return 0;
00402 unsigned int numRead=0, used=0;
00403 char line[256];
00404 unsigned int lineno=0;
00405 std::vector<std::string> modelStack;
00406 bool ignoring=false;
00407 std::string section="invalid";
00408 while (numRead<len && (used=sscanf(&buf[numRead],"%255[^\n]\n", line))!=(unsigned int)EOF) {
00409 numRead+=used;
00410 parseLine(line,++lineno,modelStack,ignoring,section);
00411 }
00412 return numRead;
00413 }
00414
00415 unsigned int Config::loadOldFormat(FILE* fp) {
00416 if (fp==NULL) return 0;
00417 unsigned int numRead=0;
00418 char line[256];
00419 unsigned int lineno=0;
00420 std::vector<std::string> modelStack;
00421 bool ignoring=false;
00422 std::string section="invalid";
00423 while (fgets(line,256,fp)!=NULL) {
00424 numRead+=strlen(line);
00425 parseLine(line,++lineno,modelStack,ignoring,section);
00426 }
00427 return numRead;
00428 }
00429
00430 void Config::parseLine(const char buf[], unsigned int lineno, std::vector<std::string>& modelStack, bool& ignoring, std::string& section) {
00431 char key[30], value[50];
00432 while(std::isspace(*buf))
00433 buf++;
00434 if(buf[0]=='#')
00435 return;
00436 if (sscanf(buf,"<%29[^>]>",key)>0) {
00437 if(key[0]=='/') {
00438 if(modelStack.size()==0) {
00439 printf("WARNING: not in a model specific section, line %d\n",lineno);
00440 return;
00441 }
00442 bool subset=matchNoCase(&key[1],modelStack.back());
00443 bool superset=matchNoCase(modelStack.back(),&key[1]);
00444 if(subset && superset) {
00445
00446 modelStack.pop_back();
00447 } else if(superset) {
00448 while(modelStack.size()>0) {
00449
00450 modelStack.pop_back();
00451 if(!matchNoCase(modelStack.back(),&key[1]))
00452 break;
00453 }
00454 } else
00455 printf("WARNING: config model mismatch, line %d\n",lineno);
00456
00457 ignoring=false;
00458 for(unsigned int i=0; i<modelStack.size(); i++)
00459 if(!matchNoCase(curModel,modelStack[i])) {
00460 ignoring=true;
00461 break;
00462 }
00463
00464
00465 } else {
00466 modelStack.push_back(key);
00467
00468 ignoring=ignoring || !matchNoCase(curModel,key);
00469
00470 }
00471 } else if(!ignoring) {
00472 if (sscanf(buf,"[%29[^]]]",key)>0) {
00473 section=key;
00474 std::transform(section.begin(), section.end(), section.begin(), (int(*)(int)) std::tolower);
00475
00476 plist::Collection * sct=dynamic_cast<plist::Collection*>(resolveEntry(section));
00477 if(sct==NULL)
00478 std::cerr << "ERROR: Unknown configuration section " << section << std::endl;
00479 } else if (sscanf(buf,"%29[^ =] =%49s",key,value)>1) {
00480
00481
00482 setValue(section, string_util::trim(key), string_util::trim(value));
00483
00484
00485
00486
00487 }
00488 }
00489 }
00490
00491 void Config::vision_config::computeRay(float x, float y, float& r_x, float& r_y, float& r_z) {
00492 x=(x+1)*calibration.calibration_res_x/2;
00493 y=(y+1)*calibration.calibration_res_y/2;
00494 float yd=(y-calibration.principle_point_y)/calibration.focal_len_y;
00495 float xd=(x-calibration.principle_point_x)/calibration.focal_len_x-calibration.skew*yd;
00496 float r2=xd*xd+yd*yd;
00497 float radial=(1 + calibration.kc1_r2*r2 + calibration.kc2_r4*r2*r2 + calibration.kc5_r6*r2*r2*r2);
00498 r_x=(xd - 2*calibration.kc3_tan1*xd*yd - calibration.kc4_tan2*(r2+2*xd*xd))/radial;
00499 r_y=(yd - calibration.kc3_tan1*(r2+2*yd*yd) - 2*calibration.kc4_tan2*xd*yd)/radial;
00500 r_z=1;
00501 }
00502
00503
00504
00505
00506
00507
00508
00509 void Config::vision_config::computePixel(float r_x, float r_y, float r_z, float& x, float& y) {
00510 if(r_z==0) {
00511 x=y=0;
00512 return;
00513 }
00514 r_x/=r_z;
00515 r_y/=r_z;
00516 float r2 = r_x*r_x + r_y*r_y;
00517 float radial=(1 + calibration.kc1_r2*r2 + calibration.kc2_r4*r2*r2 + calibration.kc5_r6*r2*r2*r2);
00518 float xd = radial*r_x + 2*calibration.kc3_tan1*r_x*r_y + calibration.kc4_tan2*(r2+2*r_x*r_x);
00519 float yd = radial*r_y + calibration.kc3_tan1*(r2+2*r_y*r_y) + 2*calibration.kc4_tan2*r_x*r_y;
00520 x=calibration.focal_len_x*(xd+calibration.skew*yd)+calibration.principle_point_x;
00521 y=calibration.focal_len_y*yd+calibration.principle_point_y;
00522 x=2*x/calibration.calibration_res_x-1;
00523 y=2*y/calibration.calibration_res_y-1;
00524 }
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537