00001 #include "Shared/Config.h"
00002 #include "Behaviors/Mon/RawCamBehavior.h"
00003 #include "Vision/RawCameraGenerator.h"
00004 #include <stdio.h>
00005 #include <string>
00006 #include <ctype.h>
00007 #include <OPENR/OPrimitiveControl.h>
00008 #include <OPENR/OPENRAPI.h>
00009
00010 Config* config=NULL;
00011
00012 void* Config::setValue(section_t section, const char *key, const char *value,
00013 bool ) {
00014 switch (section) {
00015 case sec_wireless:
00016 if (strncasecmp(key,"id",29)==0) {
00017 wireless.id=atoi(value);
00018 return &wireless.id;
00019 }
00020 break;
00021 case sec_vision:
00022 if (strncasecmp(key,"white_balance",29)==0) {
00023 if (strncasecmp(value,"indoor",49)==0) {
00024 vision.white_balance=1;
00025 } else if (strncasecmp(value,"flourescent",49)==0) {
00026 vision.white_balance=3;
00027 } else if (strncasecmp(value,"outdoor",49)==0) {
00028 vision.white_balance=2;
00029 }
00030
00031 OPrimitiveID fbkID = 0;
00032 if(OPENR::OpenPrimitive(CameraLocator, &fbkID) != oSUCCESS){
00033 std::cout << "Open FbkImageSensor failure." << std::endl;
00034 } else {
00035 OPrimitiveControl_CameraParam owb(vision.white_balance);
00036 if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_WHITE_BALANCE, &owb, sizeof(owb), 0, 0) != oSUCCESS){
00037 std::cout << "CAM_SET_WHITE_BALANCE : Failed!" << std::endl;
00038 }
00039 OPENR::ClosePrimitive(fbkID);
00040 }
00041 return &vision.white_balance;
00042 } else if (strncasecmp(key,"gain",29)==0) {
00043 if (strncasecmp(value,"low",49)==0) {
00044 vision.gain=1;
00045 } else if (strncasecmp(value,"mid",49)==0) {
00046 vision.gain=2;
00047 } else if (strncasecmp(value,"high",49)==0) {
00048 vision.gain=3;
00049 }
00050
00051 OPrimitiveID fbkID = 0;
00052 if(OPENR::OpenPrimitive(CameraLocator, &fbkID) != oSUCCESS){
00053 std::cout << "Open FbkImageSensor failure." << std::endl;
00054 } else {
00055 OPrimitiveControl_CameraParam ogain(vision.gain);
00056 if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &ogain, sizeof(ogain), 0, 0) != oSUCCESS)
00057 std::cout << "CAM_SET_GAIN : Failed!" << std::endl;
00058 OPENR::ClosePrimitive(fbkID);
00059 }
00060 return &vision.gain;
00061 } else if (strncasecmp(key,"shutter_speed",29)==0) {
00062 if (strncasecmp(value,"slow",49)==0) {
00063 vision.shutter_speed=1;
00064 } else if (strncasecmp(value,"mid",49)==0) {
00065 vision.shutter_speed=2;
00066 } else if (strncasecmp(value,"fast",49)==0) {
00067 vision.shutter_speed=3;
00068 }
00069
00070 OPrimitiveID fbkID = 0;
00071 if(OPENR::OpenPrimitive(CameraLocator, &fbkID) != oSUCCESS){
00072 std::cout << "Open FbkImageSensor failure." << std::endl;
00073 } else {
00074 OPrimitiveControl_CameraParam oshutter(vision.shutter_speed);
00075 if(OPENR::ControlPrimitive(fbkID,oprmreqCAM_SET_SHUTTER_SPEED, &oshutter, sizeof(oshutter), 0, 0) != oSUCCESS)
00076 std::cout << "CAM_SET_SHUTTER_SPEED : Failed!" << std::endl;
00077 OPENR::ClosePrimitive(fbkID);
00078 }
00079 return &vision.shutter_speed;
00080 } else if (strncasecmp(key,"resolution",29)==0) {
00081 if (strncasecmp(value,"full",49)==0) {
00082 vision.resolution=1;
00083 } else if (strncasecmp(value,"half",49)==0) {
00084 vision.resolution=2;
00085 } else if (strncasecmp(value,"quarter",49)==0) {
00086 vision.resolution=3;
00087 }
00088 return &vision.resolution;
00089 } else if (strncasecmp(key,"thresh",29)==0) {
00090 vision.thresh.push_back(value);
00091 return &vision.thresh;
00092 } else if (strncasecmp(key,"colors",29)==0) {
00093 strncpy(vision.colors,value,49);
00094 return &vision.colors;
00095 } else if (strncasecmp(key,"raw_port",29)==0) {
00096 vision.rawcam_port=atoi(value);
00097 return &vision.rawcam_port;
00098 } else if (strncasecmp(key,"rle_port",29)==0) {
00099 vision.rle_port=atoi(value);
00100 return &vision.rle_port;
00101 } else if (strncasecmp(key,"obj_port",29)==0) {
00102 vision.obj_port=atoi(value);
00103 return &vision.obj_port;
00104 } else if (strncasecmp(key,"restore_image",29)==0) {
00105 vision.restore_image=atoi(value);
00106 return &vision.obj_port;
00107 } else if (strncasecmp(key,"jpeg_dct_method",29)==0) {
00108 if (strncasecmp(value,"islow",49)==0) {
00109 vision.jpeg_dct_method=JDCT_ISLOW;
00110 } else if (strncasecmp(value,"ifast",49)==0) {
00111 vision.jpeg_dct_method=JDCT_IFAST;
00112 } else if (strncasecmp(value,"float",49)==0) {
00113 vision.jpeg_dct_method=JDCT_FLOAT;
00114 }
00115 return &vision.jpeg_dct_method;
00116 } else if (strncasecmp(key,"rawcam_encoding",29)==0) {
00117 if (strncasecmp(value,"color",49)==0) {
00118 vision.rawcam_encoding=vision_config::ENCODE_COLOR;
00119 vision.rawcam_channel=RawCameraGenerator::CHAN_Y;
00120 } else if (strncasecmp(value,"y_only",49)==0) {
00121 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00122 vision.rawcam_channel=RawCameraGenerator::CHAN_Y;
00123 } else if (strncasecmp(value,"uv_only",49)==0) {
00124 vision.rawcam_encoding=vision_config::ENCODE_COLOR;
00125 vision.rawcam_channel=-1;
00126 } else if (strncasecmp(value,"u_only",49)==0) {
00127 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00128 vision.rawcam_channel=RawCameraGenerator::CHAN_U;
00129 } else if (strncasecmp(value,"v_only",49)==0) {
00130 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00131 vision.rawcam_channel=RawCameraGenerator::CHAN_V;
00132 } else if (strncasecmp(value,"y_dx_only",49)==0) {
00133 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00134 vision.rawcam_channel=RawCameraGenerator::CHAN_Y_DX;
00135 } else if (strncasecmp(value,"y_dy_only",49)==0) {
00136 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00137 vision.rawcam_channel=RawCameraGenerator::CHAN_Y_DY;
00138 } else if (strncasecmp(value,"y_dxdy_only",49)==0) {
00139 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00140 vision.rawcam_channel=RawCameraGenerator::CHAN_Y_DXDY;
00141 }
00142 return &vision.rawcam_encoding;
00143 } else if (strncasecmp(key,"rawcam_compression",29)==0) {
00144 if (strncasecmp(value,"none",49)==0) {
00145 vision.rawcam_compression=vision_config::COMPRESS_NONE;
00146 } else if (strncasecmp(value,"jpeg",49)==0) {
00147 vision.rawcam_compression=vision_config::COMPRESS_JPEG;
00148 }
00149 return &vision.rawcam_compression;
00150 } else if (strncasecmp(key,"rawcam_compress_quality",29)==0) {
00151 vision.rawcam_compress_quality=atoi(value);
00152 return &vision.rawcam_compress_quality;
00153 } else if (strncasecmp(key,"rawcam_y_skip",29)==0) {
00154 vision.rawcam_y_skip=atoi(value);
00155 return &vision.rawcam_y_skip;
00156 } else if (strncasecmp(key,"rawcam_uv_skip",29)==0) {
00157 vision.rawcam_uv_skip=atoi(value);
00158 return &vision.rawcam_uv_skip;
00159 } else if (strncasecmp(key,"rlecam_skip",29)==0) {
00160 vision.rlecam_skip=atoi(value);
00161 return &vision.rlecam_skip;
00162 } else if (strncasecmp(key,"rlecam_channel",29)==0) {
00163 vision.rlecam_channel=atoi(value);
00164 return &vision.rlecam_channel;
00165 } else if (strncasecmp(key,"rlecam_compression",29)==0) {
00166 if (strncasecmp(value,"none",49)==0) {
00167 vision.rlecam_compression=vision_config::COMPRESS_NONE;
00168 } else if (strncasecmp(value,"rle",49)==0) {
00169 vision.rlecam_compression=vision_config::COMPRESS_RLE;
00170 }
00171 return &vision.rlecam_compression;
00172 }
00173 break;
00174 case sec_main:
00175 if (strncasecmp(key,"console_port",29)==0) {
00176 main.console_port=atoi(value);
00177 return &main.console_port;
00178 } else if (strncasecmp(key,"stderr_port",29)==0) {
00179 main.stderr_port=atoi(value);
00180 return &main.stderr_port;
00181 } else if (strncasecmp(key,"error_level",29)==0) {
00182 main.error_level=atoi(value);
00183 return &main.error_level;
00184 } else if (strncasecmp(key,"debug_level",29)==0) {
00185 main.debug_level=atoi(value);
00186 return &main.debug_level;
00187 } else if (strncasecmp(key,"verbose_level",29)==0) {
00188 main.verbose_level=atoi(value);
00189 return &main.verbose_level;
00190 } else if (strncasecmp(key,"wsjoints_port",29)==0) {
00191 main.wsjoints_port=atoi(value);
00192 return &main.wsjoints_port;
00193 } else if (strncasecmp(key,"wspids_port",29)==0) {
00194 main.wspids_port=atoi(value);
00195 return &main.wspids_port;
00196 } else if (strncasecmp(key,"headControl_port",29)==0) {
00197 main.headControl_port=atoi(value);
00198 return &main.headControl_port;
00199 } else if (strncasecmp(key,"walkControl_port",29)==0) {
00200 main.walkControl_port=atoi(value);
00201 return &main.walkControl_port;
00202 } else if (strncasecmp(key,"estopControl_port",29)==0) {
00203 main.estopControl_port=atoi(value);
00204 return &main.estopControl_port;
00205 } else if (strncasecmp(key,"aibo3d_port",29)==0) {
00206 main.aibo3d_port=atoi(value);
00207 return &main.aibo3d_port;
00208 } else if (strncasecmp(key,"wmmonitor_port",29)==0) {
00209 main.wmmonitor_port=atoi(value);
00210 return &main.wmmonitor_port;
00211 } else if (strncasecmp(key,"use_VT100",29)==0) {
00212 main.use_VT100=extractBool(value);
00213 return &main.use_VT100;
00214 }
00215 break;
00216 case sec_behaviors:
00217 break;
00218 case sec_controller:
00219 if (strncasecmp(key,"gui_port",29)==0) {
00220 controller.gui_port = atoi(value);
00221 return &controller.gui_port ;
00222 } else if (strncasecmp(key,"select_snd",29)==0) {
00223 strncpy(controller.select_snd,value,49);
00224 return &controller.select_snd;
00225 } else if (strncasecmp(key,"next_snd",29)==0) {
00226 strncpy(controller.next_snd,value,49);
00227 return &controller.next_snd;
00228 } else if (strncasecmp(key,"prev_snd",29)==0) {
00229 strncpy(controller.prev_snd,value,49);
00230 return &controller.prev_snd;
00231 } else if (strncasecmp(key,"read_snd",29)==0) {
00232 strncpy(controller.read_snd,value,49);
00233 return &controller.read_snd;
00234 } else if (strncasecmp(key,"cancel_snd",29)==0) {
00235 strncpy(controller.cancel_snd,value,49);
00236 return &controller.cancel_snd;
00237 } else if (strncasecmp(key,"error_snd",29)==0) {
00238 strncpy(controller.error_snd,value,49);
00239 return &controller.error_snd;
00240 }
00241 break;
00242 case sec_motion:
00243 if (strncasecmp(key,"root",29)==0) {
00244 motion.root=value;
00245 return &motion.root;
00246 } else if (strncasecmp(key,"walk",29)==0) {
00247 motion.walk=value;
00248 return &motion.walk;
00249 } else if (strncasecmp(key,"estop_on_snd",29)==0) {
00250 strncpy(motion.estop_on_snd,value,49);
00251 return &motion.estop_on_snd;
00252 } else if (strncasecmp(key,"estop_off_snd",29)==0) {
00253 strncpy(motion.estop_off_snd,value,49);
00254 return &motion.estop_off_snd;
00255 } else if (strncasecmp(key,"max_head_tilt_speed",29)==0) {
00256 motion.max_head_tilt_speed=atof(value);
00257 } else if (strncasecmp(key,"max_head_pan_speed",29)==0) {
00258 motion.max_head_pan_speed=atof(value);
00259 } else if (strncasecmp(key,"max_head_roll_speed",29)==0) {
00260 motion.max_head_roll_speed=atof(value);
00261 }
00262 break;
00263 case sec_worldmodel2:
00264 if (strncasecmp(key,"dm_port",29)==0) {
00265 worldmodel2.dm_port = atoi(value);
00266 return &worldmodel2.dm_port ;
00267 } else if (strncasecmp(key,"hm_port",29)==0) {
00268 worldmodel2.hm_port = atoi(value);
00269 return &worldmodel2.hm_port ;
00270 } else if (strncasecmp(key,"gm_port",29)==0) {
00271 worldmodel2.gm_port = atoi(value);
00272 return &worldmodel2.gm_port ;
00273 } else if (strncasecmp(key,"fs_port",29)==0) {
00274 worldmodel2.fs_port = atoi(value);
00275 return &worldmodel2.fs_port ;
00276 }
00277 break;
00278 case sec_sound:
00279 if (strncasecmp(key,"root",29)==0) {
00280 sound.root=value;
00281 return &sound.root;
00282 } else if (strncasecmp(key,"volume",29)==0) {
00283 if(strncasecmp(value,"mute",49)==0)
00284 sound.volume=ospkvolinfdB;
00285 else if(strncasecmp(value,"level_1",49)==0)
00286 sound.volume=ospkvol25dB;
00287 else if(strncasecmp(value,"level_2",49)==0)
00288 sound.volume=ospkvol18dB;
00289 else if(strncasecmp(value,"level_3",49)==0)
00290 sound.volume=ospkvol10dB;
00291 else
00292 sound.volume=strtol(value,NULL,0);
00293 return &sound.volume;
00294 } else if (strncasecmp(key,"sample_rate",29)==0) {
00295 sound.sample_rate = atoi(value);
00296 return &sound.sample_rate ;
00297 } else if (strncasecmp(key,"sample_bits",29)==0) {
00298 sound.sample_bits = atoi(value);
00299 return &sound.sample_bits ;
00300 } else if (strncasecmp(key,"preload",29)==0) {
00301 sound.preload.push_back(value);
00302 return &sound.preload ;
00303 }
00304 break;
00305 default:
00306 break;
00307 }
00308 return NULL;
00309 }
00310
00311 Config::section_t Config::parseSection(const char* key) {
00312 if (strncasecmp(key,"wireless",29)==0) {
00313 return sec_wireless;
00314 } else if (strncasecmp(key,"vision",29)==0) {
00315 return sec_vision;
00316 } else if (strncasecmp(key,"main",29)==0) {
00317 return sec_main;
00318 } else if (strncasecmp(key,"behaviors",29)==0) {
00319 return sec_behaviors;
00320 } else if (strncasecmp(key,"controller",29)==0) {
00321 return sec_controller;
00322 } else if (strncasecmp(key,"motion",29)==0) {
00323 return sec_motion;
00324 } else if (strncasecmp(key,"worldmodel2",29)==0) {
00325 return sec_worldmodel2;
00326 } else if (strncasecmp(key,"sound",29)==0) {
00327 return sec_sound;
00328 } else {
00329 return sec_invalid;
00330 }
00331 }
00332
00333 void Config::readConfig(const char* filename) {
00334 FILE* fp = fopen(filename, "r");
00335 char buf[80], key[30], value[50];
00336 section_t section=sec_invalid;
00337 if (fp==NULL) return;
00338
00339 bool ignoring=false;
00340 std::vector<std::string> curmodel;
00341 char rdStr[orobotdesignNAME_MAX];
00342 memset(rdStr, 0, sizeof(rdStr));
00343 if (OPENR::GetRobotDesign(rdStr) != oSUCCESS) {
00344 printf("OPENR::GetRobotDesign() failed.\n");
00345 rdStr[0]='\0';
00346 }
00347
00348 unsigned int lineno=0;
00349 while (fscanf(fp,"%79[^\n]\n", buf)!=EOF) {
00350 lineno++;
00351 if (sscanf(buf,"<%29[^>]>",key)>0) {
00352 if(key[0]=='/') {
00353 if(curmodel.size()==0) {
00354 printf("WARNING: not in a model specific section, line %d\n",lineno);
00355 continue;
00356 }
00357 bool subset=matchNoCase(&key[1],curmodel.back());
00358 bool superset=matchNoCase(curmodel.back(),&key[1]);
00359 if(subset && superset) {
00360
00361 curmodel.pop_back();
00362 } else if(superset) {
00363 while(curmodel.size()>0) {
00364
00365 curmodel.pop_back();
00366 if(!matchNoCase(curmodel.back(),&key[1]))
00367 break;
00368 }
00369 } else
00370 printf("WARNING: config model mismatch, line %d\n",lineno);
00371
00372 ignoring=false;
00373 for(unsigned int i=0; i<curmodel.size(); i++)
00374 if(!matchNoCase(rdStr,curmodel[i])) {
00375 ignoring=true;
00376 break;
00377 }
00378
00379
00380 } else {
00381 curmodel.push_back(key);
00382
00383 ignoring=ignoring || !matchNoCase(rdStr,key);
00384
00385 }
00386 } else if(!ignoring) {
00387 if (sscanf(buf,"[%29[^]]]",key)>0) {
00388 section=parseSection(key);
00389
00390 } else if (sscanf(buf,"%29[^=]=%49s",key,value)>1) {
00391
00392 setValue(section, key, value);
00393 }
00394 }
00395 }
00396 fclose(fp);
00397 }
00398
00399 bool Config::matchNoCase(const std::string& model, const std::string& pattern) {
00400 unsigned int i=0;
00401 if(i==pattern.size() && i==model.size())
00402 return true;
00403 if(i==pattern.size() || i==model.size())
00404 return false;
00405 while(pattern[i]!='*') {
00406 if(toupper(pattern[i])!=toupper(model[i]))
00407 return false;
00408 i++;
00409 if(i==pattern.size() && i==model.size())
00410 return true;
00411 if(i==pattern.size() || i==model.size())
00412 return false;
00413 }
00414 i=pattern.size()-1;
00415 unsigned int j=model.size()-1;
00416 while(pattern[i]!='*') {
00417 if(toupper(pattern[i])!=toupper(model[j]))
00418 return false;
00419 i--; j--;
00420 }
00421 return true;
00422 }
00423
00424 bool Config::extractBool(const char * value) {
00425 int i=0;
00426 while(isspace(value[i])) i++;
00427 if(strncasecmp(&value[i],"t",29)==0)
00428 return true;
00429 else if(strncasecmp(&value[i],"f",29)==0)
00430 return false;
00431 else if(strncasecmp(&value[i],"true",29)==0)
00432 return true;
00433 else if(strncasecmp(&value[i],"false",29)==0)
00434 return false;
00435 else if(strncasecmp(&value[i],"y",29)==0)
00436 return true;
00437 else if(strncasecmp(&value[i],"n",29)==0)
00438 return false;
00439 else if(strncasecmp(&value[i],"yes",29)==0)
00440 return true;
00441 else if(strncasecmp(&value[i],"no",29)==0)
00442 return false;
00443 else
00444 return atoi(value);
00445 }
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457