src/camera/rozkuk/rozkuk.cxx

   1 /*******************************************************************************
   2  *
   3  * rozkuk - OpenCV based camera recognition program.
   4  *
   5  * Created for Eurobot 2010 competition.
   6  *
   7  * Petr Kubizňák (kubiznak.petr@gmail.com), 2010
   8  *
   9  * This program is used to recognize configurations of playing elements (corns)
  10  * in Eurobot 2010 competition. Firstly external masks (generated by maskgen)
  11  * are loaded from image files. Those masks are then compared with frames
  12  * received from robot's camera and appropriate configuration is choosed.
  13  * Program can switch between several modes, also depending on platform - on PPC,
  14  * only modes RECOGNIZE and WAIT are present, on PC, there are also VIDEO and
  15  * IMAGE modes. Lets introduce them:
  16  *  RECOGNIZE: Camera image is processed and configuration is recognized.
  17  *    The results are published continually using orte. On PC, the result and
  18  *    some subresults are displayed graphically.
  19  *  WAIT: Program waits until orte.camera_control.on is set to true, or, on PC,
  20  *    waiting is interrupted by W key. This is the default mode for PPC.
  21  *  VIDEO (PC only): Video from camera is just displayed in a window. Default
  22  *    mode for PC. Use R key to switch to RECOGNIZE mode.
  23  *  IMAGE (PC only): Pseudo-mode used to analyze *one* image loaded from file.
  24  *    Is useful also for finding a good threshold value, using +/- keys. To use
  25  *    this mode, start the program with -i argument (see below). There is no way
  26  *    to switch from this mode to another one.
  27  *
  28  * +++ PC +++
  29  * Usage: ./rozkuk [-i filename]
  30  *  -i:
  31  *   Start in image mode, load filename to analyze.
  32  *
  33  * +++ PPC +++
  34  * Usage: ./rozkuk
  35  *   No arguments possible.
  36  *
  37  ******************************************************************************/
  38
  39 #ifdef ROZKUK_DEBUG
  40 #       define WITH_GUI
  41 #endif
  42
  43 /******************************************************************************/
  44
  45 #include <inttypes.h>
  46 #include <stdlib.h>
  47 #include <stdio.h>
  48 #include <sys/stat.h>
  49 #include <semaphore.h>
  50 #include <getopt.h>
  51 #include <unistd.h>
  52
  53 #include <opencv/cv.h>
  54 #include <opencv/highgui.h>
  55
  56 #include "clr2float.h"
  57 #include "masks_globals.h"
  58 #include "t_decision_box.h"
  59
  60 extern "C" {
  61 #include <roboorte_robottype.h>
  62 #include <robot.h>
  63 }
  64
  65 /******************************************************************************/
  66 /***************************** macro definitions ******************************/
  67 /******************************************************************************/
  68
  69 //uncomment next line to "log" the output frames - save them as pnm to the working directory
  70 //#define PPC_DEBUG
  71
  72 // modes definitions
  73 #define MODE_QUIT         0x01
  74 #define MODE_VIDEO        0x02
  75 #define MODE_RECOGNIZE    0x04
  76 #define MODE_WAIT         0x08
  77 #define MODE_IMAGE        0x10
  78
  79 // highgui windows names
  80 #define WINDOW_ORIG       "ROZKUK original scene"
  81 #define WINDOW_THRES      "ROZKUK threshold"
  82 #define WINDOW_MASK       "ROZKUK mask"
  83 #define WINDOW_PROD       "ROZKUK product"
  84
  85 // size of graphical windows
  86 #define WINDOW_WIDTH      640
  87 #define WINDOW_HEIGHT     480
  88
  89 // values of keys
  90 #define KEY_ESC           0x1B
  91 #define KEY_SPACE         0x20
  92 #define KEY_P             0x50
  93 #define KEY_R             0x52
  94 #define KEY_S             0x53
  95 #define KEY_V             0x56
  96 #define KEY_W             0x57
  97 #define KEY_p             0x70
  98 #define KEY_r             0x72
  99 #define KEY_s             0x73
 100 #define KEY_v             0x76
 101 #define KEY_w             0x77
 102 #define KEY_PLUS         -0x55
 103 #define KEY_MINUS        -0x53
 104
 105 // default threshold and saturation
 106 #define SATURATION        1.0
 107 #define THRESHOLD_SHIFT   -25                   //automatic threshold correction
 108
 109 // filename pattern of snapshots (PPC does not support png)
 110 #ifdef WITH_GUI
 111 #       define SNAPSHOTFILENAME  "snapshot%02d.png"
 112 #else /* PPC */
 113 #       define SNAPSHOTFILENAME  "snapshot%02d.pnm"
 114 #endif /* WITH_GUI */
 115
 116 //number of results to select the most supported one from
 117 #define DECISION_BOX_SIZE 3
 118
 119
 120 /******************************************************************************/
 121 /************************** function declarations *****************************/
 122 /******************************************************************************/
 123
 124 /********************** multimode graphical functions *************************/
 125 int saveFrame(const CvArr *img);
 126 void displayProduct(const CvMat *floatMat, int sideConfig, int centerConfig);
 127 void displayMatSum(const CvMat *mat1, const CvMat *mat2, const char *windowName);
 128 void displayFloatMat(const CvMat *floatMat, const char *windowName);
 129
 130 /******************** multimode computational functions ***********************/
 131 int countThreshold(const IplImage *frame);
 132 int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist=NULL);
 133
 134 /********************************** MODE_IMAGE ********************************/
 135 int modeImage(char *imageFilename);
 136
 137 /********************************* MODE_VIDEO *********************************/
 138 void drawPauseText(IplImage *img);
 139 int modeVideo(CvCapture* capture);
 140
 141 /****************************** MODE_RECOGNIZE ********************************/
 142 int recognizeMode_processKeys(IplImage *frame);
 143 void displayFrames(IplImage *frame, IplImage **thrFrame, CvMat *fMat,
 144                 int cfgSide, int cfgCenter);
 145 int modeRecognize(CvCapture* capture, CORBA_octet currentColor);
 146
 147 /********************************* MODE_WAIT **********************************/
 148 int waitMode_processKeys(int previousMode);
 149 int modeWait(int previousMode);
 150
 151 /******************************** mode manager ********************************/
 152 void setAnalyticWindowsVisible(bool visible);
 153 void destroyGUI(void);
 154 int modeManager(int defaultMode, char *paramC=NULL);
 155
 156 /******************************* masks handling *******************************/
 157 int loadMask(const char *filename, CvMat **mask);
 158 int loadMasks(char color);
 159 void freeMasks(void);
 160
 161 /*********************************** orte *************************************/
 162 bool getCameraControlOn(void);
 163 CORBA_octet getRobotSwitchesColor(void);
 164 void send_cmr_res_cb(const ORTESendInfo *info, void *vinstance, void *recvCallBackParam);
 165 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
 166 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
 167
 168 /********************************* application ********************************/
 169 int getParamI(int argc, char *argv[], char *imgFilename);
 170 int main(int argc, char *argv[]);
 171
 172
 173 /******************************************************************************/
 174 /**************************** variable declarations ***************************/
 175 /******************************************************************************/
 176
 177 struct robottype_orte_data orte;
 178 CORBA_octet robot_switches_color = 100;
 179 bool camera_control_on = false;
 180
 181 CvMat **sideMasks=NULL;
 182 CvMat **centerMasks=NULL;
 183
 184 CvFont fontLarge;
 185
 186
 187 /******************************************************************************/
 188 /********************** multimode graphical functions *************************/
 189 /******************************************************************************/
 190
 191 /** Saves current image to a new file in the current directory.
 192   * @param img Image to save to file.
 193   * @return Value returned by cvSaveImage. */
 194 int saveFrame(const CvArr *img) {
 195         struct stat stFileInfo;
 196         char filename[30];
 197         int i=0;
 198
 199         //find a non-existent filename (e.g. "snapshot13.png")
 200         do {
 201                 sprintf(filename, SNAPSHOTFILENAME, i++);
 202         } while(!stat(filename, &stFileInfo));
 203
 204         fprintf(stdout, "Saving frame to %s...\n", filename);
 205         return cvSaveImage(filename, img);
 206 }
 207
 208 /******************************************************************************/
 209
 210 /** Displays in product window the result of multiplying camera frame (in float)
 211   * and recognized mask.
 212   * @param floatMat Pointer to the camera frame converted to float matrix.
 213   * @param sideConfig Index of recognized side situation.
 214   * @param centerConfig Index of recognized center situation. */
 215 void displayProduct(const CvMat *floatMat, int sideConfig, int centerConfig) {
 216         CvMat *sideProduct = cvCloneMat(floatMat);
 217         CvMat *centerProduct = cvCloneMat(floatMat);
 218
 219         //multiply frame with both masks and display their sum
 220         cvMul(floatMat, sideMasks[sideConfig], sideProduct);
 221         cvMul(floatMat, centerMasks[centerConfig], centerProduct);
 222         displayMatSum(sideProduct, centerProduct, WINDOW_PROD);
 223
 224         cvReleaseMat(&sideProduct);
 225         cvReleaseMat(&centerProduct);
 226 }
 227
 228 /******************************************************************************/
 229
 230 /** Displays a sum of two matrices in specified window.
 231   * @param mat1 First float matrix.
 232   * @param mat2 Second float matrix.
 233   * @param windowName Window specifier. */
 234 void displayMatSum(const CvMat *mat1, const CvMat *mat2, const char *windowName) {
 235         CvMat *sum = cvCloneMat(mat1);
 236
 237         cvAdd(mat1, mat2, sum);
 238         displayFloatMat(sum, windowName);
 239
 240         cvReleaseMat(&sum);
 241 }
 242
 243 /******************************************************************************/
 244
 245 /** Normalizes the matrix values and displays them in window specified by name.
 246   * @param floatMat Pointer to matrix data to display.
 247   * @param windowName Name of window in which to display the image. */
 248 #ifdef WITH_GUI
 249 void displayFloatMat(const CvMat *floatMat, const char *windowName) {
 250         double min=0.0, max=0.0;
 251         CvMat *normalized = cvCloneMat(floatMat);
 252
 253         //find extremes of floatMat
 254         cvMinMaxLoc(floatMat, &min, &max);
 255         //normalize
 256         cvConvertScale(floatMat, normalized, 1/(max-min), -(min/(max-min)));
 257         cvShowImage(windowName, normalized);
 258
 259         cvReleaseMat(&normalized);
 260 }
 261 #else /* PPC */
 262
 263 void displayFloatMat(const CvMat *floatMat, const char *windowName) {}
 264 #endif /* WITH_GUI */
 265
 266 /******************************************************************************/
 267 /******************** multimode computational functions ***********************/
 268 /******************************************************************************/
 269
 270 /** Returns a threshold computed from the frame.
 271   * @param frame Frame to compute a threshold from.
 272   * @return Mean of all pixels of the frame. */
 273 int countThreshold(const IplImage *frame) {
 274         return cvAvg(frame).val[0];
 275 }
 276
 277 /******************************************************************************/
 278
 279 /** Returns an ordinary number of recognized configuration.
 280   * @param frame Float representation of frame.
 281   * @param masks Array of masks to compare with.
 282   * @param masksCnt Size of the array.
 283   * @param resDist If not NULL, a distance between the best and second best
 284   *  result is stored here.
 285   * @return Number of recognized configuration. */
 286 int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist) {
 287         double max = cvDotProduct(frame, masks[0]);
 288         double max2 = -DBL_MAX;
 289         int maxInd = 0;
 290         for(int i=1; i<masksCnt; i++) {
 291                 double prod = cvDotProduct(frame, masks[i]);
 292                 if(prod > max) {
 293                         max2 = max;
 294                         max = prod;
 295                         maxInd = i;
 296                 } else if(prod > max2) max2 = prod;
 297         }
 298         if(resDist) *resDist = max-max2;
 299         return maxInd;
 300 }
 301
 302
 303 /******************************************************************************/
 304 /********************************** MODE_IMAGE ********************************/
 305 /******************************************************************************/
 306
 307 /** Simulates MODE_RECOGNIZE over one specified image filename (PC only).
 308   * @param imageFilename Image to load and recognize.
 309   * @return Code of mode to switch to. */
 310 #ifdef WITH_GUI
 311 int modeImage(char *imageFilename) {
 312         int thrChange = 0;
 313         IplImage *frame = NULL;
 314         IplImage *thresFrame = NULL;
 315         //float <-1,1> image of the frame
 316         CvMat *floatMat = NULL;
 317         //indices of recognized configurations
 318         int sideConfig, centerConfig;
 319         //distances between the best and second best results
 320         double sideDist, centerDist;
 321
 322         frame = cvLoadImage(imageFilename);
 323         if(!frame) {
 324                 fprintf(stderr, "File %s cannot be loaded as image.\n",
 325                         imageFilename);
 326                 return MODE_QUIT;
 327         }
 328         thresFrame = cvCreateImage(cvSize(frame->width,frame->height),
 329                 IPL_DEPTH_8U,1);
 330
 331         while(1) {
 332                 int threshold = countThreshold(frame)+THRESHOLD_SHIFT+thrChange;
 333                 if(threshold<0) threshold=0;
 334
 335                 clr2float(frame, &floatMat, threshold, SATURATION, thresFrame);
 336
 337                 //recognize side and center configurations
 338                 sideConfig=recognize(floatMat,sideMasks,SIDEMASKSCNT,&sideDist);
 339                 centerConfig=recognize(floatMat,centerMasks,CENTERMASKSCNT,&centerDist);
 340
 341                 fprintf(stderr, "thr: %3d (thrChange %d), " \
 342                         "side: %d (%.2f), center: %d (%.2f)\n",
 343                         threshold, thrChange, sideConfig, sideDist,
 344                         centerConfig, centerDist);
 345
 346                 displayFrames(frame,&thresFrame,floatMat,sideConfig,centerConfig);
 347                 cvReleaseMat(&floatMat);
 348
 349                 // wait infinitely for a keyboard event
 350                 switch((char)(cvWaitKey(0) & 0xFF)) {
 351                 // increment threshold
 352                 case KEY_PLUS:
 353                         thrChange++;
 354                         break;
 355                 // decrement threshold
 356                 case KEY_MINUS:
 357                         thrChange--;
 358                         break;
 359                 // quit
 360                 case KEY_ESC:
 361                         goto _modeImage_end;
 362                         break;
 363                 }
 364         }
 365
 366 _modeImage_end:
 367         cvReleaseImage(&thresFrame);
 368         cvReleaseImage(&frame);
 369         return MODE_QUIT;
 370 }
 371 #else /* PPC */
 372
 373 int modeImage(char *imageFilename) {
 374         fprintf(stderr, "Image mode is unsupported on PPC.\nTerminating.\n");
 375         return MODE_QUIT;
 376 }
 377 #endif /* WITH_GUI */
 378
 379
 380 /******************************************************************************/
 381 /********************************* MODE_VIDEO *********************************/
 382 /******************************************************************************/
 383
 384 /** Writes text "pause" in the CAMERA window.
 385   * @param img Frame to display with PAUSE text. */
 386 #ifdef WITH_GUI
 387 void drawPauseText(IplImage *img) {
 388         cvPutText(img,"PAUSE",cvPoint(280,440),&fontLarge,cvScalar(255,255,255));
 389         cvShowImage(WINDOW_ORIG, img);
 390 }
 391 #else /* PPC */
 392
 393 void drawPauseText(IplImage *img) {}
 394 #endif /* WITH_GUI */
 395
 396 /******************************************************************************/
 397
 398 /** Displays just a real video in a window (only on PC).
 399   * @param capture Pointer to a camera capture.
 400   * @return Code of mode to switch to. */
 401 #ifdef WITH_GUI
 402 int modeVideo(CvCapture* capture) {
 403         IplImage* frame = NULL;
 404
 405         while(1) {
 406                 // wait 10ms for an event
 407                 switch(cvWaitKey(10) & 0xFF) {
 408                 // stop capturing
 409                 case KEY_ESC:
 410                         return MODE_QUIT;
 411
 412                 // switch to recognize mode
 413                 case KEY_R:
 414                 case KEY_r:
 415                         return MODE_RECOGNIZE;
 416
 417                 // pause - switch to wait mode
 418                 case KEY_P:
 419                 case KEY_p:
 420                 case KEY_W:
 421                 case KEY_w:
 422                         drawPauseText(frame);
 423                         return MODE_WAIT;
 424
 425                 // save frame to file
 426                 case KEY_S:
 427                 case KEY_s:
 428                         saveFrame(frame);
 429                         break;
 430                 }
 431
 432                 // get one frame from camera (do not release it!)
 433                 frame = cvQueryFrame(capture);
 434                 if(!frame) {
 435                         fprintf(stderr, "NULL frame\n");
 436                         continue;
 437                 }
 438                 cvShowImage(WINDOW_ORIG, frame);
 439         }
 440         return MODE_QUIT;
 441 }
 442 #else /* PPC */
 443
 444 int modeVideo(CvCapture* capture) {
 445         fprintf(stderr, "Video mode is unsupported on PPC.\nTerminating.\n");
 446         return MODE_QUIT;
 447 }
 448 #endif /* WITH_GUI */
 449
 450
 451 /******************************************************************************/
 452 /****************************** MODE_RECOGNIZE ********************************/
 453 /******************************************************************************/
 454
 455 /** On PC processes keyboard events, on PPC does nothing.
 456   * @param frame Last camera frame.
 457   * @return MODE_RECOGNIZE or mode to switch to. */
 458 #ifdef WITH_GUI
 459 int recognizeMode_processKeys(IplImage *frame) {
 460         // wait 10ms for an event
 461         switch(cvWaitKey(10) & 0xFF) {
 462         // stop capturing
 463         case KEY_ESC:
 464                 return MODE_QUIT;
 465
 466         // video mode
 467         case KEY_V:
 468         case KEY_v:
 469                 return MODE_VIDEO;
 470
 471         // pause - wait mode
 472         case KEY_P:
 473         case KEY_p:
 474         case KEY_W:
 475         case KEY_w:
 476                 drawPauseText(frame);
 477                 camera_control_on = 0;
 478                 return MODE_WAIT;
 479
 480         // save frame to file
 481         case KEY_S:
 482         case KEY_s:
 483                 saveFrame(frame);
 484                 break;
 485         }
 486         return MODE_RECOGNIZE;
 487 }
 488 #else /* PPC */
 489
 490 int recognizeMode_processKeys(IplImage *frame) {
 491         return MODE_RECOGNIZE;
 492 }
 493 #endif /* WITH_GUI */
 494
 495 /******************************************************************************/
 496
 497 /** Displays analytic frames on screen (on PC), or saves (input) frame to file
 498   * (on PPC in debug mode).
 499   * @param frame Input frame from camera.
 500   * @param thrFrame Thresholded frame.
 501   * @param fMat Float representation of frame.
 502   * @param cfgSide Index of current side configuration.
 503   * @param cfgCenter Index of current center configuration. */
 504 #ifdef WITH_GUI
 505 void displayFrames(IplImage *frame, IplImage **thrFrame, CvMat *fMat,
 506                 int cfgSide, int cfgCenter) {
 507         cvShowImage(WINDOW_ORIG, frame);
 508         cvShowImage(WINDOW_THRES, *thrFrame);
 509         displayProduct(fMat, cfgSide, cfgCenter);
 510         displayMatSum(sideMasks[cfgSide], centerMasks[cfgCenter], WINDOW_MASK);
 511
 512         cvReleaseImage(thrFrame);
 513         *thrFrame=cvCreateImage(cvSize(frame->width,frame->height),IPL_DEPTH_8U,1);
 514 }
 515 #elif defined (PPC_DEBUG) /* PPC in debug mode */
 516
 517 void displayFrames(IplImage *frame, IplImage **thrFrame, CvMat *fMat,
 518                 int cfgSide, int cfgCenter) {
 519         saveFrame(frame);
 520 }
 521 #else /* PPC standard */
 522
 523 void displayFrames(IplImage *frame, IplImage **thrFrame, CvMat *fMat,
 524                 int cfgSide, int cfgCenter) { /* nothing */ }
 525 #endif /* WITH_GUI */
 526
 527 /******************************************************************************/
 528
 529 /** Implements the camera recognition of corns configurations.
 530  * @param capture Pointer to a camera capture.
 531  * @param currentColor Number of starting color. If it changes in orte during
 532  *  recognition, this mode needs to restart (to load suitable masks).
 533  * @return Code of mode to switch to. */
 534 int modeRecognize(CvCapture* capture, CORBA_octet currentColor) {
 535         int ret;
 536         IplImage *frame = NULL;
 537         IplImage *thresFrame = NULL;
 538         //float <-1,1> matrix as a linear transformation of frame
 539         CvMat *floatMat = NULL;
 540         //indices of recognized configurations
 541         int sideConfig, centerConfig;
 542         //distances between the best and second best results
 543         double sideDist, centerDist;
 544         //"avaraging" decision box of side and center configurations
 545         TDecisionBox sideBox(DECISION_BOX_SIZE, SIDEMASKSCNT);
 546         TDecisionBox centerBox(DECISION_BOX_SIZE, CENTERMASKSCNT);
 547
 548         while(camera_control_on) {
 549
 550                 //if color changed during recognition, restart the mode to load appropriate masks
 551                 if(getRobotSwitchesColor() != currentColor) return MODE_RECOGNIZE;
 552
 553                 if((ret=recognizeMode_processKeys(frame)) != MODE_RECOGNIZE)
 554                         return ret;
 555
 556                 //get one frame from camera (do not release!)
 557                 frame = cvQueryFrame(capture);
 558                 if(!frame) {
 559                         fprintf(stderr, "NULL frame\n");
 560                         usleep(100*1000);
 561                         continue;
 562                 } else if(orte.camera_result.error & camera_ERR_NO_FRAME) {
 563                         orte.camera_result.error &= ~camera_ERR_NO_FRAME;
 564                         ORTEPublicationSend(orte.publication_camera_result);
 565                 }
 566
 567                 // convert to float
 568                 int threshold = countThreshold(frame) + THRESHOLD_SHIFT;
 569                 if(threshold<0) threshold=0;
 570                 clr2float(frame, &floatMat, threshold, SATURATION, thresFrame);
 571
 572                 // recognize side and center configurations
 573                 sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
 574                 centerConfig = recognize(floatMat, centerMasks, CENTERMASKSCNT, &centerDist);
 575                 fprintf(stderr, "SINGLE:   thr: %3d, side: %d (%.2f), center: %d (%.2f)\n",
 576                         threshold, sideConfig, sideDist, centerConfig, centerDist);
 577
 578                 //push the results to the decision box and get mean solutions from it
 579                 sideBox.insertItem(sideConfig, sideDist);
 580                 centerBox.insertItem(centerConfig, centerDist);
 581                 sideBox.decide(&sideConfig, &sideDist);
 582                 centerBox.decide(&centerConfig, &centerDist);
 583
 584                 //publish results
 585                 orte.camera_result.side = sideConfig;
 586                 orte.camera_result.center = centerConfig;
 587                 orte.camera_result.sideDist = sideDist;
 588                 orte.camera_result.centerDist = centerDist;
 589                 ORTEPublicationSend(orte.publication_camera_result);
 590                 fprintf(stderr, "ORTE:               side: %d (%.2f), center: %d (%.2f)\n",
 591                         sideConfig, sideDist, centerConfig, centerDist);
 592
 593                 displayFrames(frame,&thresFrame,floatMat,sideConfig,centerConfig);
 594                 cvReleaseMat(&floatMat);
 595         }
 596         return MODE_WAIT;
 597 }
 598
 599
 600 /******************************************************************************/
 601 /********************************* MODE_WAIT **********************************/
 602 /******************************************************************************/
 603
 604 /** Returns mode to switch to from wait mode. On PC processes keyboard events,
 605   * on PPC just tests for orte.camera_control.on value.
 606   * @param previousMode Mode to switch to, if no more waiting is needed.
 607   * @return Mode to switch to. */
 608 #ifdef WITH_GUI
 609 int waitMode_processKeys(int previousMode) {
 610         // wait 10ms for an event
 611         switch(cvWaitKey(10) & 0xFF) {
 612         // exit program
 613         case KEY_ESC:
 614                 return MODE_QUIT;
 615
 616         // stop waiting
 617         case KEY_P:
 618         case KEY_p:
 619         case KEY_W:
 620         case KEY_w:
 621                 camera_control_on = 1;
 622                 return previousMode;
 623
 624         // continue waiting
 625         default:
 626                 return MODE_WAIT;
 627         }
 628 }
 629 #else /* PPC */
 630
 631 int waitMode_processKeys(int previousMode) {
 632         return (getCameraControlOn() ? previousMode : MODE_WAIT);
 633 }
 634 #endif /* WITH_GUI */
 635
 636 /******************************************************************************/
 637
 638 /** Waits until orte.camera_control.on is set to true or, on PC, W or P key
 639   * is pressed.
 640   * @param previousMode Mode to switch to when waiting stops.
 641   * @return Code of mode to switch to. */
 642 int modeWait(int previousMode) {
 643         int mode;
 644
 645         while( (mode=waitMode_processKeys(previousMode)) == MODE_WAIT) {
 646                 fprintf(stderr, "waiting...\n");
 647                 sleep(1);
 648         }
 649         return mode;
 650 }
 651
 652
 653 /******************************************************************************/
 654 /******************************** mode manager ********************************/
 655 /******************************************************************************/
 656
 657 /** Initializes GUI, displays static video window. */
 658 #ifdef WITH_GUI
 659 void initGUI(void) {
 660         cvNamedWindow(WINDOW_ORIG, CV_WINDOW_AUTOSIZE);
 661         cvInitFont(&fontLarge, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 1);
 662 }
 663 #else /* PPC */
 664
 665 void initGUI(void) {}
 666 #endif /* WITH_GUI */
 667
 668 /******************************************************************************/
 669
 670 /** Sets visibility of 3 analytic windows (threshold, mask and product).
 671   * @param visible Use true to display windows, false to hide. */
 672 #ifdef WITH_GUI
 673 void setAnalyticWindowsVisible(bool visible) {
 674         if(visible) {
 675                 cvNamedWindow(WINDOW_THRES, CV_WINDOW_AUTOSIZE);
 676                 cvMoveWindow(WINDOW_THRES, WINDOW_WIDTH, 0);
 677                 cvNamedWindow(WINDOW_MASK, CV_WINDOW_AUTOSIZE);
 678                 cvMoveWindow(WINDOW_MASK, 0, WINDOW_HEIGHT+50);
 679                 cvNamedWindow(WINDOW_PROD, CV_WINDOW_AUTOSIZE);
 680                 cvMoveWindow(WINDOW_PROD, WINDOW_WIDTH, WINDOW_HEIGHT+50);
 681         } else {
 682                 cvDestroyWindow(WINDOW_THRES);
 683                 cvDestroyWindow(WINDOW_MASK);
 684                 cvDestroyWindow(WINDOW_PROD);
 685         }
 686 }
 687 #else /* PPC */
 688
 689 void setAnalyticWindowsVisible(bool visible) {}
 690 #endif /* WITH_GUI */
 691
 692 /******************************************************************************/
 693
 694 /** Destroys highgui windows. */
 695 #ifdef WITH_GUI
 696 void destroyGUI(void) {
 697         cvDestroyWindow(WINDOW_ORIG);
 698         cvDestroyWindow(WINDOW_THRES);
 699         cvDestroyWindow(WINDOW_MASK);
 700         cvDestroyWindow(WINDOW_PROD);
 701 }
 702 #else /* PPC */
 703
 704 void destroyGUI(void) {}
 705 #endif /* WITH_GUI */
 706
 707 /******************************************************************************/
 708
 709 /** Switches between modes of the program.
 710   * @param defaultMode The first mode to run.
 711   * @param paramC Used only in MODE_IMAGE mode (store filepath here).
 712   * @return Zero on success, error number on fail. */
 713 int modeManager(int defaultMode, char *paramC) {
 714         int mode=defaultMode;
 715         int lastMode=0;
 716         int ret;
 717         CvCapture* capture = NULL;
 718
 719         if(defaultMode==MODE_IMAGE) {
 720                 fprintf(stderr, "rozkuk started in image mode\n");
 721         } else {
 722                 // connect to a camera
 723                 while(!(capture=cvCaptureFromCAM(-1))) {
 724                         fprintf(stderr, "NULL capture (is camera connected?)\n");
 725                         orte.camera_result.error |= camera_ERR_NO_VIDEO;
 726                         ORTEPublicationSend(orte.publication_camera_result);
 727                         usleep(500*1000);
 728                 }
 729                 orte.camera_result.error &= ~camera_ERR_NO_VIDEO;
 730                 ORTEPublicationSend(orte.publication_camera_result);
 731                 fprintf(stderr, "rozkuk started, camera connected successfully\n");
 732         }
 733
 734         initGUI();
 735
 736         while(!(mode & MODE_QUIT)) {
 737                 switch(mode) {
 738
 739                 case MODE_VIDEO:
 740                         setAnalyticWindowsVisible(false);
 741                         mode = modeVideo(capture);
 742                         lastMode = MODE_VIDEO;
 743                         break;
 744
 745                 case MODE_RECOGNIZE:
 746                         setAnalyticWindowsVisible(true);
 747                         if((ret=loadMasks((getRobotSwitchesColor()==BLUE ?
 748                                 clBLUE : clYELLOW)))) return ret;
 749                         mode = modeRecognize(capture, getRobotSwitchesColor());
 750                         freeMasks();
 751                         lastMode = MODE_RECOGNIZE;
 752                         break;
 753
 754                 case MODE_WAIT:
 755                         mode = modeWait(lastMode);
 756                         break;
 757
 758                 case MODE_IMAGE:
 759                         setAnalyticWindowsVisible(true);
 760                         if((ret=loadMasks((getRobotSwitchesColor()==BLUE ?
 761                                 clBLUE : clYELLOW)))) return ret;
 762                         mode = modeImage(paramC);
 763                         freeMasks();
 764                         lastMode = MODE_IMAGE;
 765                         break;
 766
 767                 // jump out of the loop
 768                 default:
 769                         goto _modeManager_end;
 770                 }
 771         }
 772
 773 _modeManager_end:
 774         cvReleaseCapture(&capture);
 775         destroyGUI();
 776         return 0;
 777 }
 778
 779
 780 /******************************************************************************/
 781 /******************************* masks handling *******************************/
 782 /******************************************************************************/
 783
 784 /** Loads binary (float) data to *mask from file.
 785   * @param filename Path to file to read.
 786   * @param mask Address of array, where to alloc memory and store the data.
 787   * @return Length of read data or zero in case of error. */
 788 int loadMask(const char *filename, CvMat **mask) {
 789         int len;
 790
 791         IplImage *img = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
 792         if(!img) {
 793                 fprintf(stderr, "Mask %s cannot be loaded.\n", filename);
 794                 fprintf(stderr, "Run me from _build/user/camera/rozkuk\n");
 795                 return 0;
 796         }
 797
 798         *mask = NULL;
 799         if(!(len=clr2float(img, mask, CL_NEUTRAL, 1.0, NULL)))
 800                 fprintf(stderr, "A problem while converting mask to float occured.\n");
 801         else
 802                 fprintf(stderr, "Mask %s successfully loaded.\n", filename);
 803
 804         cvReleaseImage(&img);
 805         return len;
 806 }
 807
 808 /******************************************************************************/
 809
 810 /** Loads all the float masks from files.
 811   * @param color Actual team color - use 'b' or 'y'.
 812   * @return Zero if ok, else non-zero. */
 813 int loadMasks(char color) {
 814         char filename[100];
 815
 816         sideMasks = (CvMat **)malloc(SIDEMASKSCNT*sizeof(CvMat *));
 817         centerMasks = (CvMat **)malloc(CENTERMASKSCNT*sizeof(CvMat *));
 818
 819         for(int i=0; i<SIDEMASKSCNT; i++) {
 820                 sprintf(filename, MASKFILENAME, i, orSIDE, color);
 821                 if(!loadMask(filename, sideMasks+i)) return 1;
 822         }
 823         for(int i=0; i<CENTERMASKSCNT; i++) {
 824                 sprintf(filename, MASKFILENAME, i, orCENTER, color);
 825                 if(!loadMask(filename, centerMasks+i)) return 2;
 826         }
 827         return 0;
 828 }
 829
 830 /******************************************************************************/
 831
 832 /** Frees the memory allocated for masks. */
 833 void freeMasks(void) {
 834         for(int i=0; i<SIDEMASKSCNT; i++) cvReleaseMat(sideMasks+i);
 835         for(int i=0; i<CENTERMASKSCNT; i++) cvReleaseMat(centerMasks+i);
 836         free(sideMasks);
 837         free(centerMasks);
 838 }
 839
 840
 841 /******************************************************************************/
 842 /*********************************** orte *************************************/
 843 /******************************************************************************/
 844
 845 /** Returns actual state of orte.camera_control.on.
 846   * If value changed from previous call, informative output is printed. */
 847 #ifdef WITH_GUI
 848 inline bool getCameraControlOn(void) {
 849         return camera_control_on;
 850 }
 851 #else /* PPC */
 852
 853 inline bool getCameraControlOn(void) {
 854         if(orte.camera_control.on!=camera_control_on) {
 855                 camera_control_on = orte.camera_control.on;
 856                 fprintf(stderr, "orte: camera_control changed: ctrl %d\n",
 857                                 camera_control_on);
 858         }
 859         return camera_control_on;
 860 }
 861 #endif /* WITH_GUI */
 862
 863 /******************************************************************************/
 864
 865 /** Returns actual state of orte.robot_switches.team_color.
 866   * If value changed from previous call, informative output is printed.
 867   * @return Actual team color. */
 868 inline CORBA_octet getRobotSwitchesColor(void) {
 869         if(orte.robot_switches.team_color!=robot_switches_color) {
 870                 robot_switches_color = orte.robot_switches.team_color;
 871                 fprintf(stderr, "orte: robot_switches changed: color %d\n",
 872                         robot_switches_color);
 873         }
 874         return robot_switches_color;
 875 }
 876
 877 /******************************************************************************/
 878
 879 /** Orte camera result callback function. Does nothing. */
 880 void send_cmr_res_cb(const ORTESendInfo *info, void *vinstance,
 881                 void *recvCallBackParam) { /* nothing */ }
 882
 883 /******************************************************************************/
 884
 885 /** Orte camera control callback function.
 886   * Sets actual value of orte.camera_control.on to camera_control_on. */
 887 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
 888         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 889
 890         switch (info->status) {
 891                 case NEW_DATA:
 892                         fprintf(stderr, "orte: New camera data: ctrl %d\n",
 893                                 orte_data->camera_control.on);
 894                         getCameraControlOn();
 895                         break;
 896                 case DEADLINE:
 897                         fprintf(stderr, "ORTE deadline occurred - CMR_CTRL receive\n");
 898                         break;
 899         }
 900 }
 901
 902 /******************************************************************************/
 903
 904 /** Orte switches control callback function.
 905   * Sets actual value of orte.robot_switches.team_color to robot_switches_color. */
 906 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
 907         struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
 908
 909         switch (info->status) {
 910         case NEW_DATA:
 911                 fprintf(stderr, "orte: New switches data: color %d\n",
 912                         instance->team_color);
 913                 getRobotSwitchesColor();
 914                 break;
 915         case DEADLINE:
 916                 fprintf(stderr, "ORTE deadline occurred - RBT_SWTCH receive\n");
 917                 break;
 918         }
 919 }
 920
 921
 922 /******************************************************************************/
 923 /********************************* application ********************************/
 924 /******************************************************************************/
 925
 926 /** If "-i filename" is in argv, filename is stored to imgFilename.
 927   * @return Mode to switch to. */
 928 #ifdef WITH_GUI
 929 int getStartMode(int argc, char *argv[], char *imgFilename) {
 930         char opt;
 931
 932         // scan for program arguments
 933         while((opt = getopt(argc, argv, "i:")) != -1) {
 934                 switch (opt) {
 935                 //"-i filename"
 936                 case 'i':
 937                         if(optarg) sprintf(imgFilename, "%s", optarg);
 938                         else {
 939                                 fprintf(stderr, "Specify image filename " \
 940                                         "to process: rozkuk -i filename\n");
 941                                 return MODE_QUIT;
 942                         }
 943                         return MODE_IMAGE;
 944                         break;
 945                 }
 946         }
 947
 948         camera_control_on = true;
 949         return MODE_VIDEO;
 950 }
 951 #else /* PPC */
 952
 953 int getStartMode(int argc, char *argv[], char *imgFilename) {
 954         return MODE_WAIT;
 955 }
 956 #endif /* WITH_GUI */
 957
 958 /******************************************************************************/
 959
 960 /** The program may on PC be called with -i argument followed by an image
 961   * filename, i.e. rozkuk -i image.png, then the program runs in image mode.
 962   * Else call it without arguments. On PC it starts in video mode, to switch
 963   * modes keyboard is used (R-recognize, W-wait, Esc-quit). On PPC only two
 964   * modes are available - recognize and wait. Orte's camera_control_on variable
 965   * is used to switch between them. */
 966 int main(int argc, char *argv[]) {
 967         char imgFilename[100];
 968         int ret;
 969
 970         ret = robottype_roboorte_init(&orte);
 971         if(ret < 0) {
 972                 fprintf(stderr, "robottype_roboorte_init failed\n");
 973                 return ret;
 974         }
 975         robottype_publisher_camera_result_create(&orte, send_cmr_res_cb, &orte);
 976         robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
 977         robottype_subscriber_robot_switches_create(&orte, rcv_robot_switches_cb, &orte);
 978
 979         ret = getStartMode(argc, argv, imgFilename);
 980         modeManager(ret, imgFilename);
 981
 982         ret = robottype_roboorte_destroy(&orte);
 983         return ret;
 984 }
 985