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