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 probable 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);
 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  * @return Code of mode to switch to. */
 275 int modeRecognize(CvCapture* capture) {
 276         IplImage *frame = NULL;                               // frame from camera
 277         IplImage *thresFrame = NULL;                          // thresholded frame
 278         CvMat *floatMat = NULL;                               // float <-1,1> image of the frame
 279         int sideConfig, centerConfig;                         // indices of recognized configurations
 280         double sideDist, centerDist;                          // distances between the best and second best results
 281         TDecisionBox sideBox(DECISION_BOX_SIZE, SIDEMASKSCNT);     // side solutions "averaging" decision box
 282         TDecisionBox centerBox(DECISION_BOX_SIZE, CENTERMASKSCNT); // center solutions "averaging" decision box
 283 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 284         int framesPassed = PASS_FRAMES;                       // number of passed frames before refresh
 285         int pause=0;                                          // set pause=1 to pause processing
 286 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 287         while(ORTE_CAMERA_CONTROL) {
 288
 289 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 290                 /* keyboard events handeling */
 291                 switch(cvWaitKey(10) & 0xFF) {                      // wait 10ms for an event
 292                 // stop capturing
 293                 case KEY_ESC:
 294                         return MODE_QUIT;
 295                 // skip the frame
 296                 case KEY_SPACE:
 297                         framesPassed = PASS_FRAMES;
 298                         break;
 299                 // switch to recognize mode
 300                 case KEY_R:
 301                 case KEY_r:
 302                         return MODE_REALTIME;
 303                 // (un)pause
 304                 case KEY_P:
 305                 case KEY_p:
 306                         pause = !pause;
 307                         drawPauseText(frame);
 308                         break;
 309                 // save frame to file
 310                 case KEY_S:
 311                 case KEY_s:
 312                         saveFrame(frame);
 313                         break;
 314                 // wait
 315                 case KEY_W:
 316                 case KEY_w:
 317                         return MODE_WAIT;
 318                 }
 319 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 320
 321                 // Get one frame
 322 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 323                 if(pause) cvQueryFrame(capture);
 324                 else
 325 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 326                         frame = cvQueryFrame(capture);
 327                 if(!frame) {
 328                         fprintf(stderr, "NULL frame\n");
 329                         // save error code to orte and publish it
 330                         orte.camera_result.error |= camera_ERR_NO_FRAME;
 331                         ORTEPublicationSend(orte.publication_camera_result);
 332                         usleep(100*1000);
 333                         continue;
 334                 } else if(orte.camera_result.error & camera_ERR_NO_FRAME) {
 335                         orte.camera_result.error &= ~camera_ERR_NO_FRAME;
 336                         ORTEPublicationSend(orte.publication_camera_result);
 337                 }
 338
 339 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 340                 if(pause) continue;
 341
 342                 // transform colorful image to its float <-1,1> representation
 343                 cvReleaseImage(&thresFrame);
 344                 thresFrame = cvCreateImage(cvSize(FRAME_WIDTH,FRAME_HEIGHT),IPL_DEPTH_8U,1);
 345 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 346                 // convert to float
 347                 int threshold = countThreshold(frame);
 348                 clr2float(frame, &floatMat, threshold, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
 349                 //recognize side and center configurations
 350                 sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
 351                 centerConfig = recognize(floatMat, centerMasks, CENTERMASKSCNT, &centerDist);
 352                 fprintf(stderr, "SINGLE:   thr: %3d, side: %d (%f), center: %d (%f)\n", threshold, sideConfig, sideDist, centerConfig, centerDist);
 353                 //push the results to the decision box and get "averaged" solutions from it
 354                 sideBox.insertItem(sideConfig, sideDist);
 355                 centerBox.insertItem(centerConfig, centerDist);
 356                 sideBox.decide(&sideConfig, &sideDist);
 357                 centerBox.decide(&centerConfig, &centerDist);
 358                 //publish results
 359                 orte.camera_result.side = sideConfig;
 360                 orte.camera_result.center = centerConfig;
 361                 orte.camera_result.sideDist = sideDist;
 362                 orte.camera_result.centerDist = centerDist;
 363                 ORTEPublicationSend(orte.publication_camera_result);
 364                 fprintf(stderr, "ORTE:               side: %d (%f), center: %d (%f)\n", sideConfig, sideDist, centerConfig, centerDist);
 365
 366 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 367                 displayProduct(floatMat, sideConfig, centerConfig);
 368 #elif defined (PPC_DEBUG) ///////////// PPC DEBUG SESSION //////////////
 369                 saveFrame(frame);
 370 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 371                 cvReleaseMat(&floatMat);
 372
 373 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 374                 if(framesPassed++ > PASS_FRAMES) {
 375                         framesPassed=0;
 376                         cvShowImage(WINDOW_ORIG, frame);                  // show image (! Do not release the frame !)
 377                         cvShowImage(WINDOW_THRES, thresFrame);
 378                         displayMasksSum(sideMasks[sideConfig], centerMasks[centerConfig], WINDOW_MASK);  // display selected mask
 379                         cvReleaseImage(&thresFrame);
 380                 }
 381 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 382
 383         }
 384         return MODE_WAIT;
 385 }
 386
 387 /******************************************************************************/
 388
 389 /** Waits while no action is wanted. To stop waiting set orte.camera_control.on=TRUE
 390   * or in DEBUG session press W key.
 391   * @param capture Pointer to a camera capture.
 392   * @return Code of mode to switch to. */
 393 int modeWait(CvCapture *capture) {
 394         while(!ORTE_CAMERA_CONTROL) {
 395 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 396                 //process keyboard events
 397                 switch(cvWaitKey(10) & 0xFF) {                      // wait 10ms for an event
 398                 // exit program
 399                 case KEY_ESC:
 400                         return MODE_QUIT;
 401
 402                 // stop waiting
 403                 case KEY_W:
 404                 case KEY_w:
 405                         return MODE_RECOGNIZE;
 406
 407                 }
 408 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 409                 fprintf(stderr, "waiting...\n");
 410                 sleep(1);
 411         }
 412         return MODE_RECOGNIZE;
 413 }
 414
 415 /******************************************************************************/
 416
 417 /** Switches between modes of the program.
 418  * @param defaultMode The first mode to run. */
 419 int modeManager(int defaultMode) {
 420         int mode=defaultMode;
 421         int ret;
 422         CvCapture* capture;
 423
 424         // connect to a camera
 425         while(!((capture=cvCaptureFromCAM(0)) || (capture=cvCaptureFromCAM(1)))) {
 426                 fprintf(stderr, "NULL capture (is camera connected?)\n");
 427                 orte.camera_result.error |= camera_ERR_NO_VIDEO;
 428                 ORTEPublicationSend(orte.publication_camera_result);
 429                 usleep(500*1000);
 430         }
 431         orte.camera_result.error &= ~camera_ERR_NO_VIDEO;
 432         ORTEPublicationSend(orte.publication_camera_result);
 433         fprintf(stderr, "rozkuk started, camera connected successfully\n");
 434
 435 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 436         // create gui windows and init font
 437         cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE);       // real camera video / one frame viewer
 438         cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1);  //init text font
 439 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 440
 441         while(!(mode & MODE_QUIT)) {
 442                 switch(mode & MASK_MODES) {
 443
 444 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 445                 case MODE_REALTIME:
 446                         cvDestroyWindow(WINDOW_THRES);
 447                         mode = modeRealtime(capture);
 448                         break;
 449 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 450
 451                 case MODE_RECOGNIZE:
 452 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 453                         cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
 454                         cvMoveWindow(WINDOW_THRES, cvQueryFrame(capture)->width, 0);
 455                         cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
 456                         cvMoveWindow(WINDOW_MASK, 0, cvQueryFrame(capture)->height+50);
 457                         cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
 458                         cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
 459 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 460                         //load masks and run recognition mode, then free masks (they are in memory only while in recognition mode)
 461                         if((ret=loadMasks((orte.camera_control.game_color==BLUE ? clBLUE : clYELLOW)))) return ret;
 462                         mode = modeRecognize(capture);
 463                         freeMasks();
 464                         break;
 465
 466                 case MODE_WAIT:
 467                         mode = modeWait(capture);
 468                         break;
 469
 470                 default:
 471                         goto end;    // jump out of the loop
 472                 }
 473         }
 474
 475 end:
 476         cvReleaseCapture(&capture);
 477 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 478         cvDestroyWindow(WINDOW_ORIG);
 479         cvDestroyWindow(WINDOW_THRES);
 480         cvDestroyWindow(WINDOW_MASK);
 481         cvDestroyWindow(WINDOW_PROD);
 482 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 483         return mode & MASK_ERRORS;
 484 }
 485
 486 /******************************************************************************/
 487
 488 /** Returns a threshold computed from the frame. */
 489 int countThreshold(const IplImage *frame) {
 490         return cvAvg(frame).val[0];
 491 }
 492
 493 /******************************************************************************/
 494
 495 /** Returns an ordinary number of recognized layout.
 496   * @param frame Float representation of frame.
 497   * @param masks Array of masks to compare with.
 498   * @param masksCnt Size of the array.
 499   * @param resDist If not NULL, a distance between the best and second best result is stored here.*/
 500 int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist) {
 501         double max = cvDotProduct(frame, masks[0]);
 502         double max2 = -(FRAME_SIZE);                          // the least possible value
 503         int maxInd = 0;
 504         for(int i=1; i<masksCnt; i++) {
 505                 double prod = cvDotProduct(frame, masks[i]);
 506                 if(prod > max) {
 507                         max2 = max;
 508                         max = prod;
 509                         maxInd = i;
 510                 } else if(prod > max2) max2 = prod;
 511         }
 512         if(resDist) *resDist = max-max2;
 513         return maxInd;
 514 }
 515
 516 /******************************************************************************/
 517
 518 /** Loads binary (float) data to *mask from file.
 519  * @param filename Path to file to read.
 520  * @param mask Address of array, where to alloc memory and store the data.
 521  * @return Length of read data or zero in case of error. */
 522 int loadMask(const char *filename, CvMat **mask) {
 523         int len;
 524
 525         IplImage *img = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
 526         if(!img) {
 527                 fprintf(stderr, "Mask %s cannot be loaded.\n", filename);
 528                 fprintf(stderr, "Run me from _build/user/camera/rozkuk\n");
 529                 return 0;
 530         }
 531
 532         *mask = NULL;
 533         if(!(len=clr2float(img, mask, CL_NEUTRAL, 1.0, NULL)))
 534                 fprintf(stderr, "A problem while converting mask to float occured.\n");
 535         else
 536                 fprintf(stderr, "Mask %s successfully loaded.\n", filename);
 537
 538         cvReleaseImage(&img);
 539         return len;
 540 }
 541
 542 /******************************************************************************/
 543
 544 /** Loads all the float masks from files.
 545  * @return Zero if ok, else non-zero. */
 546 int loadMasks(char color) {
 547         char filename[100];
 548         //alloc memory for arrays of masks
 549         sideMasks = (CvMat **)malloc(SIDEMASKSCNT*sizeof(CvMat *));
 550         centerMasks = (CvMat **)malloc(CENTERMASKSCNT*sizeof(CvMat *));
 551         //load masks
 552         for(int i=0; i<SIDEMASKSCNT; i++) {
 553                 sprintf(filename, MASKFILENAME, i, orSIDE, color);
 554                 if(!loadMask(filename, sideMasks+i)) return ERR_MASK_READ_FAILURE;
 555         }
 556         for(int i=0; i<CENTERMASKSCNT; i++) {
 557                 sprintf(filename, MASKFILENAME, i, orCENTER, color);
 558                 if(!loadMask(filename, centerMasks+i)) return ERR_MASK_READ_FAILURE;
 559         }
 560         return 0;
 561 }
 562
 563 /******************************************************************************/
 564
 565 /** Frees the memory alloced for masks. */
 566 void freeMasks(void) {
 567         for(int i=0; i<SIDEMASKSCNT; i++) cvReleaseMat(sideMasks+i);
 568         for(int i=0; i<CENTERMASKSCNT; i++) cvReleaseMat(centerMasks+i);
 569         free(sideMasks);
 570         free(centerMasks);
 571 }
 572
 573 /******************************************************************************/
 574
 575 /** Orte camera control callback function. */
 576 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
 577 #ifdef ORTE_DEBUG
 578         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 579 #endif /*----------- ORTE_DEBUG -----------*/
 580
 581         switch (info->status) {
 582                 case NEW_DATA:
 583                         // nothing to do - changes will be processed in the recognition loop
 584 #ifdef ORTE_DEBUG
 585                         printf("orte: New data: ctrl %d, clr %d\n", orte_data->camera_control.on, orte_data->camera_control.game_color);
 586 #endif /*----------- ORTE_DEBUG -----------*/
 587                         break;
 588                 case DEADLINE:
 589                         printf("ORTE deadline occurred - CMR_CTRL receive\n");
 590                         break;
 591         }
 592 }
 593
 594 /******************************************************************************/
 595
 596 int main(int argc, char *argv[]) {
 597         int ret;
 598
 599         ret = robottype_roboorte_init(&orte);
 600         if(ret < 0) {
 601                 fprintf(stderr, "robottype_roboorte_init failed\n");
 602                 return ret;
 603         }
 604         robottype_publisher_camera_result_create(&orte, NULL, NULL);
 605         robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
 606
 607         modeManager(START_MODE);
 608
 609         ret = robottype_roboorte_destroy(&orte);
 610         return ret;
 611 }
 612