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 <cstring>
00007 #include <ctype.h>
00008 #include "Wireless/Socket.h"
00009 #ifdef PLATFORM_APERIOS
00010 # include <OPENR/OPrimitiveControl.h>
00011 # include <OPENR/OPENRAPI.h>
00012 #else
00013 #include <sys/param.h>
00014 #include <unistd.h>
00015 typedef unsigned int OSpeakerVolume;
00016 const OSpeakerVolume ospkvolinfdB = 0x8000;
00017 const OSpeakerVolume ospkvol25dB = 0xe700;
00018 const OSpeakerVolume ospkvol18dB = 0xee00;
00019 const OSpeakerVolume ospkvol10dB = 0xf600;
00020 #endif
00021
00022 Config* config=NULL;
00023
00024 void Config::setFileSystemRoot(const std::string& fsr) {
00025 #ifdef PLATFORM_APERIOS
00026 if(fsr[0]=='/')
00027 fsRoot="/ms"+fsr;
00028 else
00029 fsRoot="/ms/"+fsr;
00030 #else
00031 char buf[MAXPATHLEN+2];
00032 if(getcwd(buf,MAXPATHLEN+2)==NULL)
00033 perror("Config::setFileSystemRoot(): getcwd");
00034 buf[MAXPATHLEN+1]='\0';
00035 std::string sbuf(buf);
00036 if(sbuf[sbuf.size()-1]!='/' && fsr[0]!='/')
00037 fsRoot=sbuf+'/'+fsr;
00038 else if(sbuf[sbuf.size()-1]=='/' && fsr[0]=='/')
00039 fsRoot=sbuf+fsr.substr(1);
00040 else
00041 fsRoot=sbuf+fsr;
00042 #endif
00043 }
00044
00045
00046 std::string Config::portPath(const std::string& path) const {
00047 if(fsRoot.size()==0)
00048 return path;
00049 if(path.size()==0)
00050 return fsRoot;
00051 if(path.substr(0,fsRoot.size())==fsRoot)
00052 return path;
00053 else if(fsRoot[fsRoot.size()-1]=='/') {
00054 if(path[0]!='/')
00055 return fsRoot+path;
00056 else
00057 return fsRoot+path.substr(1);
00058 } else {
00059 if(path[0]!='/')
00060 return fsRoot+'/'+path;
00061 else
00062 return fsRoot+path;
00063 }
00064 }
00065
00066 void* Config::setValue(section_t section, const char *key, const char *value, bool ) {
00067 switch (section) {
00068 case sec_wireless:
00069 if (strncasecmp(key,"id",29)==0) {
00070 wireless.id=atoi(value);
00071 return &wireless.id;
00072 }
00073 break;
00074 case sec_vision:
00075 if (strncasecmp(key,"white_balance",29)==0) {
00076 if (strncasecmp(value,"indoor",49)==0) {
00077 vision.white_balance=1;
00078 } else if (strncasecmp(value,"flourescent",49)==0) {
00079 vision.white_balance=3;
00080 } else if (strncasecmp(value,"outdoor",49)==0) {
00081 vision.white_balance=2;
00082 }
00083 #ifdef PLATFORM_APERIOS
00084
00085 OPrimitiveID fbkID = 0;
00086 if(OPENR::OpenPrimitive(CameraLocator, &fbkID) != oSUCCESS){
00087 std::cout << "Open FbkImageSensor failure." << std::endl;
00088 } else {
00089 OPrimitiveControl_CameraParam owb(vision.white_balance);
00090 if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_WHITE_BALANCE, &owb, sizeof(owb), 0, 0) != oSUCCESS){
00091 std::cout << "CAM_SET_WHITE_BALANCE : Failed!" << std::endl;
00092 }
00093 OPENR::ClosePrimitive(fbkID);
00094 }
00095 #endif
00096 return &vision.white_balance;
00097 } else if (strncasecmp(key,"gain",29)==0) {
00098 if (strncasecmp(value,"low",49)==0) {
00099 vision.gain=1;
00100 } else if (strncasecmp(value,"mid",49)==0) {
00101 vision.gain=2;
00102 } else if (strncasecmp(value,"high",49)==0) {
00103 vision.gain=3;
00104 }
00105 #ifdef PLATFORM_APERIOS
00106
00107 OPrimitiveID fbkID = 0;
00108 if(OPENR::OpenPrimitive(CameraLocator, &fbkID) != oSUCCESS){
00109 std::cout << "Open FbkImageSensor failure." << std::endl;
00110 } else {
00111 OPrimitiveControl_CameraParam ogain(vision.gain);
00112 if(OPENR::ControlPrimitive(fbkID, oprmreqCAM_SET_GAIN, &ogain, sizeof(ogain), 0, 0) != oSUCCESS)
00113 std::cout << "CAM_SET_GAIN : Failed!" << std::endl;
00114 OPENR::ClosePrimitive(fbkID);
00115 }
00116 #endif
00117 return &vision.gain;
00118 } else if (strncasecmp(key,"shutter_speed",29)==0) {
00119 if (strncasecmp(value,"slow",49)==0) {
00120 vision.shutter_speed=1;
00121 } else if (strncasecmp(value,"mid",49)==0) {
00122 vision.shutter_speed=2;
00123 } else if (strncasecmp(value,"fast",49)==0) {
00124 vision.shutter_speed=3;
00125 }
00126 #ifdef PLATFORM_APERIOS
00127
00128 OPrimitiveID fbkID = 0;
00129 if(OPENR::OpenPrimitive(CameraLocator, &fbkID) != oSUCCESS){
00130 std::cout << "Open FbkImageSensor failure." << std::endl;
00131 } else {
00132 OPrimitiveControl_CameraParam oshutter(vision.shutter_speed);
00133 if(OPENR::ControlPrimitive(fbkID,oprmreqCAM_SET_SHUTTER_SPEED, &oshutter, sizeof(oshutter), 0, 0) != oSUCCESS)
00134 std::cout << "CAM_SET_SHUTTER_SPEED : Failed!" << std::endl;
00135 OPENR::ClosePrimitive(fbkID);
00136 }
00137 #endif
00138 return &vision.shutter_speed;
00139 } else if (strncasecmp(key,"resolution",29)==0) {
00140 if (strncasecmp(value,"full",49)==0) {
00141 vision.resolution=1;
00142 } else if (strncasecmp(value,"half",49)==0) {
00143 vision.resolution=2;
00144 } else if (strncasecmp(value,"quarter",49)==0) {
00145 vision.resolution=3;
00146 }
00147 return &vision.resolution;
00148 } else if (strncasecmp(key,"thresh",29)==0) {
00149 vision.thresh.push_back(value);
00150 return &vision.thresh;
00151 } else if (strncasecmp(key,"colors",29)==0) {
00152 strncpy(vision.colors,value,49);
00153 return &vision.colors;
00154 } else if (strncasecmp(key,"rawcam_port",29)==0 || strncasecmp(key,"raw_port",29)==0) {
00155 if(strncasecmp(key,"raw_port",29)==0)
00156 std::cout << "Your tekkotsu.cfg file uses deprecated raw_port -- use rawcam_port instead" << std::endl;
00157 vision.rawcam_port=atoi(value);
00158 return &vision.rawcam_port;
00159 } else if (strncasecmp(key,"rawcam_transport",29)==0 || strncasecmp(key,"raw_transport",29)==0) {
00160 if(strncasecmp(key,"raw_transport",29)==0)
00161 std::cout << "Your tekkotsu.cfg file uses deprecated raw_transport -- use rawcam_transport instead" << std::endl;
00162 if (strncasecmp(value,"udp",49)==0)
00163 vision.rawcam_transport=0;
00164 else if (strncasecmp(value,"tcp",49)==0)
00165 vision.rawcam_transport=1;
00166 return &vision.rawcam_transport;
00167 } else if (strncasecmp(key,"rawcam_interval",29)==0) {
00168 vision.rawcam_interval=(unsigned int)atoi(value);
00169 return &vision.rawcam_interval;
00170 } else if (strncasecmp(key,"rle_port",29)==0) {
00171 vision.rle_port=atoi(value);
00172 return &vision.rle_port;
00173 } else if (strncasecmp(key,"rle_transport",29)==0) {
00174 if (strncasecmp(value,"udp",49)==0)
00175 vision.rle_transport=0;
00176 else if (strncasecmp(value,"tcp",49)==0)
00177 vision.rle_transport=1;
00178 return &vision.rle_transport;
00179 } else if (strncasecmp(key,"rle_interval",29)==0) {
00180 vision.rle_interval=(unsigned int)atoi(value);
00181 return &vision.rle_interval;
00182 } else if (strncasecmp(key,"region_port",29)==0) {
00183 vision.region_port=atoi(value);
00184 return &vision.region_port;
00185 } else if (strncasecmp(key,"region_transport",29)==0) {
00186 if (strncasecmp(value,"udp",49)==0)
00187 vision.region_transport=0;
00188 else if (strncasecmp(value,"tcp",49)==0)
00189 vision.region_transport=1;
00190 return &vision.region_transport;
00191 } else if (strncasecmp(key,"obj_port",29)==0) {
00192 vision.obj_port=atoi(value);
00193 return &vision.obj_port;
00194 } else if (strncasecmp(key,"restore_image",29)==0) {
00195 vision.restore_image=atoi(value);
00196 return &vision.restore_image;
00197 } else if (strncasecmp(key,"jpeg_dct_method",29)==0) {
00198 if (strncasecmp(value,"islow",49)==0) {
00199 vision.jpeg_dct_method=JDCT_ISLOW;
00200 } else if (strncasecmp(value,"ifast",49)==0) {
00201 vision.jpeg_dct_method=JDCT_IFAST;
00202 } else if (strncasecmp(value,"float",49)==0) {
00203 vision.jpeg_dct_method=JDCT_FLOAT;
00204 }
00205 return &vision.jpeg_dct_method;
00206 } else if (strncasecmp(key,"rawcam_encoding",29)==0) {
00207 if (strncasecmp(value,"color",49)==0) {
00208 vision.rawcam_encoding=vision_config::ENCODE_COLOR;
00209 vision.rawcam_channel=RawCameraGenerator::CHAN_Y;
00210 } else if (strncasecmp(value,"y_only",49)==0) {
00211 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00212 vision.rawcam_channel=RawCameraGenerator::CHAN_Y;
00213 } else if (strncasecmp(value,"uv_only",49)==0) {
00214 vision.rawcam_encoding=vision_config::ENCODE_COLOR;
00215 vision.rawcam_channel=-1;
00216 } else if (strncasecmp(value,"u_only",49)==0) {
00217 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00218 vision.rawcam_channel=RawCameraGenerator::CHAN_U;
00219 } else if (strncasecmp(value,"v_only",49)==0) {
00220 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00221 vision.rawcam_channel=RawCameraGenerator::CHAN_V;
00222 } else if (strncasecmp(value,"y_dx_only",49)==0) {
00223 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00224 vision.rawcam_channel=RawCameraGenerator::CHAN_Y_DX;
00225 } else if (strncasecmp(value,"y_dy_only",49)==0) {
00226 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00227 vision.rawcam_channel=RawCameraGenerator::CHAN_Y_DY;
00228 } else if (strncasecmp(value,"y_dxdy_only",49)==0) {
00229 vision.rawcam_encoding=vision_config::ENCODE_SINGLE_CHANNEL;
00230 vision.rawcam_channel=RawCameraGenerator::CHAN_Y_DXDY;
00231 }
00232 return &vision.rawcam_encoding;
00233 } else if (strncasecmp(key,"rawcam_compression",29)==0) {
00234 if (strncasecmp(value,"none",49)==0) {
00235 vision.rawcam_compression=vision_config::COMPRESS_NONE;
00236 } else if (strncasecmp(value,"jpeg",49)==0) {
00237 vision.rawcam_compression=vision_config::COMPRESS_JPEG;
00238 }
00239 return &vision.rawcam_compression;
00240 } else if (strncasecmp(key,"rawcam_compress_quality",29)==0) {
00241 vision.rawcam_compress_quality=atoi(value);
00242 return &vision.rawcam_compress_quality;
00243 } else if (strncasecmp(key,"rawcam_y_skip",29)==0) {
00244 vision.rawcam_y_skip=atoi(value);
00245 return &vision.rawcam_y_skip;
00246 } else if (strncasecmp(key,"rawcam_uv_skip",29)==0) {
00247 vision.rawcam_uv_skip=atoi(value);
00248 return &vision.rawcam_uv_skip;
00249 } else if (strncasecmp(key,"rlecam_skip",29)==0) {
00250 vision.rlecam_skip=atoi(value);
00251 return &vision.rlecam_skip;
00252 } else if (strncasecmp(key,"rlecam_channel",29)==0) {
00253 vision.rlecam_channel=atoi(value);
00254 return &vision.rlecam_channel;
00255 } else if (strncasecmp(key,"rlecam_compression",29)==0) {
00256 if (strncasecmp(value,"none",49)==0) {
00257 vision.rlecam_compression=vision_config::COMPRESS_NONE;
00258 } else if (strncasecmp(value,"rle",49)==0) {
00259 vision.rlecam_compression=vision_config::COMPRESS_RLE;
00260 }
00261 return &vision.rlecam_compression;
00262 } else if (strncasecmp(key,"regioncam_skip",29)==0) {
00263 vision.regioncam_skip=atoi(value);
00264 return &vision.regioncam_skip;
00265 } else if (strncasecmp(key,"focal_len_x",29)==0) {
00266 vision.focal_len_x=atof(value);
00267 return &vision.focal_len_x;
00268 } else if (strncasecmp(key,"focal_len_y",29)==0) {
00269 vision.focal_len_y=atof(value);
00270 return &vision.focal_len_y;
00271 } else if (strncasecmp(key,"principle_point_x",29)==0) {
00272 vision.principle_point_x=atof(value);
00273 return &vision.principle_point_x;
00274 } else if (strncasecmp(key,"principle_point_y",29)==0) {
00275 vision.principle_point_y=atof(value);
00276 return &vision.principle_point_y;
00277 } else if (strncasecmp(key,"skew",29)==0) {
00278 vision.skew=atof(value);
00279 return &vision.skew;
00280 } else if (strncasecmp(key,"kc1_r2",29)==0) {
00281 vision.kc1_r2=atof(value);
00282 return &vision.kc1_r2;
00283 } else if (strncasecmp(key,"kc2_r4",29)==0) {
00284 vision.kc2_r4=atof(value);
00285 return &vision.kc2_r4;
00286 } else if (strncasecmp(key,"kc5_r6",29)==0) {
00287 vision.kc5_r6=atof(value);
00288 return &vision.kc5_r6;
00289 } else if (strncasecmp(key,"kc3_tan1",29)==0) {
00290 vision.kc3_tan1=atof(value);
00291 return &vision.kc3_tan1;
00292 } else if (strncasecmp(key,"kc4_tan2",29)==0) {
00293 vision.kc4_tan2=atof(value);
00294 return &vision.kc4_tan2;
00295 } else if (strncasecmp(key,"calibration_res_x",29)==0) {
00296 vision.calibration_res_x=atoi(value);
00297 return &vision.kc4_tan2;
00298 } else if (strncasecmp(key,"calibration_res_y",29)==0) {
00299 vision.calibration_res_y=atoi(value);
00300 return &vision.calibration_res_y;
00301 }
00302 break;
00303 case sec_main:
00304 if (strncasecmp(key,"seed_rng",29)==0) {
00305 main.seed_rng=atoi(value);
00306 return &main.console_port;
00307 } else if (strncasecmp(key,"console_port",29)==0) {
00308 main.console_port=atoi(value);
00309 return &main.console_port;
00310 } else if (strncasecmp(key,"stderr_port",29)==0) {
00311 main.stderr_port=atoi(value);
00312 return &main.stderr_port;
00313 } else if (strncasecmp(key,"error_level",29)==0) {
00314 main.error_level=atoi(value);
00315 return &main.error_level;
00316 } else if (strncasecmp(key,"debug_level",29)==0) {
00317 main.debug_level=atoi(value);
00318 return &main.debug_level;
00319 } else if (strncasecmp(key,"verbose_level",29)==0) {
00320 main.verbose_level=atoi(value);
00321 return &main.verbose_level;
00322 } else if (strncasecmp(key,"wsjoints_port",29)==0) {
00323 main.wsjoints_port=atoi(value);
00324 return &main.wsjoints_port;
00325 } else if (strncasecmp(key,"wspids_port",29)==0) {
00326 main.wspids_port=atoi(value);
00327 return &main.wspids_port;
00328 } else if (strncasecmp(key,"headControl_port",29)==0) {
00329 main.headControl_port=atoi(value);
00330 return &main.headControl_port;
00331 } else if (strncasecmp(key,"walkControl_port",29)==0) {
00332 main.walkControl_port=atoi(value);
00333 return &main.walkControl_port;
00334 } else if (strncasecmp(key,"estopControl_port",29)==0) {
00335 main.estopControl_port=atoi(value);
00336 return &main.estopControl_port;
00337 } else if (strncasecmp(key,"aibo3d_port",29)==0) {
00338 main.aibo3d_port=atoi(value);
00339 return &main.aibo3d_port;
00340 } else if (strncasecmp(key,"wmmonitor_port",29)==0) {
00341 main.wmmonitor_port=atoi(value);
00342 return &main.wmmonitor_port;
00343 } else if (strncasecmp(key,"use_VT100",29)==0) {
00344 main.use_VT100=extractBool(value);
00345 return &main.use_VT100;
00346 }
00347 break;
00348 case sec_behaviors:
00349 if (strncasecmp(key,"flash_bytes",29)==0) {
00350 behaviors.flash_bytes = atoi(value);
00351 return &behaviors.flash_bytes;
00352 } else if (strncasecmp(key,"flash_on_start",29)==0) {
00353 behaviors.flash_on_start = atoi(value);
00354 return &behaviors.flash_on_start;
00355 }
00356 break;
00357 case sec_controller:
00358 if (strncasecmp(key,"gui_port",29)==0) {
00359 controller.gui_port = atoi(value);
00360 return &controller.gui_port ;
00361 } else if (strncasecmp(key,"select_snd",29)==0) {
00362 strncpy(controller.select_snd,value,49);
00363 return &controller.select_snd;
00364 } else if (strncasecmp(key,"next_snd",29)==0) {
00365 strncpy(controller.next_snd,value,49);
00366 return &controller.next_snd;
00367 } else if (strncasecmp(key,"prev_snd",29)==0) {
00368 strncpy(controller.prev_snd,value,49);
00369 return &controller.prev_snd;
00370 } else if (strncasecmp(key,"read_snd",29)==0) {
00371 strncpy(controller.read_snd,value,49);
00372 return &controller.read_snd;
00373 } else if (strncasecmp(key,"cancel_snd",29)==0) {
00374 strncpy(controller.cancel_snd,value,49);
00375 return &controller.cancel_snd;
00376 } else if (strncasecmp(key,"error_snd",29)==0) {
00377 strncpy(controller.error_snd,value,49);
00378 return &controller.error_snd;
00379 }
00380 break;
00381 case sec_motion:
00382 if (strncasecmp(key,"root",29)==0) {
00383 motion.root=value;
00384 return &motion.root;
00385 } else if (strncasecmp(key,"walk",29)==0) {
00386 motion.walk=value;
00387 return &motion.walk;
00388 } else if (strncasecmp(key,"kinematics",29)==0) {
00389 motion.kinematics=value;
00390 return &motion.walk;
00391 } else if (strncasecmp(key,"kinematic_chains",29)==0) {
00392 motion.kinematic_chains.push_back(value);
00393 return &motion.kinematic_chains;
00394 } else if (strncasecmp(key,"calibrate:",10)==0) {
00395 for(unsigned int i=PIDJointOffset; i<PIDJointOffset+NumPIDJoints; i++)
00396 if(strncasecmp(&key[10],outputNames[i],outputNameLen+1)==0) {
00397 motion.calibration[i-PIDJointOffset] = atof(value);
00398 return &motion.calibration[i];
00399 }
00400 std::cout << "WARNING: Could not match '" << (strlen(key)>10?&key[10]:key) << "' as calibration parameter" << std::endl;
00401 return NULL;
00402 } else if (strncasecmp(key,"estop_on_snd",29)==0) {
00403 strncpy(motion.estop_on_snd,value,49);
00404 return &motion.estop_on_snd;
00405 } else if (strncasecmp(key,"estop_off_snd",29)==0) {
00406 strncpy(motion.estop_off_snd,value,49);
00407 return &motion.estop_off_snd;
00408 } else if (strncasecmp(key,"max_head_tilt_speed",29)==0) {
00409 motion.max_head_tilt_speed=atof(value);
00410 return &motion.max_head_tilt_speed;
00411 } else if (strncasecmp(key,"max_head_pan_speed",29)==0) {
00412 motion.max_head_pan_speed=atof(value);
00413 return &motion.max_head_pan_speed;
00414 } else if (strncasecmp(key,"max_head_roll_speed",29)==0) {
00415 motion.max_head_roll_speed=atof(value);
00416 return &motion.max_head_roll_speed;
00417 } else if (strncasecmp(key,"inf_walk_accel",29)==0) {
00418 motion.inf_walk_accel = atoi(value);
00419 return &motion.inf_walk_accel;
00420 } else if (strncasecmp(key,"console_port",29)==0) {
00421 motion.console_port = atoi(value);
00422 return &motion.console_port;
00423 } else if (strncasecmp(key,"stderr_port",29)==0) {
00424 motion.stderr_port = atoi(value);
00425 return &motion.stderr_port ;
00426 }
00427 break;
00428 case sec_sound:
00429 if (strncasecmp(key,"root",29)==0) {
00430 sound.root=value;
00431 return &sound.root;
00432 } else if (strncasecmp(key,"volume",29)==0) {
00433 if(strncasecmp(value,"mute",49)==0)
00434 sound.volume=ospkvolinfdB;
00435 else if(strncasecmp(value,"level_1",49)==0)
00436 sound.volume=ospkvol25dB;
00437 else if(strncasecmp(value,"level_2",49)==0)
00438 sound.volume=ospkvol18dB;
00439 else if(strncasecmp(value,"level_3",49)==0)
00440 sound.volume=ospkvol10dB;
00441 else
00442 sound.volume=strtol(value,NULL,0);
00443 return &sound.volume;
00444 } else if (strncasecmp(key,"sample_rate",29)==0) {
00445 sound.sample_rate = atoi(value);
00446 return &sound.sample_rate ;
00447 } else if (strncasecmp(key,"sample_bits",29)==0) {
00448 sound.sample_bits = atoi(value);
00449 return &sound.sample_bits ;
00450 } else if (strncasecmp(key,"preload",29)==0) {
00451 sound.preload.push_back(value);
00452 return &sound.preload ;
00453 } else if (strncasecmp(key,"streaming.mic_port",29)==0) {
00454 sound.streaming.mic_port = atoi(value);
00455 return &sound.streaming.mic_port;
00456 } else if (strncasecmp(key,"streaming.mic_sample_rate",29)==0) {
00457 sound.streaming.mic_sample_rate = atoi(value);
00458 return &sound.streaming.mic_sample_rate;
00459 } else if (strncasecmp(key,"streaming.mic_sample_bits",29)==0) {
00460 sound.streaming.mic_sample_bits = atoi(value);
00461 return &sound.streaming.mic_sample_bits;
00462 } else if (strncasecmp(key,"streaming.mic_stereo",29)==0) {
00463 sound.streaming.mic_stereo = extractBool(value);
00464 return &sound.streaming.mic_stereo;
00465 } else if (strncasecmp(key,"streaming.speaker_port",29)==0) {
00466 sound.streaming.speaker_port = atoi(value);
00467 return &sound.streaming.speaker_port;
00468 } else if (strncasecmp(key,"streaming.speaker_frame_length",30)==0) {
00469 sound.streaming.speaker_frame_length = atoi(value);
00470 return &sound.streaming.speaker_frame_length;
00471 } else if (strncasecmp(key,"streaming.speaker_max_delay",29)==0) {
00472 sound.streaming.speaker_max_delay = atoi(value);
00473 return &sound.streaming.speaker_max_delay;
00474 }
00475 break;
00476 default:
00477 break;
00478 }
00479 return NULL;
00480 }
00481
00482 Config::section_t Config::parseSection(const char* key) {
00483 if (strncasecmp(key,"wireless",29)==0) {
00484 return sec_wireless;
00485 } else if (strncasecmp(key,"vision",29)==0) {
00486 return sec_vision;
00487 } else if (strncasecmp(key,"main",29)==0) {
00488 return sec_main;
00489 } else if (strncasecmp(key,"behaviors",29)==0) {
00490 return sec_behaviors;
00491 } else if (strncasecmp(key,"controller",29)==0) {
00492 return sec_controller;
00493 } else if (strncasecmp(key,"motion",29)==0) {
00494 return sec_motion;
00495 } else if (strncasecmp(key,"sound",29)==0) {
00496 return sec_sound;
00497 } else {
00498 return sec_invalid;
00499 }
00500 }
00501
00502 void Config::readConfig(const std::string& filename) {
00503 FILE* fp = fopen(filename.c_str(), "r");
00504 char buf[80], key[30], value[50];
00505 section_t section=sec_invalid;
00506 if (fp==NULL) return;
00507
00508 bool ignoring=false;
00509 std::vector<std::string> curmodel;
00510 #ifdef PLATFORM_APERIOS
00511 char rdStr[orobotdesignNAME_MAX + 1];
00512 memset(rdStr, 0, sizeof(rdStr));
00513 if (OPENR::GetRobotDesign(rdStr) != oSUCCESS) {
00514 printf("OPENR::GetRobotDesign() failed.\n");
00515 rdStr[0]='\0';
00516 }
00517 #else
00518 # if TGT_ERS7
00519 char rdStr[]="ERS-7";
00520 # elif TGT_ERS210
00521 char rdStr[]="ERS-210";
00522 # elif TGT_ERS220
00523 char rdStr[]="ERS-220";
00524 # elif TGT_ERS2xx
00525 # warning "TGT_2xx is not specific for simulation purposes - defaulting to ERS210"
00526 char rdStr[]="ERS-210";
00527 # else
00528 # warning "TGT_<model> undefined - defaulting to ERS7"
00529 char rdStr[]="ERS-7";
00530 # endif
00531 #endif
00532
00533
00534 unsigned int lineno=0;
00535 while (fscanf(fp,"%79[^\n]\n", buf)!=EOF) {
00536 lineno++;
00537 if (sscanf(buf,"<%29[^>]>",key)>0) {
00538 if(key[0]=='/') {
00539 if(curmodel.size()==0) {
00540 printf("WARNING: not in a model specific section, line %d\n",lineno);
00541 continue;
00542 }
00543 bool subset=matchNoCase(&key[1],curmodel.back());
00544 bool superset=matchNoCase(curmodel.back(),&key[1]);
00545 if(subset && superset) {
00546
00547 curmodel.pop_back();
00548 } else if(superset) {
00549 while(curmodel.size()>0) {
00550
00551 curmodel.pop_back();
00552 if(!matchNoCase(curmodel.back(),&key[1]))
00553 break;
00554 }
00555 } else
00556 printf("WARNING: config model mismatch, line %d\n",lineno);
00557
00558 ignoring=false;
00559 for(unsigned int i=0; i<curmodel.size(); i++)
00560 if(!matchNoCase(rdStr,curmodel[i])) {
00561 ignoring=true;
00562 break;
00563 }
00564
00565
00566 } else {
00567 curmodel.push_back(key);
00568
00569 ignoring=ignoring || !matchNoCase(rdStr,key);
00570
00571 }
00572 } else if(!ignoring) {
00573 if (sscanf(buf,"[%29[^]]]",key)>0) {
00574 section=parseSection(key);
00575
00576 } else if (sscanf(buf,"%29[^=]=%49s",key,value)>1) {
00577
00578 setValue(section, key, value);
00579 }
00580 }
00581 }
00582 fclose(fp);
00583 }
00584
00585 bool Config::matchNoCase(const std::string& model, const std::string& pattern) {
00586 unsigned int i=0;
00587 if(i==pattern.size() && i==model.size())
00588 return true;
00589 if(i==pattern.size() || i==model.size())
00590 return false;
00591 while(pattern[i]!='*') {
00592 if(toupper(pattern[i])!=toupper(model[i]))
00593 return false;
00594 i++;
00595 if(i==pattern.size() && i==model.size())
00596 return true;
00597 if(i==pattern.size() || i==model.size())
00598 return false;
00599 }
00600 i=pattern.size()-1;
00601 unsigned int j=model.size()-1;
00602 while(pattern[i]!='*') {
00603 if(toupper(pattern[i])!=toupper(model[j]))
00604 return false;
00605 i--; j--;
00606 }
00607 return true;
00608 }
00609
00610 bool Config::extractBool(const char * value) {
00611 int i=0;
00612 while(isspace(value[i])) i++;
00613 if(strncasecmp(&value[i],"t",29)==0)
00614 return true;
00615 else if(strncasecmp(&value[i],"f",29)==0)
00616 return false;
00617 else if(strncasecmp(&value[i],"true",29)==0)
00618 return true;
00619 else if(strncasecmp(&value[i],"false",29)==0)
00620 return false;
00621 else if(strncasecmp(&value[i],"y",29)==0)
00622 return true;
00623 else if(strncasecmp(&value[i],"n",29)==0)
00624 return false;
00625 else if(strncasecmp(&value[i],"yes",29)==0)
00626 return true;
00627 else if(strncasecmp(&value[i],"no",29)==0)
00628 return false;
00629 else
00630 return atoi(value);
00631 }
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643