If the game_color is changed during recognition, load appropriate masks.

[eurobot/public.git] / src / camera / rozkuk / rozkuk.cxx

diff --git a/src/camera/rozkuk/rozkuk.cxx b/src/camera/rozkuk/rozkuk.cxx
index 5926232df5dc0706b5bd013e91a991ad135f83ae..cc2d30d1456594293201715b8322c6b9744cf0f7 100644 (file)
--- a/src/camera/rozkuk/rozkuk.cxx
+++ b/src/camera/rozkuk/rozkuk.cxx
@@ -98,7 +98,7 @@ extern "C" {
 #define MASK_CENTER_SHIFT 4
 #define MASK_CENTER       (0x03 << 4)
 
-//number of results to select the most probable one from
+//number of results to select the most supported one from
 #define DECISION_BOX_SIZE 5
 
 //camera control system (if true, rozkuk recognizes, else it waits)
@@ -119,7 +119,7 @@ void displayMasksSum(const CvMat *mat1, const CvMat *mat2, const char *windowNam
 void displayFloatMat(const CvMat *floatMat, const char *windowName);
 int modeRealtime(CvCapture* capture);
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
-int modeRecognize(CvCapture* capture);
+int modeRecognize(CvCapture* capture, CORBA_octet currentColor);
 int modeWait(CvCapture *capture);
 int modeManager(int defaultMode);
 int countThreshold(const IplImage *frame);
@@ -271,8 +271,10 @@ int modeRealtime(CvCapture* capture) {
 /** Implements the camera recognition of corns.
  * In DEBUG session the results are also displayed on screen.
  * @param capture Pointer to a camera capture.
+ * @param currentColor Number of starting color. If it changes in orte during
+ *   recognition, this mode needs to restart (to load suitable masks).
  * @return Code of mode to switch to. */
-int modeRecognize(CvCapture* capture) {
+int modeRecognize(CvCapture* capture, CORBA_octet currentColor) {
        IplImage *frame = NULL;                               // frame from camera
        IplImage *thresFrame = NULL;                          // thresholded frame
        CvMat *floatMat = NULL;                               // float <-1,1> image of the frame
@@ -285,6 +287,9 @@ int modeRecognize(CvCapture* capture) {
        int pause=0;                                          // set pause=1 to pause processing
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
        while(ORTE_CAMERA_CONTROL) {
+       
+               //if color changed during recognition, restart the mode to load appropriate masks
+               if(orte.camera_control.game_color != currentColor) return MODE_RECOGNIZE;
 
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
                /* keyboard events handeling */
@@ -349,7 +354,7 @@ int modeRecognize(CvCapture* capture) {
                //recognize side and center configurations
                sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
                centerConfig = recognize(floatMat, centerMasks, CENTERMASKSCNT, &centerDist);
-               fprintf(stderr, "SINGLE:   thr: %3d, side: %d (%f), center: %d (%f)\n", threshold, sideConfig, sideDist, centerConfig, centerDist);
+               fprintf(stderr, "SINGLE:   thr: %3d, side: %d (%.2f), center: %d (%.2f)\n", threshold, sideConfig, sideDist, centerConfig, centerDist);
                //push the results to the decision box and get "averaged" solutions from it
                sideBox.insertItem(sideConfig, sideDist);
                centerBox.insertItem(centerConfig, centerDist);
@@ -361,7 +366,7 @@ int modeRecognize(CvCapture* capture) {
                orte.camera_result.sideDist = sideDist;
                orte.camera_result.centerDist = centerDist;
                ORTEPublicationSend(orte.publication_camera_result);
-               fprintf(stderr, "ORTE:               side: %d (%f), center: %d (%f)\n", sideConfig, sideDist, centerConfig, centerDist);
+               fprintf(stderr, "ORTE:               side: %d (%.2f), center: %d (%.2f)\n", sideConfig, sideDist, centerConfig, centerDist);
                
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
                displayProduct(floatMat, sideConfig, centerConfig);
@@ -420,6 +425,7 @@ int modeManager(int defaultMode) {
        int mode=defaultMode;
        int ret;
        CvCapture* capture;
+       CORBA_octet color;
 
        // connect to a camera
        while(!((capture=cvCaptureFromCAM(0)) || (capture=cvCaptureFromCAM(1)))) {
@@ -458,8 +464,9 @@ int modeManager(int defaultMode) {
                        cvMoveWindow(WINDOW_PROD, cvQueryFrame(capture)->width, cvQueryFrame(capture)->height+50);
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
                        //load masks and run recognition mode, then free masks (they are in memory only while in recognition mode)
-                       if((ret=loadMasks((orte.camera_control.game_color==BLUE ? clBLUE : clYELLOW)))) return ret;
-                       mode = modeRecognize(capture);
+                       color = orte.camera_control.game_color;
+                       if((ret=loadMasks((color==BLUE ? clBLUE : clYELLOW)))) return ret;
+                       mode = modeRecognize(capture, color);
                        freeMasks();
                        break;