Threshold manual shift (to corigate unsuitable autothreshold).

Log improvement - print every CHANGE incoming from orte.

diff --git a/src/camera/rozkuk/rozkuk.cxx b/src/camera/rozkuk/rozkuk.cxx
index 676d7833f9092e5b64d2cd8d6366ff50afd85aa3..1b478ed56960b574952476a2789828fede223573 100644 (file)
--- a/src/camera/rozkuk/rozkuk.cxx
+++ b/src/camera/rozkuk/rozkuk.cxx
@@ -31,7 +31,7 @@ extern "C" {
 
 //uncomment next line to "log" the output frames - save them as pnm to the working directory
 //#define PPC_DEBUG
-//uncomment next line to print orte state
+//uncomment next line to print orte state messages
 //#define ORTE_DEBUG
 
 // modes definitions
@@ -80,8 +80,9 @@ extern "C" {
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 
 //default threshold and saturation
-#define THRESHOLD         70
+#define THRESHOLD         70      //not used
 #define SATURATION        1.0
+#define THRESHOLD_SHIFT   -10     //improves the automatic threshold by shifting by this value
 
 //mask filename pattern
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
@@ -105,9 +106,9 @@ extern "C" {
 
 //camera control system (if true, rozkuk recognizes, else it waits)
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
-#define ORTE_CAMERA_CONTROL 1
+#define ORTE_CAMERA_CONTROL camera_control_on
 #else                     /******************** PPC *******************/
-#define ORTE_CAMERA_CONTROL orte.camera_control.on
+#define ORTE_CAMERA_CONTROL camera_control_on
 #endif                    /*------------------------------------------*/
 
 /******************************************************************************/
@@ -129,12 +130,18 @@ int recognize(const CvMat *frame, CvMat **masks, int masksCnt, double *resDist=N
 int loadMask(const char *filename, CvMat **mask);
 int loadMasks(char color);
 void freeMasks(void);
+bool getCameraControlOn(void);
+CORBA_octet getRobotSwitchesColor(void);
+void send_cmr_res_cb(const ORTESendInfo *info, void *vinstance, void *recvCallBackParam);
 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
+void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
 
 /******************************************************************************/
 
 // variable declarations
 struct robottype_orte_data orte;
+bool camera_control_on = false;             //"undefault value"
+CORBA_octet robot_switches_color = 100;     //"undefault value"
 int orteError=0;
 CvMat **sideMasks=NULL;     //float mask matrices
 CvMat **centerMasks=NULL;
@@ -291,7 +298,7 @@ int modeRecognize(CvCapture* capture, CORBA_octet currentColor) {
        while(ORTE_CAMERA_CONTROL) {
        
                //if color changed during recognition, restart the mode to load appropriate masks
-               if(orte.robot_switches.team_color != currentColor) return MODE_RECOGNIZE;
+               if(robot_switches_color != currentColor) return MODE_RECOGNIZE;
 
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
                /* keyboard events handeling */
@@ -333,9 +340,9 @@ int modeRecognize(CvCapture* capture, CORBA_octet currentColor) {
                        frame = cvQueryFrame(capture);
                if(!frame) {
                        fprintf(stderr, "NULL frame\n");
-                       // save error code to orte and publish it
-                       orte.camera_result.error |= camera_ERR_NO_FRAME;
-                       ORTEPublicationSend(orte.publication_camera_result);
+                       // save error code to orte and publish it (not used, else even one failed frame would degrade the whole result)
+/*                     orte.camera_result.error |= camera_ERR_NO_FRAME;
+                       ORTEPublicationSend(orte.publication_camera_result); */
                        usleep(100*1000);
                        continue;
                } else if(orte.camera_result.error & camera_ERR_NO_FRAME) {
@@ -351,7 +358,8 @@ int modeRecognize(CvCapture* capture, CORBA_octet currentColor) {
                thresFrame = cvCreateImage(cvSize(FRAME_WIDTH,FRAME_HEIGHT),IPL_DEPTH_8U,1);
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
                // convert to float
-               int threshold = countThreshold(frame);
+               int threshold = countThreshold(frame) + THRESHOLD_SHIFT;
+               if(threshold<0) threshold=0;
                clr2float(frame, &floatMat, threshold, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
                //recognize side and center configurations
                sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
@@ -466,7 +474,7 @@ 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)
-                       color = orte.robot_switches.team_color;
+                       color = robot_switches_color;
                        if((ret=loadMasks((color==BLUE ? clBLUE : clYELLOW)))) return ret;
                        mode = modeRecognize(capture, color);
                        freeMasks();
@@ -581,7 +589,38 @@ void freeMasks(void) {
 
 /******************************************************************************/
 
-/** Orte camera control callback function. */
+/** Returns actual state of orte.camera_control.on.
+  *  If value changed from previous call, informative output is printed. */
+inline bool getCameraControlOn(void) {
+       if(orte.camera_control.on!=camera_control_on) {
+               camera_control_on = orte.camera_control.on;
+               fprintf(stderr, "orte: camera_control changed: ctrl %d\n", camera_control_on);
+       }
+       return camera_control_on;
+}
+
+/******************************************************************************/
+
+/** Returns actual state of orte.robot_switches.team_color.
+  *  If value changed from previous call, informative output is printed. */
+inline CORBA_octet getRobotSwitchesColor(void) {
+       if(orte.robot_switches.team_color!=robot_switches_color) {
+               robot_switches_color = orte.robot_switches.team_color;
+               fprintf(stderr, "orte: robot_switches changed: color %d\n", robot_switches_color);
+       }
+       return robot_switches_color;
+}
+
+/******************************************************************************/
+
+/** Orte camera result callback function. */
+void send_cmr_res_cb(const ORTESendInfo *info, void *vinstance, void *recvCallBackParam) {
+       /* empty function */
+}
+
+/******************************************************************************/
+
+/** Orte camera control callback function. Sets actual value to orte.camera_control.on. */
 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
 #ifdef ORTE_DEBUG
        struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
@@ -589,10 +628,10 @@ void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBa
 
        switch (info->status) {
                case NEW_DATA:
-                       // nothing to do - changes will be processed in the recognition loop
 #ifdef ORTE_DEBUG
                        fprintf(stderr, "orte: New camera data: ctrl %d\n", orte_data->camera_control.on);
 #endif /*----------- ORTE_DEBUG -----------*/
+                       getCameraControlOn();
                        break;
                case DEADLINE:
                        fprintf(stderr, "ORTE deadline occurred - CMR_CTRL receive\n");
@@ -602,7 +641,7 @@ void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBa
 
 /******************************************************************************/
 
-/** Orte switches control callback function. */
+/** Orte switches control callback function. Sets actual value to orte.robot_switches.team_color. */
 void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
 #ifdef ORTE_DEBUG
        struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
@@ -610,10 +649,10 @@ void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance, void *recv
 
        switch (info->status) {
                case NEW_DATA:
-                       // nothing to do - changes will be processed in the recognition loop
 #ifdef ORTE_DEBUG
-                       fprintf(stderr, "orte: New switches data: clr %d\n", instance->team_color);
+                       fprintf(stderr, "orte: New switches data: color %d\n", instance->team_color);
 #endif /*----------- ORTE_DEBUG -----------*/
+                       getRobotSwitchesColor();
                        break;
                case DEADLINE:
                        fprintf(stderr, "ORTE deadline occurred - RBT_SWTCH receive\n");
@@ -631,7 +670,7 @@ int main(int argc, char *argv[]) {
                fprintf(stderr, "robottype_roboorte_init failed\n");
                return ret;
        }
-       robottype_publisher_camera_result_create(&orte, NULL, NULL);
+       robottype_publisher_camera_result_create(&orte, send_cmr_res_cb, &orte);
        robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
        robottype_subscriber_robot_switches_create(&orte, rcv_robot_switches_cb, &orte);