src/camera/rozkuk/rozkuk.cxx

   1 /*******************************************************************************
   2  *
   3  * rozkuk - OpenCV based corns camera recognition program
   4  *
   5  * Created for Eurobot 2010 competition.
   6  *
   7  * Petr Kubizňák (kubiznak.petr@gmail.com), 2010
   8  *
   9  ******************************************************************************/
  10
  11 #include <inttypes.h>
  12 #include <stdlib.h>
  13 #include <stdio.h>
  14 #include <sys/stat.h>
  15 #include <semaphore.h>
  16 #include <getopt.h>
  17 #include <unistd.h>
  18
  19 #include <opencv/cv.h>
  20 #include <opencv/highgui.h>
  21
  22 #include "clr2float.h"
  23 #include "masks_globals.h"
  24 #include "t_decision_box.h"
  25
  26 extern "C" {
  27 #include <roboorte_robottype.h>
  28 #include <robot.h>
  29 }
  30 /******************************************************************************/
  31
  32 // uncomment next line to "log" the output frames - save them as pnm to the working directory
  33 //#define PPC_DEBUG
  34
  35 // modes definitions
  36 #define MODE_QUIT         0x01
  37 #define MODE_REALTIME     0x02
  38 #define MODE_RECOGNIZE    0x04
  39 #define MODE_WAIT         0x08
  40
  41 // mask of all modes / errors
  42 #define MASK_MODES        0x0F
  43 #define MASK_ERRORS       (~MASK_MODES)
  44
  45 // errors
  46 #define ERR_MASK_READ_FAILURE 0x10
  47
  48 // default mode in standard session (i.e. on ppc)
  49 #define START_MODE        MODE_WAIT
  50
  51 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
  52
  53 // default mode in debug session (i.e. on pc)
  54 #undef START_MODE
  55 #define START_MODE        MODE_REALTIME
  56
  57 // highgui windows names
  58 #define WINDOW_ORIG       "ROZKUK original scene"
  59 #define WINDOW_THRES      "ROZKUK threshold"
  60 #define WINDOW_MASK       "ROZKUK mask"
  61 #define WINDOW_PROD       "ROZKUK product"
  62
  63 // values of keys
  64 #define KEY_ESC           0x1B
  65 #define KEY_SPACE         0x20
  66 #define KEY_P             0x50
  67 #define KEY_R             0x52
  68 #define KEY_S             0x53
  69 #define KEY_W             0x57
  70 #define KEY_p             0x70
  71 #define KEY_r             0x72
  72 #define KEY_s             0x73
  73 #define KEY_w             0x77
  74
  75 // number of frames passed in rozkuk mode before refresh
  76 #define PASS_FRAMES       0
  77
  78 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
  79
  80 //default threshold and saturation
  81 #define THRESHOLD         70
  82 #define SATURATION        1.0
  83
  84 //mask filename pattern
  85 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
  86 #define MASKFILENAME      "./mask%02d%c%c.pnm"
  87 #else                     /******************** PPC *******************/
  88 #define MASKFILENAME      "./mask%02d%c%c.pnm"
  89 #endif                    /*------------------------------------------*/
  90
  91 //size of frames from camera
  92 #define FRAME_WIDTH       640
  93 #define FRAME_HEIGHT      480
  94 #define FRAME_SIZE        (FRAME_WIDTH * FRAME_HEIGHT)
  95
  96 //both (side and center) subresults will be returned in one int masked binary as 00ccssss
  97 #define MASK_SIDE         0x0F
  98 #define MASK_CENTER_SHIFT 4
  99 #define MASK_CENTER       (0x03 << 4)
 100
 101 //number of results to select the most supported one from
 102 #define DECISION_BOX_SIZE 5
 103
 104 //camera control system (if true, rozkuk recognizes, else it waits)
 105 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 106 #define ORTE_CAMERA_CONTROL 1
 107 #else                     /******************** PPC *******************/
 108 #define ORTE_CAMERA_CONTROL orte.camera_control.on
 109 #endif                    /*------------------------------------------*/
 110
 111 /******************************************************************************/
 112
 113 // function declarations
 114 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 115 int saveFrame(const CvArr *img);
 116 void drawPauseText(IplImage *img);
 117 void displayProduct(const CvMat *floatMat, int sideConfig, int centerConfig);
 118 void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName);
 119 void displayFloatMat(const CvMat *floatMat, const char *windowName);
 120 int modeRealtime(CvCapture* capture);
 121 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 122 int modeRecognize(CvCapture* capture, CORBA_octet currentColor);
 123 int modeWait(CvCapture *capture);
 124 int modeManager(int defaultMode);
 125 int countThreshold(const IplImage *frame);
 126 int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist=NULL);
 127 int loadMask(const char *filename, CvMat **mask);
 128 int loadMasks(char color);
 129 void freeMasks(void);
 130 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
 131
 132 /******************************************************************************/
 133
 134 // variable declarations
 135 struct robottype_orte_data orte;
 136 int orteError=0;
 137 CvMat **sideMasks=NULL;     //float mask matrices
 138 CvMat **centerMasks=NULL;
 139 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 140 CvFont font;                //text font
 141 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 142
 143 /******************************************************************************/
 144
 145 #if defined (ROZKUK_DEBUG) || defined (PPC_DEBUG)  /************ BOTH DEBUG SESSIONS ************/
 146
 147 /** Saves current image to a new file in the current directory. */
 148 int saveFrame(const CvArr *img) {
 149         struct stat stFileInfo;
 150         unsigned int i=0;
 151
 152         //find a non-existent filename
 153 #if defined (ROZKUK_DEBUG)
 154         char filename[30] = "snapshot00.png";
 155         while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.png", ++i);
 156 #else
 157         char filename[30] = "snapshot00.pnm";
 158         while(!stat(filename, &stFileInfo)) sprintf(filename, "snapshot%02d.pnm", ++i);
 159 #endif
 160
 161         fprintf(stdout, "Saving frame to %s...\n", filename);
 162         //save image to file
 163         return cvSaveImage(filename, img);
 164 }
 165
 166 #endif                    /*----------- BOTH DEBUG SESSIONS -----------*/
 167
 168 /******************************************************************************/
 169
 170 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 171
 172 /** Writes text "pause" in the CAMERA window. */
 173 void drawPauseText(IplImage *img) {
 174         cvPutText(img,"pause", cvPoint(300,440), &font, cvScalar(255,255,255));
 175         cvShowImage(WINDOW_ORIG, img);
 176 }
 177
 178 /******************************************************************************/
 179
 180 /** Displays in separate window the result of multiplying camera frame (in float)
 181   * and recognized mask.
 182   * @param floatMat Pointer to the camera frame converted to float matrix.
 183   * @param sideConfig Index of recognized side situation.
 184   * @param centerConfig Index of recognized center situation. */
 185 void displayProduct(const CvMat *floatMat, int sideConfig, int centerConfig) {
 186         CvMat *sideProduct = cvCloneMat(floatMat);
 187         CvMat *centerProduct = cvCloneMat(floatMat);
 188
 189         cvMul(floatMat, sideMasks[sideConfig], sideProduct);            //count product of frame and side mask
 190         cvMul(floatMat, centerMasks[centerConfig], centerProduct);      //count product of frame and center mask
 191         displayMasksSum(sideProduct, centerProduct, WINDOW_PROD);       //display sum
 192
 193         cvReleaseMat(&sideProduct);
 194         cvReleaseMat(&centerProduct);
 195 }
 196
 197 /******************************************************************************/
 198
 199 /** Displays a sum of two matrices in specified window. */
 200 void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowName) {
 201         CvMat *sum = cvCloneMat(mat1);
 202
 203         cvAdd(mat1, mat2, sum);
 204         displayFloatMat(sum, windowName);
 205
 206         cvReleaseMat(&sum);
 207 }
 208
 209 /******************************************************************************/
 210
 211 /** Normalizes the matrix values and displays them in window specified by name.
 212   * @param floatMat Pointer to matrix data to display.
 213   * @param windowName Name of window in which to display the image. */
 214 void displayFloatMat(const CvMat *floatMat, const char *windowName) {
 215         double min=0.0, max=0.0;
 216         CvMat *normalized = cvCloneMat(floatMat);
 217
 218         cvMinMaxLoc(floatMat, &min, &max);                    // find extremes
 219         cvConvertScale(floatMat, normalized, 1/(max-min), -(min/(max-min)));  // normalize
 220         cvShowImage(windowName, normalized);
 221
 222         cvReleaseMat(&normalized);
 223 }
 224
 225 /******************************************************************************/
 226
 227 /** Displays a real video in a window (only for DEBUG session).
 228  * @param capture Pointer to a camera capture.
 229  * @return Code of mode to switch to. */
 230 int modeRealtime(CvCapture* capture) {
 231         IplImage* frame = NULL;                               // frame from camera
 232         int pause=0;                                          // set pause=1 to pause the video
 233
 234         while(1) {
 235                 /* keyboard events handeling */
 236                 switch(cvWaitKey(10) & 0xFF) {                      // wait 10ms for an event
 237                 // stop capturing
 238                 case KEY_ESC:
 239                         return MODE_QUIT;
 240                 // switch to recognize mode
 241                 case KEY_R:
 242                 case KEY_r:
 243                         return MODE_RECOGNIZE;
 244                 // (un)pause
 245                 case KEY_P:
 246                 case KEY_p:
 247                         pause = !pause;
 248                         drawPauseText(frame);
 249                         break;
 250                 // save frame to file
 251                 case KEY_S:
 252                 case KEY_s:
 253                         saveFrame(frame);
 254                         break;
 255                 }
 256
 257                 frame = cvQueryFrame(capture);                      // Get one frame
 258                 if(!frame) {
 259                         fprintf(stderr, "NULL frame\n");
 260                         continue;
 261                 }
 262
 263                 if(!pause) cvShowImage(WINDOW_ORIG, frame);         // show image (! Do not release the frame !)
 264         }
 265         return MODE_QUIT;
 266 }
 267 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 268
 269 /******************************************************************************/
 270
 271 /** Implements the camera recognition of corns.
 272  * In DEBUG session the results are also displayed on screen.
 273  * @param capture Pointer to a camera capture.
 274  * @param currentColor Number of starting color. If it changes in orte during
 275  *   recognition, this mode needs to restart (to load suitable masks).
 276  * @return Code of mode to switch to. */
 277 int modeRecognize(CvCapture* capture, CORBA_octet currentColor) {
 278         IplImage *frame = NULL;                               // frame from camera
 279         IplImage *thresFrame = NULL;                          // thresholded frame
 280         CvMat *floatMat = NULL;                               // float <-1,1> image of the frame
 281         int sideConfig, centerConfig;                         // indices of recognized configurations
 282         double sideDist, centerDist;                          // distances between the best and second best results
 283         TDecisionBox sideBox(DECISION_BOX_SIZE, SIDEMASKSCNT);     // side solutions "averaging" decision box
 284         TDecisionBox centerBox(DECISION_BOX_SIZE, CENTERMASKSCNT); // center solutions "averaging" decision box
 285 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 286         int framesPassed = PASS_FRAMES;                       // number of passed frames before refresh
 287         int pause=0;                                          // set pause=1 to pause processing
 288 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 289         while(ORTE_CAMERA_CONTROL) {
 290
 291                 //if color changed during recognition, restart the mode to load appropriate masks
 292                 if(orte.camera_control.game_color != currentColor) return MODE_RECOGNIZE;
 293
 294 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 295                 /* keyboard events handeling */
 296                 switch(cvWaitKey(10) & 0xFF) {                      // wait 10ms for an event
 297                 // stop capturing
 298                 case KEY_ESC:
 299                         return MODE_QUIT;
 300                 // skip the frame
 301                 case KEY_SPACE:
 302                         framesPassed = PASS_FRAMES;
 303                         break;
 304                 // switch to recognize mode
 305                 case KEY_R:
 306                 case KEY_r:
 307                         return MODE_REALTIME;
 308                 // (un)pause
 309                 case KEY_P:
 310                 case KEY_p:
 311                         pause = !pause;
 312                         drawPauseText(frame);
 313                         break;
 314                 // save frame to file
 315                 case KEY_S:
 316                 case KEY_s:
 317                         saveFrame(frame);
 318                         break;
 319                 // wait
 320                 case KEY_W:
 321                 case KEY_w:
 322                         return MODE_WAIT;
 323                 }
 324 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 325
 326                 // Get one frame
 327 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 328                 if(pause) cvQueryFrame(capture);
 329                 else
 330 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 331                         frame = cvQueryFrame(capture);
 332                 if(!frame) {
 333                         fprintf(stderr, "NULL frame\n");
 334                         // save error code to orte and publish it
 335                         orte.camera_result.error |= camera_ERR_NO_FRAME;
 336                         ORTEPublicationSend(orte.publication_camera_result);
 337                         usleep(100*1000);
 338                         continue;
 339                 } else if(orte.camera_result.error & camera_ERR_NO_FRAME) {
 340                         orte.camera_result.error &= ~camera_ERR_NO_FRAME;
 341                         ORTEPublicationSend(orte.publication_camera_result);
 342                 }
 343
 344 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 345                 if(pause) continue;
 346
 347                 // transform colorful image to its float <-1,1> representation
 348                 cvReleaseImage(&thresFrame);
 349                 thresFrame = cvCreateImage(cvSize(FRAME_WIDTH,FRAME_HEIGHT),IPL_DEPTH_8U,1);
 350 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 351                 // convert to float
 352                 int threshold = countThreshold(frame);
 353                 clr2float(frame, &floatMat, threshold, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
 354                 //recognize side and center configurations
 355                 sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
 356                 centerConfig = recognize(floatMat, centerMasks, CENTERMASKSCNT, &centerDist);
 357                 fprintf(stderr, "SINGLE:   thr: %3d, side: %d (%.2f), center: %d (%.2f)\n", threshold, sideConfig, sideDist, centerConfig, centerDist);
 358                 //push the results to the decision box and get "averaged" solutions from it
 359                 sideBox.insertItem(sideConfig, sideDist);
 360                 centerBox.insertItem(centerConfig, centerDist);
 361                 sideBox.decide(&sideConfig, &sideDist);
 362                 centerBox.decide(&centerConfig, &centerDist);
 363                 //publish results
 364                 orte.camera_result.side = sideConfig;
 365                 orte.camera_result.center = centerConfig;
 366                 orte.camera_result.sideDist = sideDist;
 367                 orte.camera_result.centerDist = centerDist;
 368                 ORTEPublicationSend(orte.publication_camera_result);
 369                 fprintf(stderr, "ORTE:               side: %d (%.2f), center: %d (%.2f)\n", sideConfig, sideDist, centerConfig, centerDist);
 370
 371 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 372                 displayProduct(floatMat, sideConfig, centerConfig);
 373 #elif defined (PPC_DEBUG) ///////////// PPC DEBUG SESSION //////////////
 374                 saveFrame(frame);
 375 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 376                 cvReleaseMat(&floatMat);
 377
 378 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 379                 if(framesPassed++ > PASS_FRAMES) {
 380                         framesPassed=0;
 381                         cvShowImage(WINDOW_ORIG, frame);                  // show image (! Do not release the frame !)
 382                         cvShowImage(WINDOW_THRES, thresFrame);
 383                         displayMasksSum(sideMasks[sideConfig], centerMasks[centerConfig], WINDOW_MASK);  // display selected mask
 384                         cvReleaseImage(&thresFrame);
 385                 }
 386 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 387
 388         }
 389         return MODE_WAIT;
 390 }
 391
 392 /******************************************************************************/
 393
 394 /** Waits while no action is wanted. To stop waiting set orte.camera_control.on=TRUE
 395   * or in DEBUG session press W key.
 396   * @param capture Pointer to a camera capture.
 397   * @return Code of mode to switch to. */
 398 int modeWait(CvCapture *capture) {
 399         while(!ORTE_CAMERA_CONTROL) {
 400 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 401                 //process keyboard events
 402                 switch(cvWaitKey(10) & 0xFF) {                      // wait 10ms for an event
 403                 // exit program
 404                 case KEY_ESC:
 405                         return MODE_QUIT;
 406
 407                 // stop waiting
 408                 case KEY_W:
 409                 case KEY_w:
 410                         return MODE_RECOGNIZE;
 411
 412                 }
 413 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 414                 fprintf(stderr, "waiting...\n");
 415                 sleep(1);
 416         }
 417         return MODE_RECOGNIZE;
 418 }
 419
 420 /******************************************************************************/
 421
 422 /** Switches between modes of the program.
 423  * @param defaultMode The first mode to run. */
 424 int modeManager(int defaultMode) {
 425         int mode=defaultMode;
 426         int ret;
 427         CvCapture* capture;
 428         CORBA_octet color;
 429
 430         // connect to a camera
 431         while(!((capture=cvCaptureFromCAM(0)) || (capture=cvCaptureFromCAM(1)))) {
 432                 fprintf(stderr, "NULL capture (is camera connected?)\n");
 433                 orte.camera_result.error |= camera_ERR_NO_VIDEO;
 434                 ORTEPublicationSend(orte.publication_camera_result);
 435                 usleep(500*1000);
 436         }
 437         orte.camera_result.error &= ~camera_ERR_NO_VIDEO;
 438         ORTEPublicationSend(orte.publication_camera_result);
 439         fprintf(stderr, "rozkuk started, camera connected successfully\n");
 440
 441 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 442         // create gui windows and init font
 443         cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE);       // real camera video / one frame viewer
 444         cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1);  //init text font
 445 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 446
 447         while(!(mode & MODE_QUIT)) {
 448                 switch(mode & MASK_MODES) {
 449
 450 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 451                 case MODE_REALTIME:
 452                         cvDestroyWindow(WINDOW_THRES);
 453                         mode = modeRealtime(capture);
 454                         break;
 455 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 456
 457                 case MODE_RECOGNIZE:
 458 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 459                         cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
 460                         cvMoveWindow(WINDOW_THRES, cvQueryFrame(capture)->width, 0);
 461                         cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
 462                         cvMoveWindow(WINDOW_MASK, 0, cvQueryFrame(capture)->height+50);
 463                         cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
 464                         cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
 465 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 466                         //load masks and run recognition mode, then free masks (they are in memory only while in recognition mode)
 467                         color = orte.camera_control.game_color;
 468                         if((ret=loadMasks((color==BLUE ? clBLUE : clYELLOW)))) return ret;
 469                         mode = modeRecognize(capture, color);
 470                         freeMasks();
 471                         break;
 472
 473                 case MODE_WAIT:
 474                         mode = modeWait(capture);
 475                         break;
 476
 477                 default:
 478                         goto end;    // jump out of the loop
 479                 }
 480         }
 481
 482 end:
 483         cvReleaseCapture(&capture);
 484 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 485         cvDestroyWindow(WINDOW_ORIG);
 486         cvDestroyWindow(WINDOW_THRES);
 487         cvDestroyWindow(WINDOW_MASK);
 488         cvDestroyWindow(WINDOW_PROD);
 489 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 490         return mode & MASK_ERRORS;
 491 }
 492
 493 /******************************************************************************/
 494
 495 /** Returns a threshold computed from the frame. */
 496 int countThreshold(const IplImage *frame) {
 497         return cvAvg(frame).val[0];
 498 }
 499
 500 /******************************************************************************/
 501
 502 /** Returns an ordinary number of recognized layout.
 503   * @param frame Float representation of frame.
 504   * @param masks Array of masks to compare with.
 505   * @param masksCnt Size of the array.
 506   * @param resDist If not NULL, a distance between the best and second best result is stored here.*/
 507 int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist) {
 508         double max = cvDotProduct(frame, masks[0]);
 509         double max2 = -(FRAME_SIZE);                          // the least possible value
 510         int maxInd = 0;
 511         for(int i=1; i<masksCnt; i++) {
 512                 double prod = cvDotProduct(frame, masks[i]);
 513                 if(prod > max) {
 514                         max2 = max;
 515                         max = prod;
 516                         maxInd = i;
 517                 } else if(prod > max2) max2 = prod;
 518         }
 519         if(resDist) *resDist = max-max2;
 520         return maxInd;
 521 }
 522
 523 /******************************************************************************/
 524
 525 /** Loads binary (float) data to *mask from file.
 526  * @param filename Path to file to read.
 527  * @param mask Address of array, where to alloc memory and store the data.
 528  * @return Length of read data or zero in case of error. */
 529 int loadMask(const char *filename, CvMat **mask) {
 530         int len;
 531
 532         IplImage *img = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
 533         if(!img) {
 534                 fprintf(stderr, "Mask %s cannot be loaded.\n", filename);
 535                 fprintf(stderr, "Run me from _build/user/camera/rozkuk\n");
 536                 return 0;
 537         }
 538
 539         *mask = NULL;
 540         if(!(len=clr2float(img, mask, CL_NEUTRAL, 1.0, NULL)))
 541                 fprintf(stderr, "A problem while converting mask to float occured.\n");
 542         else
 543                 fprintf(stderr, "Mask %s successfully loaded.\n", filename);
 544
 545         cvReleaseImage(&img);
 546         return len;
 547 }
 548
 549 /******************************************************************************/
 550
 551 /** Loads all the float masks from files.
 552  * @return Zero if ok, else non-zero. */
 553 int loadMasks(char color) {
 554         char filename[100];
 555         //alloc memory for arrays of masks
 556         sideMasks = (CvMat **)malloc(SIDEMASKSCNT*sizeof(CvMat *));
 557         centerMasks = (CvMat **)malloc(CENTERMASKSCNT*sizeof(CvMat *));
 558         //load masks
 559         for(int i=0; i<SIDEMASKSCNT; i++) {
 560                 sprintf(filename, MASKFILENAME, i, orSIDE, color);
 561                 if(!loadMask(filename, sideMasks+i)) return ERR_MASK_READ_FAILURE;
 562         }
 563         for(int i=0; i<CENTERMASKSCNT; i++) {
 564                 sprintf(filename, MASKFILENAME, i, orCENTER, color);
 565                 if(!loadMask(filename, centerMasks+i)) return ERR_MASK_READ_FAILURE;
 566         }
 567         return 0;
 568 }
 569
 570 /******************************************************************************/
 571
 572 /** Frees the memory alloced for masks. */
 573 void freeMasks(void) {
 574         for(int i=0; i<SIDEMASKSCNT; i++) cvReleaseMat(sideMasks+i);
 575         for(int i=0; i<CENTERMASKSCNT; i++) cvReleaseMat(centerMasks+i);
 576         free(sideMasks);
 577         free(centerMasks);
 578 }
 579
 580 /******************************************************************************/
 581
 582 /** Orte camera control callback function. */
 583 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
 584 #ifdef ORTE_DEBUG
 585         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 586 #endif /*----------- ORTE_DEBUG -----------*/
 587
 588         switch (info->status) {
 589                 case NEW_DATA:
 590                         // nothing to do - changes will be processed in the recognition loop
 591 #ifdef ORTE_DEBUG
 592                         printf("orte: New data: ctrl %d, clr %d\n", orte_data->camera_control.on, orte_data->camera_control.game_color);
 593 #endif /*----------- ORTE_DEBUG -----------*/
 594                         break;
 595                 case DEADLINE:
 596                         printf("ORTE deadline occurred - CMR_CTRL receive\n");
 597                         break;
 598         }
 599 }
 600
 601 /******************************************************************************/
 602
 603 int main(int argc, char *argv[]) {
 604         int ret;
 605
 606         ret = robottype_roboorte_init(&orte);
 607         if(ret < 0) {
 608                 fprintf(stderr, "robottype_roboorte_init failed\n");
 609                 return ret;
 610         }
 611         robottype_publisher_camera_result_create(&orte, NULL, NULL);
 612         robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
 613
 614         modeManager(START_MODE);
 615
 616         ret = robottype_roboorte_destroy(&orte);
 617         return ret;
 618 }
 619