int main(int argc, const char **argv) { int r, i, j; playerc_client_t *client; playerc_position2d_t *position2d; //sonar playerc_sonar_t *sonar; celda celdas[25]; int actual=0; int forward=1; int no_solucion=0; int flag_celda_final=0; // Create a client and connect it to the server. client = playerc_client_create(NULL, "localhost", 6665); if (playerc_client_connect(client) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } // Create and subscribe to a position2d device. position2d = playerc_position2d_create(client, 0); if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } // Fixing initial position playerc_position2d_set_odom(position2d, 0.0, 0.0, 0.0); // Create and subscribe to a sonar device sonar = playerc_sonar_create(client, 0); if (playerc_sonar_subscribe(sonar, PLAYER_OPEN_MODE) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } /* Obtener la geometría de los sensores de ultrasonidos sobre el pioneer 2 */ if (playerc_sonar_get_geom(sonar) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } // Enable motors playerc_position2d_enable(position2d, 1); playerc_client_read(client); inspeccionar_celda(sonar,&celdas[actual]); //Rutina para asegurarse de que ni nos dejamos un camino atrás, //ni estamos ya en la salida:"Ponemos el culo contra la pared" if(celda_final(&celdas[actual])) {//Si no veo ninguna pared girar_dch(client,position2d); // giro para comprobar la espalda inspeccionar_celda(sonar,&celdas[actual]); if(celdas[actual].pared[D_DCH]) { // había pared detrás girar_izq(client,position2d); //establecer orientación de referencia aquí theta = D_ARRIBA; inspeccionar_celda(sonar,&celdas[actual]); } else { printf("Ya estamos en la salida\n"); flag_celda_final=1; } } else { // me pongo de espaldas a la pared if(celdas[actual].pared[D_DCH]) { //si tengo pared a la dcha girar_izq(client,position2d); } else if(celdas[actual].pared[D_IZQ]) { //si tengo pared a la izda girar_dch(client,position2d); } else if(celdas[actual].pared[D_ARRIBA]) { //si la tengo en fente girar_180(client,position2d); } //establecer orientación de referencia aquí theta = D_ARRIBA; inspeccionar_celda(sonar,&celdas[actual]); } while(!(flag_celda_final || no_solucion)) { printf("celda: %d [%d,%d]\n",actual, x, y); r = mejor_ruta(celdas, actual, &forward); if (forward) { ir_direccion(client, position2d, r); actual++; inspeccionar_celda(sonar,&celdas[actual]); } else if (actual > 0) { ir_direccion(client, position2d, r); actual--; } else { no_solucion=1; printf("no hay solucion\n"); } if ((flag_celda_final = celda_final(&celdas[actual])) != 0) { printf("Fuera del laberinto!!\n\n"); printf("Camino:\n"); printf("-------\n"); for (i=0; i <= actual; i++) { for (j = 0; (j < 3) && (celdas[i].pared[j] != RUTA); j++) ; printf("(%d,%d) - %s\n", celdas[i].pos[0], celdas[i].pos[1], dirs[j]); } } } // Unsuscribe and Destroy // position2d playerc_position2d_unsubscribe(position2d); playerc_position2d_destroy(position2d); // sonar playerc_sonar_unsubscribe(sonar); playerc_sonar_destroy(sonar); // client playerc_client_disconnect(client); playerc_client_destroy(client); // End return 0; }
int main(int argc, const char **argv) { //Variaveis int degrees,PosRelX,PosRelY; float radians,Dlaser,ODM_ang, ang; int width = 500, height = 500; //Coloque o tamanho do mapa aqui (em pixel) int centroX = (width / 2); int centroY = (height / 2); playerc_client_t *client; playerc_laser_t *laser; playerc_position2d_t *position2d; CvPoint pt,pt1,pt2; CvScalar cinzaE,preto,cinzaC; char window_name[] = "Mapa"; IplImage* image = cvCreateImage( cvSize(width,height), 8, 3 ); cvNamedWindow(window_name, 1 ); preto = CV_RGB(0, 0, 0); //Para indicar obstaculos cinzaE = CV_RGB(92, 92, 92); //Para indicar o desconhecido cinzaC = CV_RGB(150, 150, 150); //Para indicar espacos livres printf ("debug: 11 - INICIO\n"); client = playerc_client_create(NULL, "localhost", 6665); printf ("debug: 12\n"); if (playerc_client_connect(client) != 0) return -1; printf ("debug: 13\n"); laser = playerc_laser_create(client, 0); printf ("debug: 21\n"); if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE)) return -1; printf ("debug: 22\n"); position2d = playerc_position2d_create(client, 0); if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE) != 0) { printf ("err1\n"); fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } printf ("debug: 23\n"); if (playerc_client_datamode (client, PLAYERC_DATAMODE_PULL) != 0) { printf ("err2\n"); fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } printf ("debug: 24\n"); if (playerc_client_set_replace_rule (client, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1) != 0) { printf ("err3\n"); fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } playerc_position2d_enable(position2d, 1); // Liga os motores printf ("debug: 25\n"); playerc_position2d_set_odom(position2d, 0, 0, 0); // Zera o odômetro cvSet(image, cinzaE,0); //Preencha a imagem com fundo cinza escuro pt.x = centroX; // Zera a coordenada X pt.y = centroY; // Zera a coordenada Y /* if( 0 != playerc_position2d_set_cmd_vel(position2d, 0, 0, DTOR(40.0), 1)) return -1; */ while(1) { printf ("debug: 26\n"); playerc_client_read(client); printf ("debug: 27\n"); //cvSaveImage("mapa1.jpg",image,0); printf ("debug: 28\n"); //playerc_client_read(client); printf ("debug: 29\n"); for (degrees = 2; degrees <= 360; degrees+=2) { printf ("debug: 30\n"); Dlaser = laser->scan[degrees][0]; printf ("debug: 31\n"); if (Dlaser < 8) { radians = graus2rad (degrees/2); //Converte o angulo do laser em graus para radianos printf ("debug: 32\n"); ODM_ang = position2d->pa; //Obtem o angulo relativo do robo ang = ((1.5*PI)+radians+ODM_ang); //Converte o angulo relativo em global printf ("debug: 33\n"); PosRelX = arredonda(position2d->px); //Posicao X relativa do robo PosRelY = arredonda(position2d->py); //Posicao Y relativa do robo printf ("debug: 34\n"); pt1.y = (centroY-PosRelY); //Coordenada y global do robo pt1.x = (centroX+PosRelX); //Coordenada x global do robo //converte coordenadas polares para retangulares (global) printf ("debug: 35\n"); pt.y = (int)(pt1.y-(sin(ang)*Dlaser*10)); pt.x = (int)(pt1.x+(cos(ang)*Dlaser*10)); printf ("debug: 36\n"); //Desenha a area livre cvLine(image, pt1,pt,cinzaC, 1,4,0); printf ("debug: 37\n"); //Marca o objeto no mapa cvLine(image, pt,pt,preto, 1,4,0); printf ("debug: 38\n"); //Mostra o resultado do mapeamento na tela //cvShowImage(window_name, image ); printf ("debug: 39\n"); //cvWaitKey(10); printf ("debug: 40\n"); } } } //Desconecta o player printf ("debug: 41\n"); playerc_laser_unsubscribe(laser); printf ("debug: 42\n"); playerc_laser_destroy(laser); printf ("debug: 43\n"); playerc_client_disconnect(client); printf ("debug: 44\n"); playerc_client_destroy(client); printf ("debug: 45\n"); //Destroi a janela OpenCV cvReleaseImage(&image); printf ("debug: 46\n"); cvDestroyWindow(window_name); printf ("debug: 47\n"); return 0; }
int main(int argc, const char **argv) { //Variables int degrees,PosRelX,PosRelY; float radians,Dlaser,ODM_ang, ang; int width = 500, height = 500; //Create the size of the map here (in pixel) int centroX = (width / 2); int centroY = (height / 2); playerc_client_t *client; playerc_laser_t *laser; playerc_position2d_t *position2d; CvPoint pt,pt1,pt2; CvScalar cinzaE,preto,cinzaC; char window_name[] = "Map"; IplImage* image = cvCreateImage( cvSize(width,height), 8, 3 ); cvNamedWindow(window_name, 1 ); preto = CV_RGB(0, 0, 0); //for indicating obstacles cinzaE = CV_RGB(92, 92, 92); //To indicate the stranger cinzaC = CV_RGB(150, 150, 150); //To indicate free spaces client = playerc_client_create(NULL, "localhost", 6665); if (playerc_client_connect(client) != 0) return -1; laser = playerc_laser_create(client, 0); if (playerc_laser_subscribe(laser, PLAYERC_OPEN_MODE)) return -1; position2d = playerc_position2d_create(client, 0); if (playerc_position2d_subscribe(position2d, PLAYERC_OPEN_MODE) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } if (playerc_client_datamode (client, PLAYERC_DATAMODE_PULL) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } if (playerc_client_set_replace_rule (client, -1, -1, PLAYER_MSGTYPE_DATA, -1, 1) != 0) { fprintf(stderr, "error: %s\n", playerc_error_str()); return -1; } playerc_position2d_enable(position2d, 1); // initialise motors playerc_position2d_set_odom(position2d, 0, 0, 0); // Set odometer to zero cvSet(image, cinzaE,0); //set the image colour to dark pt.x = centroX; // Zero coordinate for x pt.y = centroY; // Zero coordinate for y while(1) { playerc_client_read(client); cvSaveImage("mapa.jpg",image); playerc_client_read(client); for (degrees = 2; degrees <= 360; degrees+=2) { Dlaser = laser->scan[degrees][0]; if (Dlaser < 8) { radians = graus2rad (degrees/2); //Convert the angle of the laser to radians ODM_ang = position2d->pa; //Obtain the angle relative to the robot ang = ((1.5*PI)+radians+ODM_ang); //Converte the angle relative to the world PosRelX = arredonda(position2d->px); //Position x relative to robot PosRelY = arredonda(position2d->py); //Position y relative to robot pt1.y = (centroY-PosRelY); //Co-ordinated global y of the robot pt1.x = (centroX+PosRelX); //Co-ordinated global x of the robot //t converts polar coordinates for rectangular (global) pt.y = (int)(pt1.y-(sin(ang)*Dlaser*10)); pt.x = (int)(pt1.x+(cos(ang)*Dlaser*10)); //The free area draws cvline cvLine(image, pt1,pt,cinzaC, 1,4,0); //marks the object in the map cvLine(image, pt,pt,preto, 1,4,0); //Shows the result of the map to the screen cvShowImage(window_name, image ); cvWaitKey(10); } } } //Disconnect player playerc_laser_unsubscribe(laser); playerc_laser_destroy(laser); playerc_client_disconnect(client); playerc_client_destroy(client); //Destroy the OpenCV window cvReleaseImage cvReleaseImage(&image); cvDestroyWindow(window_name); return 0; }