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