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