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;
}
