src/camera/color-finder/main/revue.cpp

   1 #include <stdlib.h>
   2 #include <CCamera.h>
   3 #include <CGui.h>
   4 #include <CRobot.h>
   5 #include <CTimer.h>
   6 #include <CRecognition.h>
   7 #include <signal.h>
   8
   9 extern "C" {
  10 #include <roboorte_robottype.h>
  11 #include <robot.h>
  12 #include "../control/CRecognition.h"
  13 }
  14
  15 int i = 0;
  16 int numSaved = 0;
  17 bool stop = false;
  18 bool cp = true;
  19 CCamera* camera;
  20 CGui* gui;
  21 CRawImage *image;
  22 SDL_Event event;
  23 //CRobot* robot;
  24 CRecognition *recognition;
  25 //SRobotCommand command;
  26 SPixelPosition meanPosition;
  27 bool move = false;
  28 Uint8 lastKeys[10000];
  29 int keyNumber = 1000;
  30 Uint8 *keys = NULL;
  31
  32 unsigned char color [3];
  33 unsigned char* pix = NULL;
  34 bool data = false;
  35 int teamcolor = 0;
  36
  37
  38 /* Public data for ORTE comuunication */
  39 struct robottype_orte_data orte;
  40 bool camera_control_on = false;
  41
  42 void send_cmr_res_cb(const ORTESendInfo *info, void *vinstance, void *recvCallBackParam);
  43 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
  44 void rcv_cmr_rsw_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam);
  45
  46 void processKeys()
  47 {
  48 #ifdef __i386__
  49         while (SDL_PollEvent(&event)){
  50                 if (event.type == SDL_MOUSEBUTTONDOWN) {
  51                 pix = &image->data[3*(image->width*event.motion.y + event.motion.x)];
  52                 recognition->learnPixel(pix, color, teamcolor);
  53                 }
  54         }
  55
  56         keys = SDL_GetKeyState(&keyNumber);
  57         if (keys[SDLK_ESCAPE]) stop = true;
  58         if (keys[SDLK_a]) camera->setBrightness(camera->getBrightness()+10);
  59         if (keys[SDLK_z]) camera->setBrightness(camera->getBrightness()-10);
  60         if (keys[SDLK_s]) camera->setExposition(camera->getExposition()+10);
  61         if (keys[SDLK_x]) camera->setExposition(camera->getExposition()-10);
  62         if (keys[SDLK_u]) camera->setExposition(7);
  63         if (keys[SDLK_SPACE] && lastKeys[SDLK_SPACE] == false) move = (move == false);
  64         if (keys[SDLK_m]) recognition->increaseTolerance();
  65         if (keys[SDLK_n]) recognition->decreaseTolerance();
  66         //if (keys[SDLK_l]) recognition->learnPixel(&image->data[3*(640*240+320)]);
  67         if (keys[SDLK_RETURN])image->saveBmp();
  68         memcpy(lastKeys,keys,keyNumber);
  69 #endif
  70 }
  71 static void sig_handler(int sig)
  72 {
  73         stop = true;
  74 }
  75
  76 void controlExposition(CRawImage *img, CCamera *camera) {
  77
  78
  79         const int referenceBr = 130;
  80
  81         int actualBr = img->getOverallBrightness(false);
  82         //fprintf(stderr,"Overal brightness=%d\n",actualBr);
  83
  84         int curExp = camera->getExposition();
  85         //fprintf(stderr,"Current exposition is %d\n",curExp);
  86         int correction = lround(0.01*(referenceBr-actualBr));
  87
  88         curExp += correction;
  89         if (curExp < 0) {
  90                 curExp = 1;
  91         }
  92         //fprintf(stderr,"Correction is %d\n",correction);
  93
  94         if (correction != 0) {
  95                 //fprintf(stderr,"Setting exposition to %d\n",curExp);
  96         }
  97         camera->setExposition(curExp);
  98
  99         //static int aa = 0;
 100         //char name[200];
 101         //sprintf(name,"ex%06d.bmp",aa++);
 102         //img->saveBmp(name);
 103
 104 }
 105
 106 /* Funkce pro odeslani dat do ORTE */
 107 void send_data(SPixelPosition pos)
 108 {       data = true;
 109         //publish results
 110         orte.camera_result.x = pos.x;
 111         orte.camera_result.y = pos.y;
 112         orte.camera_result.target_valid = pos.info; // OK / NOT OK
 113         orte.camera_result.data_valid = data;
 114
 115         ORTEPublicationSend(orte.publication_camera_result);
 116         fprintf(stderr, "ORTE: x: %d y: %d, info: %d \n", pos.x, pos.y, pos.info);
 117 }
 118
 119 /* Fce pro zapnuti a vypnuti rozpoznavani */
 120 inline bool getCameraControlOn(void) {
 121         bool col = false;
 122
 123         if(orte.camera_control.on != camera_control_on) {
 124                 camera_control_on = orte.camera_control.on;
 125                 fprintf(stderr, "orte: camera_control changed: ctrl %d\n",
 126                                 camera_control_on);
 127         }
 128
 129         for (int i =0;i<3;i++){
 130                 if (color[i] != orte.camera_control.color[i]) {
 131                 color[i] = orte.camera_control.color[i];
 132                 col = true;
 133                 }
 134         }
 135         if(col) recognition->learnPixel(pix, color, teamcolor);
 136         return camera_control_on;
 137 }
 138
 139 /** Orte camera control callback function.
 140   * Sets actual value of orte.camera_control.on to camera_control_on. */
 141 void rcv_cmr_ctrl_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
 142         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 143
 144         switch (info->status) {
 145                 case NEW_DATA:
 146                         fprintf(stderr, "orte: New camera data: ctrl %d\n",
 147                                 orte_data->camera_control.on);
 148                         getCameraControlOn();
 149                         break;
 150                 case DEADLINE:
 151                         fprintf(stderr, "ORTE deadline occurred - CMR_CTRL receive\n");
 152                         break;
 153         }
 154 }
 155
 156 /** Orte robot switches callback function.
 157   * Sets actual value of orte.robot_switches to rswitch */
 158 void rcv_cmr_rsw_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) {
 159         struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
 160
 161         switch (info->status) {
 162                 case NEW_DATA:
 163                         fprintf(stderr, "orte: New camera data: ctrl %d\n",
 164                                 orte_data->robot_switches.team_color);
 165                         teamcolor = orte.robot_switches.team_color;
 166                         break;
 167                 case DEADLINE:
 168                         fprintf(stderr, "ORTE deadline occurred - CMR_CTRL receive\n");
 169                         break;
 170         }
 171 }
 172
 173 int main(int argc,char* argv[])
 174 {
 175         //Pokud neni zadan argument na prikazove radce, program vypise nasledujici chybove hlaseni a ukonci se
 176         if (argc < 2) {
 177                 fprintf(stderr,"usage: %s cameraDevice \ne.g. %s /dev/video0\n",argv[0],argv[0]);
 178                 return 0;
 179         }
 180
 181         // Signal handler registration
 182         signal(SIGINT, sig_handler);
 183         signal(SIGTERM, sig_handler);
 184
 185         const char* cameraDevice = argv[1];
 186         //const char* robotDevice = argv[2];
 187
 188         int ret;
 189
 190         ret = robottype_roboorte_init(&orte);
 191         if(ret < 0) {
 192                 fprintf(stderr, "robottype_roboorte_init failed\n");
 193                 return ret;
 194         }
 195
 196         robottype_subscriber_camera_control_create(&orte, rcv_cmr_ctrl_cb, &orte);
 197         robottype_subscriber_robot_switches_create(&orte, rcv_cmr_rsw_cb, &orte);
 198
 199         //Vytvoreni instanci tridy kamery, gui a obrazku
 200         camera = new CCamera();
 201 #ifdef __i386__
 202         gui = new CGui();
 203 #endif
 204         image = new CRawImage();
 205         //robot = new CRobot();
 206         recognition = new CRecognition();
 207
 208         //Inicializace zarizeni
 209         camera->init(cameraDevice);
 210         //robot->init(robotDevice);
 211         //image->getSaveNumber();
 212
 213         camera->setExposition(8);
 214         CTimer timer;
 215         while (!stop) {
 216                 while (camera_control_on && !stop) {
 217                         //Nacteni obrazku
 218                         camera->renewImage(image);
 219
 220                         //start timeru pro mereni doby zpracovani obrazu
 221                         timer.reset();
 222                         timer.start();
 223
 224                         //zpracovani obrazku
 225                         //meanPosition = recognition->findMean(image);
 226                         meanPosition = recognition->findSegment(image);
 227                         //fprintf(stdout,"meanPosition: xy = %i %i\n", meanPosition.x, meanPosition.y);
 228                         //vypis timeru doby zpracovani obrazu
 229                         //fprintf(stdout,"Timer %i\n",timer.getTime());
 230                         //vypocet rychlosti
 231                         //command = robot->computeCommand(meanPosition);
 232                         //odeslani obrazu robotovi
 233                         //if (move) robot->sendCommand(command);
 234
 235                         //vykresleni vysledku zpracovani
 236                         if (move==false || i%1 ==0) {
 237                                 fprintf(stdout,"pred send_data: xy = %i %i\n", meanPosition.x, meanPosition.y);
 238                                 send_data(meanPosition);
 239                                 image->plotLine(meanPosition.x,meanPosition.y);
 240                                 image->plotCenter();
 241                         #ifdef __i386__
 242                                 //Vykresleni obrazku
 243                                 gui->drawImage(image);
 244                                 //Vykresleni GUI
 245                                 gui->update();
 246                         #endif
 247                                 //Zpracovani udalosti z klavesnice
 248                                 processKeys();
 249                          }
 250
 251                         //fprintf(stderr,"Device exposition=%d\n",camera->getExposition());
 252                         //fprintf(stderr,"%d\n\nBR=%lf\n",i,image->getOverallBrightness(true));
 253                         i++;
 254                         if (i > 100) {
 255                                 controlExposition(image,camera);
 256                         }
 257                 }
 258         }
 259         delete recognition;
 260         //delete robot;
 261         delete image;
 262         delete gui;
 263         delete camera;
 264
 265         ret = robottype_roboorte_destroy(&orte);
 266
 267         return 0;
 268 }