#include "clr2float.h"
#include "masks_globals.h"
+#include "t_decision_box.h"
extern "C" {
#include <roboorte_robottype.h>
#include <robot.h>
-#include <robottype_utils.h>
}
/******************************************************************************/
#define MASK_CENTER_SHIFT 4
#define MASK_CENTER (0x03 << 4)
+//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)
+#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
+#define ORTE_CAMERA_CONTROL 1
+#else /******************** PPC *******************/
+#define ORTE_CAMERA_CONTROL orte.camera_control.on
+#endif /*------------------------------------------*/
+
/******************************************************************************/
// function declarations
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);
frame = cvQueryFrame(capture); // Get one frame
if(!frame) {
- fprintf(stdout, "NULL frame\n");
+ fprintf(stderr, "NULL frame\n");
continue;
}
/** 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
int sideConfig, centerConfig; // indices of recognized configurations
double sideDist, centerDist; // distances between the best and second best results
+ TDecisionBox sideBox(DECISION_BOX_SIZE, SIDEMASKSCNT); // side solutions "averaging" decision box
+ TDecisionBox centerBox(DECISION_BOX_SIZE, CENTERMASKSCNT); // center solutions "averaging" decision box
#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
int framesPassed = PASS_FRAMES; // number of passed frames before refresh
int pause=0; // set pause=1 to pause processing
#endif /*----------- DEBUG SESSION ONLY -----------*/
- while(orte.camera_control.on) {
+ 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 */
#endif /*----------- DEBUG SESSION ONLY -----------*/
frame = cvQueryFrame(capture);
if(!frame) {
- fprintf(stdout, "NULL frame\n");
+ fprintf(stderr, "NULL frame\n");
// save error code to orte and publish it
- orte.camera_result.error |= ERR_CAM_NO_FRAME;
+ orte.camera_result.error |= camera_ERR_NO_FRAME;
ORTEPublicationSend(orte.publication_camera_result);
+ usleep(100*1000);
continue;
- } else if(orte.camera_result.error & ERR_CAM_NO_FRAME) {
- orte.camera_result.error &= ~ERR_CAM_NO_FRAME;
+ } else if(orte.camera_result.error & camera_ERR_NO_FRAME) {
+ orte.camera_result.error &= ~camera_ERR_NO_FRAME;
ORTEPublicationSend(orte.publication_camera_result);
}
// convert to float
int threshold = countThreshold(frame);
clr2float(frame, &floatMat, threshold, SATURATION, thresFrame); // get float representation (in DEBUG session, thresholded image is stored to thresFrame)
- //recognize side and center layouts
+ //recognize side and center configurations
sideConfig = recognize(floatMat, sideMasks, SIDEMASKSCNT, &sideDist);
centerConfig = recognize(floatMat, centerMasks, CENTERMASKSCNT, ¢erDist);
+ 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);
+ sideBox.decide(&sideConfig, &sideDist);
+ centerBox.decide(¢erConfig, ¢erDist);
//publish results
- 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;
orte.camera_result.sideDist = sideDist;
orte.camera_result.centerDist = centerDist;
ORTEPublicationSend(orte.publication_camera_result);
+ fprintf(stderr, "ORTE: side: %d (%.2f), center: %d (%.2f)\n", sideConfig, sideDist, centerConfig, centerDist);
#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
displayProduct(floatMat, sideConfig, centerConfig);
* @param capture Pointer to a camera capture.
* @return Code of mode to switch to. */
int modeWait(CvCapture *capture) {
- while(!orte.camera_control.on) {
+ while(!ORTE_CAMERA_CONTROL) {
#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
//process keyboard events
switch(cvWaitKey(10) & 0xFF) { // wait 10ms for an event
}
#endif /*----------- DEBUG SESSION ONLY -----------*/
- fprintf(stdout, "waiting...\n");
+ fprintf(stderr, "waiting...\n");
sleep(1);
}
return MODE_RECOGNIZE;
int mode=defaultMode;
int ret;
CvCapture* capture;
+ CORBA_octet color;
// 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;
+ fprintf(stderr, "NULL capture (is camera connected?)\n");
+ orte.camera_result.error |= camera_ERR_NO_VIDEO;
+ ORTEPublicationSend(orte.publication_camera_result);
usleep(500*1000);
}
- orte.camera_result.error &= ~ERR_CAM_NO_VIDEO;
+ orte.camera_result.error &= ~camera_ERR_NO_VIDEO;
+ ORTEPublicationSend(orte.publication_camera_result);
+ fprintf(stderr, "rozkuk started, camera connected successfully\n");
#ifdef ROZKUK_DEBUG /************ DEBUG SESSION ONLY ************/
// create gui windows and init font
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;
IplImage *img = cvLoadImage(filename, CV_LOAD_IMAGE_GRAYSCALE);
if(!img) {
- fprintf(stdout, "Mask %s cannot be loaded.\n", filename);
+ fprintf(stderr, "Mask %s cannot be loaded.\n", filename);
fprintf(stderr, "Run me from _build/user/camera/rozkuk\n");
return 0;
}
if(!(len=clr2float(img, mask, CL_NEUTRAL, 1.0, NULL)))
fprintf(stderr, "A problem while converting mask to float occured.\n");
else
- fprintf(stdout, "Mask %s successfully loaded.\n", filename);
+ fprintf(stderr, "Mask %s successfully loaded.\n", filename);
cvReleaseImage(&img);
return len;
/** Orte camera control callback function. */
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;
+#endif /*----------- ORTE_DEBUG -----------*/
switch (info->status) {
case NEW_DATA:
// nothing to do - changes will be processed in the recognition loop
+#ifdef ORTE_DEBUG
printf("orte: New data: ctrl %d, clr %d\n", orte_data->camera_control.on, orte_data->camera_control.game_color);
+#endif /*----------- ORTE_DEBUG -----------*/
break;
case DEADLINE:
printf("ORTE deadline occurred - CMR_CTRL receive\n");
projects / eurobot / public.git / blobdiff