Orte integrated to rozkuk, but it does not work properly yet - needs revision.

Now orte "provides" still the same (and probably incorrect) values.

diff --git a/src/camera/rozkuk/rozkuk.cxx b/src/camera/rozkuk/rozkuk.cxx
index 9c09d372a9ef4748c1b7286ead20400d27860894..058c3d7c71c51e29cf219ef7c526da6c1be0fa1e 100644 (file)
--- a/src/camera/rozkuk/rozkuk.cxx
+++ b/src/camera/rozkuk/rozkuk.cxx
@@ -25,6 +25,7 @@
 extern "C" {
 #include <roboorte_robottype.h>
 #include <robot.h>
+#include <robottype_utils.h>
 }
 /******************************************************************************/
 
@@ -44,12 +45,12 @@ extern "C" {
 // errors
 #define ERR_MASK_READ_FAILURE 0x10
 
-// default mode in standard session
-#define START_MODE        MODE_RECOGNIZE
+// default mode in standard session (i.e. on ppc)
+#define START_MODE        MODE_WAIT
 
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
 
-// default mode in debug session
+// default mode in debug session (i.e. on pc)
 #undef START_MODE
 #define START_MODE        MODE_REALTIME
 
@@ -97,8 +98,6 @@ extern "C" {
 #define MASK_CENTER_SHIFT 4
 #define MASK_CENTER       (0x03 << 4)
 
-struct robottype_orte_data orte;
-
 /******************************************************************************/
 
 // function declarations
@@ -118,10 +117,13 @@ 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);
+void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
 
 /******************************************************************************/
 
 // variable declarations
+struct robottype_orte_data orte;
+int orteError=0;
 CvMat **sideMasks=NULL;     //float mask matrices
 CvMat **centerMasks=NULL;
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
@@ -270,7 +272,7 @@ int modeRecognize(CvCapture* capture) {
        int framesPassed = PASS_FRAMES;                       // number of passed frames before refresh
        int pause=0;                                          // set pause=1 to pause processing
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
-       while(1) {  // TODO: test on ~ camera.running.on
+       while(orte.camera_control.on) {
 
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
                /* keyboard events handeling */
@@ -312,7 +314,13 @@ int modeRecognize(CvCapture* capture) {
                        frame = cvQueryFrame(capture);
                if(!frame) {
                        fprintf(stdout, "NULL frame\n");
+                       // save error code to orte and publish it
+                       orte.camera_result.error |= ERR_CAM_NO_FRAME;
+                       ORTEPublicationSend(orte.publication_camera_result);
                        continue;
+               } else if(orte.camera_result.error & ERR_CAM_NO_FRAME) {
+                       orte.camera_result.error &= ~ERR_CAM_NO_FRAME;
+                       ORTEPublicationSend(orte.publication_camera_result);
                }
 
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
@@ -320,7 +328,7 @@ int modeRecognize(CvCapture* capture) {
 
                // transform colorful image to its float <-1,1> representation
                cvReleaseImage(&thresFrame);
-               thresFrame = cvCreateImage(cvSize(640,480),IPL_DEPTH_8U,1);
+               thresFrame = cvCreateImage(cvSize(FRAME_WIDTH,FRAME_HEIGHT),IPL_DEPTH_8U,1);
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
                // convert to float
                int threshold = countThreshold(frame);
@@ -332,7 +340,8 @@ int modeRecognize(CvCapture* capture) {
                fprintf(stdout, "thr: %d, side: %d (%f), center: %d (%f)\n", threshold, sideConfig, sideDist, centerConfig, centerDist);
                orte.camera_result.side = sideConfig;
                orte.camera_result.center = centerConfig;
-                       // TODO: do propagate also the distances
+               orte.camera_result.sideDist = sideDist;
+               orte.camera_result.centerDist = centerDist;
                ORTEPublicationSend(orte.publication_camera_result);
                
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
@@ -353,7 +362,7 @@ int modeRecognize(CvCapture* capture) {
 #endif                    /*----------- DEBUG SESSION ONLY -----------*/
 
        }
-       return MODE_QUIT;
+       return MODE_WAIT;
 }
 
 /******************************************************************************/
@@ -396,8 +405,10 @@ int modeManager(int defaultMode) {
        // connect to a camera
        while(!((capture=cvCaptureFromCAM(0)) || (capture=cvCaptureFromCAM(1)))) {
                fprintf(stdout, "NULL capture - no /dev/video# found\n");
+               orte.camera_result.error |= ERR_CAM_NO_VIDEO;
                usleep(500*1000);
        }
+       orte.camera_result.error &= ~ERR_CAM_NO_VIDEO;
 
 #ifdef ROZKUK_DEBUG       /************ DEBUG SESSION ONLY ************/
        // create gui windows and init font
@@ -497,10 +508,10 @@ int loadMask(const char *filename, CvMat **mask) {
        }
        
        *mask = NULL;
-       if(!(len=clr2float(img, mask, CL_NEUTRAL, 1.0, NULL))) {
+       if(!(len=clr2float(img, mask, CL_NEUTRAL, 1.0, NULL)))
                fprintf(stderr, "A problem while converting mask to float occured.\n");
-       }
-       fprintf(stdout, "Mask %s successfully loaded.\n", filename);
+       else
+               fprintf(stdout, "Mask %s successfully loaded.\n", filename);
 
        cvReleaseImage(&img);
        return len;
@@ -539,6 +550,23 @@ void freeMasks(void) {
 
 /******************************************************************************/
 
+/** Orte camera control callback function. */
+void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
+       struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
+
+       switch (info->status) {
+               case NEW_DATA:
+                       // nothing to do - changes will be processed in the recognition loop
+                       printf("orte: New data: ctrl %d, clr %d\n", orte_data->camera_control.on, orte_data->camera_control.game_color);
+                       break;
+               case DEADLINE:
+                       printf("ORTE deadline occurred - CMR_CTRL receive\n");
+                       break;
+       }
+}
+
+/******************************************************************************/
+
 int main(int argc, char *argv[]) {
        int ret;
 
@@ -548,8 +576,8 @@ int main(int argc, char *argv[]) {
                return ret;
        }
        robottype_publisher_camera_result_create(&orte, NULL, NULL);
-       robottype_subscriber_camera_control_create(&orte, NULL, NULL);
-
+       robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
+       
        modeManager(START_MODE);
 
        ret = robottype_roboorte_destroy(&orte);