Exemplo n.º 1
0
// Basic sonar test
int test_sonar(playerc_client_t *client, int index)
{
  int t, i;
  void *rdevice;
  playerc_sonar_t *device;

  printf("device [sonar] index [%d]\n", index);

  device = playerc_sonar_create(client, index);

  TEST("subscribing (read)");
  if (playerc_sonar_subscribe(device, PLAYER_OPEN_MODE) == 0)
    PASS();
  else
    FAIL();

  TEST("getting geometry");
  if (playerc_sonar_get_geom(device) == 0)
    PASS();
  else
    FAIL();

  printf("sonar geom: ");
  for (i = 0; i < device->pose_count; i++)
    printf("[%6.3f %6.3f %6.3f] ", device->poses[i].px, device->poses[i].py, device->poses[i].pyaw);
  printf("\n");

  for (t = 0; t < 10; t++)
  {
    TEST1("reading data (attempt %d)", t);

    do
      rdevice = playerc_client_read(client);
    while (rdevice == client);
    
    if (rdevice == device)
    {
      PASS();
      printf("sonar range: [%d] ", device->scan_count);
      for (i = 0; i < 3; i++)
        printf("[%6.3f] ", device->scan[i]);
      printf("\n");
    }
    else
    {
      FAIL();
      break;
    }
  }
  
  TEST("unsubscribing");
  if (playerc_sonar_unsubscribe(device) == 0)
    PASS();
  else
    FAIL();
  
  playerc_sonar_destroy(device);
  
  return 0;
}
Exemplo n.º 2
0
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;
}