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